From ccbf9ef1be68d9152166e54c7c34d9ddeb01b387 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Wed, 19 Oct 2016 15:02:34 +0200 Subject: [PATCH] Adapt to changes in cartographer and write PLY files on exporting. (#128) Also use reflectivity in the 'PointsBatch'es created in 'WriteAssets'. In the online case, we throw away reflectivities during SLAMing to save on memory, so there will be none. --- .../cartographer_ros/CMakeLists.txt | 18 ++++++------- .../{xray.cc => assets_writer.cc} | 27 ++++++++++++------- .../{xray.h => assets_writer.h} | 16 +++++------ .../cartographer_ros/node_main.cc | 12 ++++----- 4 files changed, 41 insertions(+), 32 deletions(-) rename cartographer_ros/cartographer_ros/{xray.cc => assets_writer.cc} (66%) rename cartographer_ros/cartographer_ros/{xray.h => assets_writer.h} (64%) diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index 7ba290b..a84ef2d 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -1,3 +1,11 @@ +google_library(assets_writer + USES_CARTOGRAPHER + SRCS + assets_writer.cc + HDRS + assets_writer.h +) + google_library(map_writer USES_GLOG USES_ROS @@ -87,14 +95,6 @@ google_library(time_conversion time_conversion.h ) -google_library(xray - USES_CARTOGRAPHER - SRCS - xray.cc - HDRS - xray.h -) - google_catkin_test(time_conversion_test USES_CARTOGRAPHER USES_ROS @@ -113,6 +113,7 @@ google_binary(cartographer_node SRCS node_main.cc DEPENDS + assets_writer map_writer msg_conversion node_options @@ -121,7 +122,6 @@ google_binary(cartographer_node sensor_bridge tf_bridge time_conversion - xray ) install(TARGETS cartographer_node diff --git a/cartographer_ros/cartographer_ros/xray.cc b/cartographer_ros/cartographer_ros/assets_writer.cc similarity index 66% rename from cartographer_ros/cartographer_ros/xray.cc rename to cartographer_ros/cartographer_ros/assets_writer.cc index 1d3dc0b..d0807f9 100644 --- a/cartographer_ros/cartographer_ros/xray.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -14,9 +14,11 @@ * limitations under the License. */ -#include "cartographer_ros/xray.h" +#include "cartographer_ros/assets_writer.h" +#include "cartographer/common/make_unique.h" #include "cartographer/io/null_points_processor.h" +#include "cartographer/io/ply_writing_points_processor.h" #include "cartographer/io/points_processor.h" #include "cartographer/io/xray_points_processor.h" @@ -24,9 +26,9 @@ namespace cartographer_ros { namespace carto = ::cartographer; -void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, - const double voxel_size, const std::string& stem) { +void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + const double voxel_size, const std::string& stem) { carto::io::NullPointsProcessor null_points_processor; carto::io::XRayPointsProcessor xy_xray_points_processor( voxel_size, carto::transform::Rigid3f::Rotation( @@ -40,18 +42,25 @@ void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>& voxel_size, carto::transform::Rigid3f::Rotation( Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), stem + "_xray_xz.png", &yz_xray_points_processor); + carto::io::PlyWritingPointsProcessor ply_writing_points_processor( + stem + ".ply", &xz_xray_points_processor); for (const auto& node : trajectory_nodes) { const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan( carto::sensor::Decompress(node.constant_data->laser_fan_3d), node.pose.cast()); - carto::io::PointsBatch points_batch; - points_batch.origin = laser_fan.origin; - points_batch.points = laser_fan.returns; - xz_xray_points_processor.Process(points_batch); + auto points_batch = carto::common::make_unique(); + points_batch->origin = laser_fan.origin; + points_batch->points = laser_fan.returns; + for (const uint8 reflectivity : + node.constant_data->laser_fan_3d.reflectivities) { + points_batch->colors.push_back( + carto::io::Color{{reflectivity, reflectivity, reflectivity}}); + } + ply_writing_points_processor.Process(std::move(points_batch)); } - xz_xray_points_processor.Flush(); + ply_writing_points_processor.Flush(); } } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/xray.h b/cartographer_ros/cartographer_ros/assets_writer.h similarity index 64% rename from cartographer_ros/cartographer_ros/xray.h rename to cartographer_ros/cartographer_ros/assets_writer.h index 8c8a1f2..92bc0b1 100644 --- a/cartographer_ros/cartographer_ros/xray.h +++ b/cartographer_ros/cartographer_ros/assets_writer.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_ROS_XRAY_H_ -#define CARTOGRAPHER_ROS_XRAY_H_ +#ifndef CARTOGRAPHER_ROS_ASSETS_WRITER_H_ +#define CARTOGRAPHER_ROS_ASSETS_WRITER_H_ #include #include @@ -24,12 +24,12 @@ namespace cartographer_ros { -// Writes X-ray images from the 'trajectory_nodes'. The filenames will all start -// with 'stem'. -void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, - double voxel_size, const std::string& stem); +// Writes X-ray images and PLY files from the 'trajectory_nodes'. The filenames +// will all start with 'stem'. +void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + double voxel_size, const std::string& stem); } // namespace cartographer_ros -#endif // CARTOGRAPHER_ROS_XRAY_H_ +#endif // CARTOGRAPHER_ROS_ASSETS_WRITER_H_ diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 1dacf41..06b040b 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -47,6 +47,7 @@ #include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" +#include "cartographer_ros/assets_writer.h" #include "cartographer_ros/map_writer.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/node_options.h" @@ -55,7 +56,6 @@ #include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/tf_bridge.h" #include "cartographer_ros/time_conversion.h" -#include "cartographer_ros/xray.h" #include "cartographer_ros_msgs/FinishTrajectory.h" #include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapList.h" @@ -378,11 +378,11 @@ bool Node::HandleFinishTrajectory( } if (options_.map_builder_options.use_trajectory_builder_3d()) { - WriteXRayImages(trajectory_nodes, - options_.map_builder_options.trajectory_builder_3d_options() - .submaps_options() - .high_resolution(), - request.stem); + WriteAssets(trajectory_nodes, + options_.map_builder_options.trajectory_builder_3d_options() + .submaps_options() + .high_resolution(), + request.stem); } return true; }