Serialize the pose graph instead of just the trajectory. (#365)

This is related to googlecartographer/cartographer#310
and googlecartographer/cartographer#253.

cartographer_ros/cartographer_ros/assets_writer.cc

@@ -31,34 +31,6 @@
 
 namespace cartographer_ros {
 
-namespace {
-
-namespace carto = ::cartographer;
-
-void WriteTrajectory(const std::vector<::cartographer::mapping::TrajectoryNode>&
-                         trajectory_nodes,
-                     const std::string& stem) {
-  carto::mapping::proto::Trajectory trajectory;
-  // TODO(whess): Add multi-trajectory support.
-  for (const auto& node : trajectory_nodes) {
-    const auto& data = *node.constant_data;
-    auto* node_proto = trajectory.add_node();
-    node_proto->set_timestamp(carto::common::ToUniversal(data.time));
-    *node_proto->mutable_pose() =
-        carto::transform::ToProto(node.pose * data.tracking_to_pose);
-  }
-
-  // Write the trajectory.
-  std::ofstream proto_file(stem + ".pb",
-                           std::ios_base::out | std::ios_base::binary);
-  CHECK(trajectory.SerializeToOstream(&proto_file))
-      << "Could not serialize trajectory.";
-  proto_file.close();
-  CHECK(proto_file) << "Could not write trajectory.";
-}
-
-}  // namespace
-
 // Writes an occupancy grid.
 void Write2DAssets(
     const std::vector<::cartographer::mapping::TrajectoryNode>&
@@ -66,8 +38,6 @@ void Write2DAssets(
     const string& map_frame,
     const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
     const std::string& stem) {
-  WriteTrajectory(trajectory_nodes, stem);
-
   ::nav_msgs::OccupancyGrid occupancy_grid;
   BuildOccupancyGrid2D(trajectory_nodes, map_frame, submaps_options,
                        &occupancy_grid);
@@ -79,8 +49,7 @@ void Write2DAssets(
 void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
                        trajectory_nodes,
                    const double voxel_size, const std::string& stem) {
-  WriteTrajectory(trajectory_nodes, stem);
-
+  namespace carto = ::cartographer;
   const auto file_writer_factory = [](const string& filename) {
     return carto::common::make_unique<carto::io::StreamFileWriter>(filename);
   };

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -26,6 +26,7 @@
 #include "cartographer/io/file_writer.h"
 #include "cartographer/io/points_processor.h"
 #include "cartographer/io/points_processor_pipeline_builder.h"
+#include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/sensor/range_data.h"
 #include "cartographer/transform/transform_interpolation_buffer.h"
@@ -55,8 +56,8 @@ DEFINE_string(
     "URDF file that contains static links for your sensor configuration.");
 DEFINE_string(bag_filename, "", "Bag to process.");
 DEFINE_string(
-    trajectory_filename, "",
-    "Proto containing the trajectory written by /finish_trajectory service.");
+    pose_graph_filename, "",
+    "Proto containing the pose graph written by /write_assets service.");
 DEFINE_bool(use_bag_transforms, true,
             "Whether to read and use the transforms from the bag.");
 
@@ -134,7 +135,7 @@ void HandleMessage(
   pipeline.back()->Process(std::move(batch));
 }
 
-void Run(const string& trajectory_filename, const string& bag_filename,
+void Run(const string& pose_graph_filename, const string& bag_filename,
          const string& configuration_directory,
          const string& configuration_basename, const string& urdf_filename) {
   auto file_resolver =
@@ -145,9 +146,15 @@ void Run(const string& trajectory_filename, const string& bag_filename,
   carto::common::LuaParameterDictionary lua_parameter_dictionary(
       code, std::move(file_resolver));
 
-  std::ifstream stream(trajectory_filename.c_str());
-  carto::mapping::proto::Trajectory trajectory_proto;
-  CHECK(trajectory_proto.ParseFromIstream(&stream));
+  std::ifstream stream(pose_graph_filename.c_str());
+  carto::mapping::proto::SparsePoseGraph pose_graph_proto;
+  CHECK(pose_graph_proto.ParseFromIstream(&stream));
+  CHECK_EQ(pose_graph_proto.trajectory_size(), 1)
+      << "Only pose graphs containing a single trajectory are supported.";
+  const carto::mapping::proto::Trajectory& trajectory_proto =
+      pose_graph_proto.trajectory(0);
+  CHECK_GT(trajectory_proto.node_size(), 0)
+      << "Trajectory does not contain any nodes.";
 
   const auto file_writer_factory = [](const string& filename) {
     return carto::common::make_unique<carto::io::StreamFileWriter>(filename);
@@ -222,10 +229,10 @@ int main(int argc, char** argv) {
   CHECK(!FLAGS_configuration_basename.empty())
       << "-configuration_basename is missing.";
   CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
-  CHECK(!FLAGS_trajectory_filename.empty())
-      << "-trajectory_filename is missing.";
+  CHECK(!FLAGS_pose_graph_filename.empty())
+      << "-pose_graph_filename is missing.";
 
-  ::cartographer_ros::Run(FLAGS_trajectory_filename, FLAGS_bag_filename,
+  ::cartographer_ros::Run(FLAGS_pose_graph_filename, FLAGS_bag_filename,
                           FLAGS_configuration_directory,
                           FLAGS_configuration_basename, FLAGS_urdf_filename);
 }

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -59,22 +59,29 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
   sensor_bridges_.erase(trajectory_id);
 }
 
-void MapBuilderBridge::WriteAssets(const string& stem) {
-  std::vector<::cartographer::mapping::TrajectoryNode> trajectory_nodes;
-  for (const auto& single_trajectory :
-       map_builder_.sparse_pose_graph()->GetTrajectoryNodes()) {
-    trajectory_nodes.insert(trajectory_nodes.end(), single_trajectory.begin(),
-                            single_trajectory.end());
+void MapBuilderBridge::SerializeState(const std::string& stem) {
+  std::ofstream proto_file(stem + ".pb",
+                           std::ios_base::out | std::ios_base::binary);
+  CHECK(map_builder_.sparse_pose_graph()->ToProto().SerializeToOstream(
+      &proto_file))
+      << "Could not serialize pose graph.";
+  proto_file.close();
+  CHECK(proto_file) << "Could not write pose graph.";
 }
-  if (trajectory_nodes.empty()) {
+
+void MapBuilderBridge::WriteAssets(const string& stem) {
+  const auto trajectory_nodes =
+      map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
+  // TODO(yutakaoka): Add multi-trajectory support.
+  CHECK_EQ(trajectory_options_.count(0), 1);
+  CHECK_EQ(trajectory_nodes.size(), 1);
+  if (trajectory_nodes[0].empty()) {
     LOG(WARNING) << "No data was collected and no assets will be written.";
   } else {
     LOG(INFO) << "Writing assets with stem '" << stem << "'...";
-    // TODO(yutakaoka): Add multi-trajectory support.
-    CHECK_EQ(trajectory_options_.count(0), 1);
     if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
       Write2DAssets(
-          trajectory_nodes, node_options_.map_frame,
+          trajectory_nodes[0], node_options_.map_frame,
           trajectory_options_[0]
               .trajectory_builder_options.trajectory_builder_2d_options()
               .submaps_options(),
@@ -83,7 +90,7 @@ void MapBuilderBridge::WriteAssets(const string& stem) {
 
     if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
       Write3DAssets(
-          trajectory_nodes,
+          trajectory_nodes[0],
           trajectory_options_[0]
               .trajectory_builder_options.trajectory_builder_3d_options()
               .submaps_options()

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -52,6 +52,7 @@ class MapBuilderBridge {
   int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids,
                     const TrajectoryOptions& trajectory_options);
   void FinishTrajectory(int trajectory_id);
+  void SerializeState(const string& stem);
   void WriteAssets(const string& stem);
 
   bool HandleSubmapQuery(

cartographer_ros/cartographer_ros/node.cc

@@ -484,6 +484,7 @@ bool Node::HandleWriteAssets(
     ::cartographer_ros_msgs::WriteAssets::Request& request,
     ::cartographer_ros_msgs::WriteAssets::Response& response) {
   carto::common::MutexLocker lock(&mutex_);
+  map_builder_bridge_.SerializeState(request.stem);
   map_builder_bridge_.WriteAssets(request.stem);
   return true;
 }

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -262,6 +262,7 @@ void Run(const std::vector<string>& bag_filenames) {
     node.map_builder_bridge()->FinishTrajectory(trajectory_id);
   }
 
+  node.map_builder_bridge()->SerializeState(bag_filenames.front());
   node.map_builder_bridge()->WriteAssets(bag_filenames.front());
 }
 

cartographer_ros/launch/assets_writer_backpack_2d.launch

@@ -21,7 +21,7 @@
           -configuration_basename assets_writer_backpack_2d.lua
           -urdf_filename $(find cartographer_ros)/urdf/backpack_2d.urdf
           -bag_filename $(arg bag_filenames)
-          -trajectory_filename $(arg trajectory_filename)"
+          -pose_graph_filename $(arg pose_graph_filename)"
       output="screen">
   </node>
 </launch>

cartographer_ros/launch/assets_writer_backpack_3d.launch

@@ -21,7 +21,7 @@
           -configuration_basename assets_writer_backpack_3d.lua
           -urdf_filename $(find cartographer_ros)/urdf/backpack_3d.urdf
           -bag_filename $(arg bag_filenames)
-          -trajectory_filename $(arg trajectory_filename)"
+          -pose_graph_filename $(arg pose_graph_filename)"
       output="screen">
   </node>
 </launch>