From 04df159d82739c8ab339b189437fdae2c1ec7a06 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Fri, 23 Jun 2017 18:16:32 +0200 Subject: [PATCH] Make WriteAssets multi trajectory aware. (#394) Related to #343. --- .../cartographer_ros/assets_writer.cc | 53 ++++++++----- .../cartographer_ros/assets_writer.h | 19 +++-- .../cartographer_ros/map_builder_bridge.cc | 64 +++++++-------- .../cartographer_ros/occupancy_grid.cc | 77 +++++++++---------- .../cartographer_ros/occupancy_grid.h | 8 +- .../cartographer_ros/occupancy_grid_test.cc | 2 +- 6 files changed, 122 insertions(+), 101 deletions(-) diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index 05cc123..49527a7 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -31,24 +31,35 @@ namespace cartographer_ros { -// Writes an occupancy grid. +bool HasNonTrimmedNode( + const std::vector>& + all_trajectory_nodes) { + for (const auto& trajectory_nodes : all_trajectory_nodes) { + for (const auto& node : trajectory_nodes) { + if (!node.trimmed()) { + return true; + } + } + } + return false; +} + void Write2DAssets( - const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, + const std::vector>& + all_trajectory_nodes, const string& map_frame, const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options, const std::string& stem) { ::nav_msgs::OccupancyGrid occupancy_grid; - BuildOccupancyGrid2D(trajectory_nodes, map_frame, submaps_options, + BuildOccupancyGrid2D(all_trajectory_nodes, map_frame, submaps_options, &occupancy_grid); WriteOccupancyGridToPgmAndYaml(occupancy_grid, stem); } -// Writes X-ray images and PLY files from the 'trajectory_nodes'. The filenames -// will all start with 'stem'. -void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, - const double voxel_size, const std::string& stem) { +void Write3DAssets( + const std::vector>& + all_trajectory_nodes, + const double voxel_size, const std::string& stem) { namespace carto = ::cartographer; const auto file_writer_factory = [](const string& filename) { return carto::common::make_unique(filename); @@ -73,16 +84,20 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& carto::io::PlyWritingPointsProcessor ply_writing_points_processor( file_writer_factory(stem + ".ply"), &xz_xray_points_processor); - for (const auto& node : trajectory_nodes) { - const carto::sensor::RangeData range_data = - carto::sensor::TransformRangeData( - carto::sensor::Decompress(node.constant_data->range_data_3d), - node.pose.cast()); - - auto points_batch = carto::common::make_unique(); - points_batch->origin = range_data.origin; - points_batch->points = range_data.returns; - ply_writing_points_processor.Process(std::move(points_batch)); + for (size_t trajectory_id = 0; trajectory_id < all_trajectory_nodes.size(); + ++trajectory_id) { + for (const auto& node : all_trajectory_nodes[trajectory_id]) { + const carto::sensor::RangeData range_data = + carto::sensor::TransformRangeData( + carto::sensor::Decompress(node.constant_data->range_data_3d), + node.pose.cast()); + auto points_batch = carto::common::make_unique(); + points_batch->time = node.time(); + points_batch->origin = range_data.origin; + points_batch->trajectory_index = trajectory_id; + points_batch->points = range_data.returns; + ply_writing_points_processor.Process(std::move(points_batch)); + } } ply_writing_points_processor.Flush(); } diff --git a/cartographer_ros/cartographer_ros/assets_writer.h b/cartographer_ros/cartographer_ros/assets_writer.h index 5eb8218..51625dc 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.h +++ b/cartographer_ros/cartographer_ros/assets_writer.h @@ -25,19 +25,26 @@ namespace cartographer_ros { +// Returns 'true' if there is at least one untrimmed node for any trajectory. +// The Write?DAssets functions expects this to be 'true'. +bool HasNonTrimmedNode( + const std::vector>& + all_trajectory_nodes); + // Writes a trajectory proto and an occupancy grid. void Write2DAssets( - const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, + const std::vector>& + all_trajectory_nodes, const string& map_frame, const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options, const std::string& stem); // Writes X-ray images, trajectory proto, and PLY files from the -// 'trajectory_nodes'. The filenames will all start with 'stem'. -void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, - const double voxel_size, const std::string& stem); +// 'all_trajectory_nodes'. The filenames will all start with 'stem'. +void Write3DAssets( + const std::vector>& + all_trajectory_nodes, + const double voxel_size, const std::string& stem); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 14e2c04..1688eee 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -38,6 +38,7 @@ int MapBuilderBridge::AddTrajectory( expected_sensor_ids, trajectory_options.trajectory_builder_options); LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'."; + // Make sure there is no trajectory with 'trajectory_id' yet. CHECK_EQ(sensor_bridges_.count(trajectory_id), 0); sensor_bridges_[trajectory_id] = cartographer::common::make_unique( @@ -53,6 +54,7 @@ int MapBuilderBridge::AddTrajectory( void MapBuilderBridge::FinishTrajectory(const int trajectory_id) { LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'..."; + // Make sure there is a trajectory with 'trajectory_id'. CHECK_EQ(sensor_bridges_.count(trajectory_id), 1); map_builder_.FinishTrajectory(trajectory_id); map_builder_.sparse_pose_graph()->RunFinalOptimization(); @@ -72,31 +74,32 @@ void MapBuilderBridge::SerializeState(const std::string& stem) { void MapBuilderBridge::WriteAssets(const string& stem) { const auto all_trajectory_nodes = map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); - // TODO(yutakaoka): Add multi-trajectory support. - CHECK_EQ(trajectory_options_.count(0), 1); - CHECK_EQ(all_trajectory_nodes.size(), 1); - if (all_trajectory_nodes[0].empty()) { + if (!HasNonTrimmedNode(all_trajectory_nodes)) { LOG(WARNING) << "No data was collected and no assets will be written."; - } else { - LOG(INFO) << "Writing assets with stem '" << stem << "'..."; - if (node_options_.map_builder_options.use_trajectory_builder_2d()) { - Write2DAssets( - all_trajectory_nodes[0], node_options_.map_frame, - trajectory_options_[0] - .trajectory_builder_options.trajectory_builder_2d_options() - .submaps_options(), - stem); - } + return; + } + // Make sure there is a trajectory with id = 0. + CHECK_EQ(trajectory_options_.count(0), 1); + LOG(INFO) << "Writing assets with stem '" << stem << "'..."; + if (node_options_.map_builder_options.use_trajectory_builder_2d()) { + // We arbitrarily use the submap_options() from the first trajectory to + // write the 2D assets. + Write2DAssets( + all_trajectory_nodes, node_options_.map_frame, + trajectory_options_[0] + .trajectory_builder_options.trajectory_builder_2d_options() + .submaps_options(), + stem); + } - if (node_options_.map_builder_options.use_trajectory_builder_3d()) { - Write3DAssets( - all_trajectory_nodes[0], - trajectory_options_[0] - .trajectory_builder_options.trajectory_builder_3d_options() - .submaps_options() - .high_resolution(), - stem); - } + if (node_options_.map_builder_options.use_trajectory_builder_3d()) { + Write3DAssets( + all_trajectory_nodes, + trajectory_options_[0] + .trajectory_builder_options.trajectory_builder_3d_options() + .submaps_options() + .high_resolution(), + stem); } } @@ -153,20 +156,16 @@ std::unique_ptr MapBuilderBridge::BuildOccupancyGrid() { CHECK(node_options_.map_builder_options.use_trajectory_builder_2d()) << "Publishing OccupancyGrids for 3D data is not yet supported"; - std::vector<::cartographer::mapping::TrajectoryNode> flat_trajectory_nodes; - for (const auto& single_trajectory_nodes : - map_builder_.sparse_pose_graph()->GetTrajectoryNodes()) { - flat_trajectory_nodes.insert(flat_trajectory_nodes.end(), - single_trajectory_nodes.begin(), - single_trajectory_nodes.end()); - } + const auto all_trajectory_nodes = + map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); std::unique_ptr occupancy_grid; - if (!flat_trajectory_nodes.empty()) { + if (HasNonTrimmedNode(all_trajectory_nodes)) { occupancy_grid = cartographer::common::make_unique(); + // Make sure there is a trajectory with id = 0. CHECK_EQ(trajectory_options_.count(0), 1); BuildOccupancyGrid2D( - flat_trajectory_nodes, node_options_.map_frame, + all_trajectory_nodes, node_options_.map_frame, trajectory_options_[0] .trajectory_builder_options.trajectory_builder_2d_options() .submaps_options(), @@ -190,6 +189,7 @@ MapBuilderBridge::GetTrajectoryStates() { continue; } + // Make sure there is a trajectory with 'trajectory_id'. CHECK_EQ(trajectory_options_.count(trajectory_id), 1); trajectory_states[trajectory_id] = { pose_estimate, diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.cc b/cartographer_ros/cartographer_ros/occupancy_grid.cc index 1032479..8d6fc53 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid.cc @@ -24,30 +24,26 @@ namespace { -Eigen::AlignedBox2f ComputeMapBoundingBox( - const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes) { +Eigen::AlignedBox2f ComputeMapBoundingBox2D( + const std::vector>& + all_trajectory_nodes) { Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero()); - for (const auto& node : trajectory_nodes) { - if (node.trimmed()) { - continue; - } - const auto& data = *node.constant_data; - ::cartographer::sensor::RangeData range_data; - if (!data.range_data_3d.returns.empty()) { - range_data = ::cartographer::sensor::TransformRangeData( - ::cartographer::sensor::Decompress(data.range_data_3d), - node.pose.cast()); - } else { + for (const auto& trajectory_nodes : all_trajectory_nodes) { + for (const auto& node : trajectory_nodes) { + if (node.trimmed()) { + continue; + } + const auto& data = *node.constant_data; + ::cartographer::sensor::RangeData range_data; range_data = ::cartographer::sensor::TransformRangeData( data.range_data_2d, node.pose.cast()); - } - bounding_box.extend(range_data.origin.head<2>()); - for (const Eigen::Vector3f& hit : range_data.returns) { - bounding_box.extend(hit.head<2>()); - } - for (const Eigen::Vector3f& miss : range_data.misses) { - bounding_box.extend(miss.head<2>()); + bounding_box.extend(range_data.origin.head<2>()); + for (const Eigen::Vector3f& hit : range_data.returns) { + bounding_box.extend(hit.head<2>()); + } + for (const Eigen::Vector3f& miss : range_data.misses) { + bounding_box.extend(miss.head<2>()); + } } } return bounding_box; @@ -58,31 +54,33 @@ Eigen::AlignedBox2f ComputeMapBoundingBox( namespace cartographer_ros { void BuildOccupancyGrid2D( - const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, + const std::vector>& + all_trajectory_nodes, const string& map_frame, const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options, ::nav_msgs::OccupancyGrid* const occupancy_grid) { - CHECK(!trajectory_nodes.empty()); namespace carto = ::cartographer; const carto::mapping_2d::MapLimits map_limits = - ComputeMapLimits(submaps_options.resolution(), trajectory_nodes); + ComputeMapLimits(submaps_options.resolution(), all_trajectory_nodes); carto::mapping_2d::ProbabilityGrid probability_grid(map_limits); carto::mapping_2d::RangeDataInserter range_data_inserter( submaps_options.range_data_inserter_options()); - for (const auto& node : trajectory_nodes) { - if (node.trimmed()) { - continue; + carto::common::Time latest_time = carto::common::Time::min(); + for (const auto& trajectory_nodes : all_trajectory_nodes) { + for (const auto& node : trajectory_nodes) { + if (node.trimmed()) { + continue; + } + latest_time = std::max(latest_time, node.time()); + CHECK(node.constant_data->range_data_3d.returns.empty()); + range_data_inserter.Insert( + carto::sensor::TransformRangeData(node.constant_data->range_data_2d, + node.pose.cast()), + &probability_grid); } - CHECK(node.constant_data->range_data_3d.returns.empty()); // No 3D yet. - range_data_inserter.Insert( - carto::sensor::TransformRangeData(node.constant_data->range_data_2d, - node.pose.cast()), - &probability_grid); } - - // TODO(whess): Compute the latest time of in 'trajectory_nodes'. - occupancy_grid->header.stamp = ToRos(trajectory_nodes.back().time()); + CHECK(latest_time != carto::common::Time::min()); + occupancy_grid->header.stamp = ToRos(latest_time); occupancy_grid->header.frame_id = map_frame; occupancy_grid->info.map_load_time = occupancy_grid->header.stamp; @@ -128,9 +126,10 @@ void BuildOccupancyGrid2D( ::cartographer::mapping_2d::MapLimits ComputeMapLimits( const double resolution, - const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes) { - Eigen::AlignedBox2f bounding_box = ComputeMapBoundingBox(trajectory_nodes); + const std::vector>& + all_trajectory_nodes) { + Eigen::AlignedBox2f bounding_box = + ComputeMapBoundingBox2D(all_trajectory_nodes); // Add some padding to ensure all rays are still contained in the map after // discretization. const float kPadding = 3.f * resolution; diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.h b/cartographer_ros/cartographer_ros/occupancy_grid.h index 0b27be2..3a2cd4e 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid.h +++ b/cartographer_ros/cartographer_ros/occupancy_grid.h @@ -29,8 +29,8 @@ namespace cartographer_ros { void BuildOccupancyGrid2D( - const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, + const std::vector>& + all_trajectory_nodes, const string& map_frame, const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options, ::nav_msgs::OccupancyGrid* const occupancy_grid); @@ -39,8 +39,8 @@ void BuildOccupancyGrid2D( // misses) in the 'trajectory_nodes'. ::cartographer::mapping_2d::MapLimits ComputeMapLimits( double resolution, - const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes); + const std::vector>& + all_trajectory_nodes); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_test.cc b/cartographer_ros/cartographer_ros/occupancy_grid_test.cc index f65e42c..665ae77 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_test.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid_test.cc @@ -41,7 +41,7 @@ TEST(OccupancyGridTest, ComputeMapLimits) { ::cartographer::transform::Rigid3d::Identity()}; constexpr double kResolution = 0.05; const ::cartographer::mapping_2d::MapLimits limits = - ComputeMapLimits(kResolution, {trajectory_node}); + ComputeMapLimits(kResolution, {{trajectory_node}}); constexpr float kPaddingAwareTolerance = 5 * kResolution; EXPECT_NEAR(50.f, limits.max().x(), kPaddingAwareTolerance); EXPECT_NEAR(1.f, limits.max().y(), kPaddingAwareTolerance);