Expose complete poses in 2D SLAM. (#498)

This changes the trajectory nodes to contain the complete 'pose'
including 'tracking_to_tracking_2d' applied already. Similar for
the 'zbar_ij' as it is returned by 'constraints()'. This allows
2D and 3D to be handled in the same way.

cartographer/mapping/map_builder.cc

@@ -183,9 +183,7 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
         range_data_proto->mutable_node_id()->set_node_index(node_index);
         const auto& data = *node_data[trajectory_id][node_index].constant_data;
         *range_data_proto->mutable_range_data() =
-            sensor::ToProto(sensor::Compress(sensor::TransformRangeData(
-                sensor::Decompress(data.range_data),
-                data.tracking_to_pose.inverse().cast<float>())));
+            sensor::ToProto(data.range_data);
         // TODO(whess): Only enable optionally? Resulting pbstream files will be
         // a lot larger now.
         writer->WriteProto(proto);

cartographer/mapping/sparse_pose_graph.cc

@@ -84,8 +84,7 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
         auto* node_proto = trajectory_proto->add_node();
         node_proto->set_timestamp(
             common::ToUniversal(node.constant_data->time));
-        *node_proto->mutable_pose() = transform::ToProto(
-            node.pose * node.constant_data->tracking_to_pose);
+        *node_proto->mutable_pose() = transform::ToProto(node.pose);
       }
     }
 
@@ -107,10 +106,8 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
 
   for (const auto& constraint : constraints()) {
     auto* const constraint_proto = proto.add_constraint();
-    const auto& node = all_trajectory_nodes.at(constraint.node_id.trajectory_id)
-                           .at(constraint.node_id.node_index);
-    *constraint_proto->mutable_relative_pose() = transform::ToProto(
-        constraint.pose.zbar_ij * node.constant_data->tracking_to_pose);
+    *constraint_proto->mutable_relative_pose() =
+        transform::ToProto(constraint.pose.zbar_ij);
     constraint_proto->set_translation_weight(
         constraint.pose.translation_weight);
     constraint_proto->set_rotation_weight(constraint.pose.rotation_weight);

cartographer/mapping/trajectory_node.h

@@ -32,7 +32,7 @@ struct TrajectoryNode {
   struct Data {
     common::Time time;
 
-    // Range data in 'pose' frame.
+    // Range data in 'tracking' frame. Only used in 3D.
     sensor::CompressedRangeData range_data;
 
     // Used for loop closure in 2D: voxel filtered returns in 'pose' frame.
@@ -42,10 +42,9 @@ struct TrajectoryNode {
     sensor::PointCloud high_resolution_point_cloud;
     sensor::PointCloud low_resolution_point_cloud;
 
-    // Transform from the 3D 'tracking' frame to the 'pose' frame of the range
-    // data, which contains roll, pitch and height for 2D. In 3D this is always
-    // identity.
-    transform::Rigid3d tracking_to_pose;
+    // Transforms the 'tracking' frame into a gravity-aligned 'tracking_2d'
+    // frame. Only used in 2D.
+    transform::Rigid3d tracking_to_tracking_2d;
   };
 
   common::Time time() const { return constant_data->time; }

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -98,7 +98,8 @@ void SparsePoseGraph::AddScan(
     const transform::Rigid2d& pose, const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
-      GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
+      GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose) *
+      constant_data->tracking_to_tracking_2d);
 
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
@@ -445,7 +446,8 @@ void SparsePoseGraph::RunOptimization() {
          ++node_data_index, ++node_index) {
       const mapping::NodeId node_id{trajectory_id, node_index};
       trajectory_nodes_.at(node_id).pose =
-          transform::Embed3D(node_data[trajectory_id][node_data_index].pose);
+          transform::Embed3D(node_data[trajectory_id][node_data_index].pose) *
+          trajectory_nodes_.at(node_id).constant_data->tracking_to_tracking_2d;
     }
     // Extrapolate all point cloud poses that were added later.
     const auto local_to_new_global =
@@ -482,8 +484,19 @@ SparsePoseGraph::GetTrajectoryNodes() {
 }
 
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
+  std::vector<Constraint> result;
   common::MutexLocker locker(&mutex_);
-  return constraints_;
+  for (const Constraint& constraint : constraints_) {
+    result.push_back(Constraint{
+        constraint.submap_id, constraint.node_id,
+        Constraint::Pose{constraint.pose.zbar_ij *
+                             trajectory_nodes_.at(constraint.node_id)
+                                 .constant_data->tracking_to_tracking_2d,
+                         constraint.pose.translation_weight,
+                         constraint.pose.rotation_weight},
+        constraint.tag});
+  }
+  return result;
 }
 
 transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -213,7 +213,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
               {},  // 'filtered_point_cloud' is only used in 2D.
               high_resolution_point_cloud,
               low_resolution_point_cloud,
-              transform::Rigid3d::Identity()}),
+              {}}),  // 'tracking_to_tracking_2d' in only used in 2D.
       pose_observation, std::move(insertion_submaps)});
 }