diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index d3ec13c..f9c9756 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -51,15 +51,15 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& carto::io::XRayPointsProcessor xy_xray_points_processor( voxel_size, carto::transform::Rigid3f::Rotation( Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), - stem + "_xray_xy.png", &null_points_processor); + {}, stem + "_xray_xy.png", &null_points_processor); carto::io::XRayPointsProcessor yz_xray_points_processor( voxel_size, carto::transform::Rigid3f::Rotation( Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), - stem + "_xray_yz.png", &xy_xray_points_processor); + {}, stem + "_xray_yz.png", &xy_xray_points_processor); carto::io::XRayPointsProcessor xz_xray_points_processor( voxel_size, carto::transform::Rigid3f::Rotation( Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), - stem + "_xray_xz.png", &yz_xray_points_processor); + {}, stem + "_xray_xz.png", &yz_xray_points_processor); carto::io::PlyWritingPointsProcessor ply_writing_points_processor( stem + ".ply", &xz_xray_points_processor); diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 8edc3cc..360e0fd 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -56,15 +56,6 @@ namespace { namespace carto = ::cartographer; -std::unique_ptr ReadTrajectory( - const std::string& trajectory_filename) { - std::ifstream stream(trajectory_filename.c_str()); - carto::mapping::proto::Trajectory trajectory_proto; - CHECK(trajectory_proto.ParseFromIstream(&stream)); - return carto::transform::TransformInterpolationBuffer::FromTrajectory( - trajectory_proto); -} - void ReadStaticTransformsFromUrdf(const string& urdf_filename, ::tf2_ros::Buffer* const buffer) { urdf::Model model; @@ -99,9 +90,16 @@ void Run(const string& trajectory_filename, const string& bag_filename, ::tf2_ros::Buffer buffer; ReadStaticTransformsFromUrdf(urdf_filename, &buffer); - auto transform_interpolation_buffer = ReadTrajectory(trajectory_filename); + std::ifstream stream(trajectory_filename.c_str()); + carto::mapping::proto::Trajectory trajectory_proto; + CHECK(trajectory_proto.ParseFromIstream(&stream)); - auto* builder = carto::io::PointsProcessorPipelineBuilder::instance(); + auto transform_interpolation_buffer = + carto::transform::TransformInterpolationBuffer::FromTrajectory( + trajectory_proto); + + carto::io::PointsProcessorPipelineBuilder builder; + carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, &builder); auto file_resolver = carto::common::make_unique( @@ -114,7 +112,7 @@ void Run(const string& trajectory_filename, const string& bag_filename, lua_parameter_dictionary.GetString("tracking_frame"); std::vector> pipeline = - builder->CreatePipeline( + builder.CreatePipeline( lua_parameter_dictionary.GetDictionary("pipeline").get()); do { diff --git a/cartographer_ros/configuration_files/assets_writer.lua b/cartographer_ros/configuration_files/assets_writer.lua index 72197f1..c992f5f 100644 --- a/cartographer_ros/configuration_files/assets_writer.lua +++ b/cartographer_ros/configuration_files/assets_writer.lua @@ -1,4 +1,6 @@ VOXEL_SIZE = 5e-2 + + options = { tracking_frame = "base_link", pipeline = { @@ -27,7 +29,7 @@ options = { { action = "write_xray_image", voxel_size = VOXEL_SIZE, - filename = "xray_yz.png", + filename = "xray_yz_all", transform = { translation = { 0., 0., 0. }, rotation = { 0. , 0., math.pi, }, @@ -36,7 +38,7 @@ options = { { action = "write_xray_image", voxel_size = VOXEL_SIZE, - filename = "xray_xy.png", + filename = "xray_xy_all", transform = { translation = { 0., 0., 0. }, rotation = { 0., -math.pi / 2., 0., }, @@ -45,7 +47,37 @@ options = { { action = "write_xray_image", voxel_size = VOXEL_SIZE, - filename = "xray_xz.png", + filename = "xray_xz_all", + transform = { + translation = { 0., 0., 0. }, + rotation = { 0. , 0., -math.pi / 2, }, + }, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + separate_floors = true, + filename = "xray_yz_level_", + transform = { + translation = { 0., 0., 0. }, + rotation = { 0. , 0., math.pi, }, + }, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + separate_floors = true, + filename = "xray_xy_level_", + transform = { + translation = { 0., 0., 0. }, + rotation = { 0., -math.pi / 2., 0., }, + }, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + separate_floors = true, + filename = "xray_xz_level_", transform = { translation = { 0., 0., 0. }, rotation = { 0. , 0., -math.pi / 2, },