Store node initial pose in constant data. (#568)

cartographer/mapping/global_trajectory_builder.h

@@ -51,9 +51,8 @@ class GlobalTrajectoryBuilder
     if (insertion_result == nullptr) {
       return;
     }
-    sparse_pose_graph_->AddScan(
+    sparse_pose_graph_->AddScan(insertion_result->constant_data, trajectory_id_,
-        insertion_result->constant_data, insertion_result->pose_observation,
+                                insertion_result->insertion_submaps);
-        trajectory_id_, insertion_result->insertion_submaps);
   }
 
   void AddSensorData(const sensor::ImuData& imu_data) override {

cartographer/mapping/proto/trajectory_node_data.proto

@@ -28,4 +28,5 @@ message TrajectoryNodeData {
   optional sensor.proto.CompressedPointCloud high_resolution_point_cloud = 4;
   optional sensor.proto.CompressedPointCloud low_resolution_point_cloud = 5;
   repeated float rotational_scan_matcher_histogram = 6;
+  optional transform.proto.Rigid3d initial_pose = 7;
 }

cartographer/mapping/trajectory_node.cc

@@ -44,6 +44,8 @@ proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data) {
     proto.add_rotational_scan_matcher_histogram(
         constant_data.rotational_scan_matcher_histogram(i));
   }
+  *proto.mutable_initial_pose() =
+      transform::ToProto(constant_data.initial_pose);
   return proto;
 }
 
@@ -63,7 +65,8 @@ TrajectoryNode::Data FromProto(const proto::TrajectoryNodeData& proto) {
           .Decompress(),
       sensor::CompressedPointCloud(proto.low_resolution_point_cloud())
           .Decompress(),
-      rotational_scan_matcher_histogram};
+      rotational_scan_matcher_histogram,
+      transform::ToRigid3(proto.initial_pose())};
 }
 
 }  // namespace mapping

cartographer/mapping/trajectory_node.h

@@ -45,6 +45,9 @@ struct TrajectoryNode {
     sensor::PointCloud high_resolution_point_cloud;
     sensor::PointCloud low_resolution_point_cloud;
     Eigen::VectorXf rotational_scan_matcher_histogram;
+
+    // The initial unoptimized node pose.
+    transform::Rigid3d initial_pose;
   };
 
   common::Time time() const { return constant_data->time; }

cartographer/mapping/trajectory_node_test.cc

@@ -21,6 +21,7 @@
 #include "Eigen/Core"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/proto/trajectory_node_data.pb.h"
+#include "cartographer/transform/rigid_transform_test_helpers.h"
 #include "gtest/gtest.h"
 
 namespace cartographer {
@@ -36,7 +37,8 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
       sensor::CompressedPointCloud({{2.f, 3.f, 4.f}}).Decompress(),
       sensor::CompressedPointCloud({{-1.f, 2.f, 0.f}}).Decompress(),
       Eigen::VectorXf::Unit(20, 4),
-  };
+      transform::Rigid3d({1., 2., 3.},
+                         Eigen::Quaterniond(4., 5., -6., -7.).normalized())};
   const proto::TrajectoryNodeData proto = ToProto(expected);
   const TrajectoryNode::Data actual = FromProto(proto);
   EXPECT_EQ(expected.time, actual.time);
@@ -49,6 +51,8 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
             actual.low_resolution_point_cloud);
   EXPECT_EQ(expected.rotational_scan_matcher_histogram,
             actual.rotational_scan_matcher_histogram);
+  EXPECT_THAT(actual.initial_pose,
+              transform::IsNearly(expected.initial_pose, 1e-9));
 }
 
 }  // namespace

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -184,8 +184,9 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
               filtered_gravity_aligned_point_cloud,
               {},  // 'high_resolution_point_cloud' is only used in 3D.
               {},  // 'low_resolution_point_cloud' is only used in 3D.
-          }),
+              {},  // 'rotational_scan_matcher_histogram' is only used in 3D.
-      pose_estimate, std::move(insertion_submaps)});
+              pose_estimate}),
+      std::move(insertion_submaps)});
 }
 
 const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {

cartographer/mapping_2d/local_trajectory_builder.h

@@ -42,7 +42,6 @@ class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
-    transform::Rigid3d pose_observation;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };
 

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -96,10 +96,10 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 
 void SparsePoseGraph::AddScan(
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-    const transform::Rigid3d& pose, const int trajectory_id,
+    const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
-      GetLocalToGlobalTransform(trajectory_id) * pose);
+      GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose);
 
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
@@ -132,11 +132,8 @@ void SparsePoseGraph::AddScan(
   // execute the lambda.
   const bool newly_finished_submap = insertion_submaps.front()->finished();
   AddWorkItem([=]() REQUIRES(mutex_) {
-    ComputeConstraintsForScan(
+    ComputeConstraintsForScan(trajectory_id, insertion_submaps,
-        trajectory_id, insertion_submaps, newly_finished_submap,
+                              newly_finished_submap);
-        transform::Project2D(pose *
-                             transform::Rigid3d::Rotation(
-                                 constant_data->gravity_alignment.inverse())));
   });
 }
 
@@ -227,19 +224,11 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
 void SparsePoseGraph::ComputeConstraintsForScan(
     const int trajectory_id,
     std::vector<std::shared_ptr<const Submap>> insertion_submaps,
-    const bool newly_finished_submap, const transform::Rigid2d& pose) {
+    const bool newly_finished_submap) {
   const std::vector<mapping::SubmapId> submap_ids =
       GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
   CHECK_EQ(submap_ids.size(), insertion_submaps.size());
   const mapping::SubmapId matching_id = submap_ids.front();
-  const transform::Rigid2d optimized_pose =
-      optimization_problem_.submap_data()
-          .at(matching_id.trajectory_id)
-          .at(matching_id.submap_index)
-          .pose *
-      sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
-          .inverse() *
-      pose;
   const mapping::NodeId node_id{
       matching_id.trajectory_id,
       static_cast<size_t>(matching_id.trajectory_id) <
@@ -252,10 +241,21 @@ void SparsePoseGraph::ComputeConstraintsForScan(
           ->first +
           1)
       : 0};
-  const auto& node_data = trajectory_nodes_.at(node_id).constant_data;
+  const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
-  optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
+  const transform::Rigid2d pose = transform::Project2D(
-                                          node_data->time, pose, optimized_pose,
+      constant_data->initial_pose *
-                                          node_data->gravity_alignment);
+      transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
+  const transform::Rigid2d optimized_pose =
+      optimization_problem_.submap_data()
+          .at(matching_id.trajectory_id)
+          .at(matching_id.submap_index)
+          .pose *
+      sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
+          .inverse() *
+      pose;
+  optimization_problem_.AddTrajectoryNode(
+      matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
+      constant_data->gravity_alignment);
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
     const mapping::SubmapId submap_id = submap_ids[i];
     // Even if this was the last scan added to 'submap_id', the submap will only

cartographer/mapping_2d/sparse_pose_graph.h

@@ -71,7 +71,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // 'true', this submap was inserted into for the last time.
   void AddScan(
       std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-      const transform::Rigid3d& pose, int trajectory_id,
+      int trajectory_id,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);
 
@@ -130,8 +130,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void ComputeConstraintsForScan(
       int trajectory_id,
       std::vector<std::shared_ptr<const Submap>> insertion_submaps,
-      bool newly_finished_submap, const transform::Rigid2d& pose)
+      bool newly_finished_submap) REQUIRES(mutex_);
-      REQUIRES(mutex_);
 
   // Computes constraints for a scan and submap pair.
   void ComputeConstraint(const mapping::NodeId& node_id,

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -164,8 +164,9 @@ class SparsePoseGraphTest : public ::testing::Test {
                                           range_data.returns,
                                           {},
                                           {},
-                                          {}}),
+                                          {},
-        transform::Embed3D(pose_estimate), kTrajectoryId, insertion_submaps);
+                                          transform::Embed3D(pose_estimate)}),
+        kTrajectoryId, insertion_submaps);
   }
 
   void MoveRelative(const transform::Rigid2d& movement) {

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -221,8 +221,9 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
               {},  // 'filtered_point_cloud' is only used in 2D.
               high_resolution_point_cloud,
               low_resolution_point_cloud,
-              rotational_scan_matcher_histogram}),
+              rotational_scan_matcher_histogram,
-      pose_observation, std::move(insertion_submaps)});
+              pose_observation}),
+      std::move(insertion_submaps)});
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/local_trajectory_builder.h

@@ -42,7 +42,6 @@ class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
-    transform::Rigid3d pose_observation;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };
 

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -94,10 +94,10 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 
 void SparsePoseGraph::AddScan(
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-    const transform::Rigid3d& pose, const int trajectory_id,
+    const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
-      GetLocalToGlobalTransform(trajectory_id) * pose);
+      GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose);
 
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
@@ -131,7 +131,7 @@ void SparsePoseGraph::AddScan(
   const bool newly_finished_submap = insertion_submaps.front()->finished();
   AddWorkItem([=]() REQUIRES(mutex_) {
     ComputeConstraintsForScan(trajectory_id, insertion_submaps,
-                              newly_finished_submap, pose);
+                              newly_finished_submap);
   });
 }
 
@@ -250,17 +250,11 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
 void SparsePoseGraph::ComputeConstraintsForScan(
     const int trajectory_id,
     std::vector<std::shared_ptr<const Submap>> insertion_submaps,
-    const bool newly_finished_submap, const transform::Rigid3d& pose) {
+    const bool newly_finished_submap) {
   const std::vector<mapping::SubmapId> submap_ids =
       GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
   CHECK_EQ(submap_ids.size(), insertion_submaps.size());
   const mapping::SubmapId matching_id = submap_ids.front();
-  const transform::Rigid3d optimized_pose =
-      optimization_problem_.submap_data()
-          .at(matching_id.trajectory_id)
-          .at(matching_id.submap_index)
-          .pose *
-      insertion_submaps.front()->local_pose().inverse() * pose;
   const mapping::NodeId node_id{
       matching_id.trajectory_id,
       static_cast<size_t>(matching_id.trajectory_id) <
@@ -273,9 +267,16 @@ void SparsePoseGraph::ComputeConstraintsForScan(
           ->first +
           1)
       : 0};
-  const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
+  const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
+  const transform::Rigid3d& pose = constant_data->initial_pose;
+  const transform::Rigid3d optimized_pose =
+      optimization_problem_.submap_data()
+          .at(matching_id.trajectory_id)
+          .at(matching_id.submap_index)
+          .pose *
+      insertion_submaps.front()->local_pose().inverse() * pose;
   optimization_problem_.AddTrajectoryNode(
-      matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
+      matching_id.trajectory_id, constant_data->time, pose, optimized_pose);
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
     const mapping::SubmapId submap_id = submap_ids[i];
     // Even if this was the last scan added to 'submap_id', the submap will only

cartographer/mapping_3d/sparse_pose_graph.h

@@ -72,7 +72,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // 'true', this submap was inserted into for the last time.
   void AddScan(
       std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-      const transform::Rigid3d& pose, int trajectory_id,
+      int trajectory_id,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);
 
@@ -131,8 +131,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void ComputeConstraintsForScan(
       int trajectory_id,
       std::vector<std::shared_ptr<const Submap>> insertion_submaps,
-      bool newly_finished_submap, const transform::Rigid3d& pose)
+      bool newly_finished_submap) REQUIRES(mutex_);
-      REQUIRES(mutex_);
 
   // Computes constraints for a scan and submap pair.
   void ComputeConstraint(const mapping::NodeId& node_id,