Make WriteAssets multi trajectory aware. (#394)

Related to #343.

cartographer_ros/cartographer_ros/assets_writer.cc

@@ -31,23 +31,34 @@
 
 namespace cartographer_ros {
 
-// Writes an occupancy grid.
+bool HasNonTrimmedNode(
+    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
+        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>&
+    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        trajectory_nodes,
+        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
+void Write3DAssets(
-// will all start with 'stem'.
+    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
+        all_trajectory_nodes,
-                       trajectory_nodes,
     const double voxel_size, const std::string& stem) {
   namespace carto = ::cartographer;
   const auto file_writer_factory = [](const string& filename) {
@@ -73,17 +84,21 @@ 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) {
+  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<float>());
-
       auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
+      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();
 }
 

cartographer_ros/cartographer_ros/assets_writer.h

@@ -25,18 +25,25 @@
 
 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<std::vector<::cartographer::mapping::TrajectoryNode>>&
+        all_trajectory_nodes);
+
 // Writes a trajectory proto and an occupancy grid.
 void Write2DAssets(
-    const std::vector<::cartographer::mapping::TrajectoryNode>&
+    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        trajectory_nodes,
+        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'.
+// 'all_trajectory_nodes'. The filenames will all start with 'stem'.
-void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
+void Write3DAssets(
-                       trajectory_nodes,
+    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
+        all_trajectory_nodes,
     const double voxel_size, const std::string& stem);
 
 }  // namespace cartographer_ros

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<SensorBridge>(
@@ -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,16 +74,18 @@ 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.
+  if (!HasNonTrimmedNode(all_trajectory_nodes)) {
-  CHECK_EQ(trajectory_options_.count(0), 1);
-  CHECK_EQ(all_trajectory_nodes.size(), 1);
-  if (all_trajectory_nodes[0].empty()) {
     LOG(WARNING) << "No data was collected and no assets will be written.";
-  } else {
+    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[0], node_options_.map_frame,
+        all_trajectory_nodes, node_options_.map_frame,
         trajectory_options_[0]
             .trajectory_builder_options.trajectory_builder_2d_options()
             .submaps_options(),
@@ -90,14 +94,13 @@ void MapBuilderBridge::WriteAssets(const string& stem) {
 
   if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
     Write3DAssets(
-          all_trajectory_nodes[0],
+        all_trajectory_nodes,
         trajectory_options_[0]
             .trajectory_builder_options.trajectory_builder_3d_options()
             .submaps_options()
             .high_resolution(),
         stem);
   }
-  }
 }
 
 bool MapBuilderBridge::HandleSubmapQuery(
@@ -153,20 +156,16 @@ std::unique_ptr<nav_msgs::OccupancyGrid>
 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;
+  const auto all_trajectory_nodes =
-  for (const auto& single_trajectory_nodes :
+      map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
-       map_builder_.sparse_pose_graph()->GetTrajectoryNodes()) {
-    flat_trajectory_nodes.insert(flat_trajectory_nodes.end(),
-                                 single_trajectory_nodes.begin(),
-                                 single_trajectory_nodes.end());
-  }
   std::unique_ptr<nav_msgs::OccupancyGrid> occupancy_grid;
-  if (!flat_trajectory_nodes.empty()) {
+  if (HasNonTrimmedNode(all_trajectory_nodes)) {
     occupancy_grid =
         cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
+    // 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,

cartographer_ros/cartographer_ros/occupancy_grid.cc

@@ -24,24 +24,19 @@
 
 namespace {
 
-Eigen::AlignedBox2f ComputeMapBoundingBox(
+Eigen::AlignedBox2f ComputeMapBoundingBox2D(
-    const std::vector<::cartographer::mapping::TrajectoryNode>&
+    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        trajectory_nodes) {
+        all_trajectory_nodes) {
   Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero());
+  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;
-    if (!data.range_data_3d.returns.empty()) {
-      range_data = ::cartographer::sensor::TransformRangeData(
-          ::cartographer::sensor::Decompress(data.range_data_3d),
-          node.pose.cast<float>());
-    } else {
       range_data = ::cartographer::sensor::TransformRangeData(
           data.range_data_2d, node.pose.cast<float>());
-    }
       bounding_box.extend(range_data.origin.head<2>());
       for (const Eigen::Vector3f& hit : range_data.returns) {
         bounding_box.extend(hit.head<2>());
@@ -50,6 +45,7 @@ Eigen::AlignedBox2f ComputeMapBoundingBox(
         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>&
+    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        trajectory_nodes,
+        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());
+  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;
       }
-    CHECK(node.constant_data->range_data_3d.returns.empty());  // No 3D yet.
+      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<float>()),
           &probability_grid);
     }
-
+  }
-  // TODO(whess): Compute the latest time of in 'trajectory_nodes'.
+  CHECK(latest_time != carto::common::Time::min());
-  occupancy_grid->header.stamp = ToRos(trajectory_nodes.back().time());
+  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>&
+    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        trajectory_nodes) {
+        all_trajectory_nodes) {
-  Eigen::AlignedBox2f bounding_box = ComputeMapBoundingBox(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;

cartographer_ros/cartographer_ros/occupancy_grid.h

@@ -29,8 +29,8 @@
 namespace cartographer_ros {
 
 void BuildOccupancyGrid2D(
-    const std::vector<::cartographer::mapping::TrajectoryNode>&
+    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        trajectory_nodes,
+        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>&
+    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        trajectory_nodes);
+        all_trajectory_nodes);
 
 }  // namespace cartographer_ros
 

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);