Use odometry in 2D pose graph optimization. (#456)

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -52,6 +52,8 @@ void GlobalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
 void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
                                               const transform::Rigid3d& pose) {
   local_trajectory_builder_.AddOdometerData(time, pose);
+  sparse_pose_graph_->AddOdometerData(trajectory_id_,
+                                      sensor::OdometryData{time, pose});
 }
 
 const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -164,6 +164,14 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
   });
 }
 
+void SparsePoseGraph::AddOdometerData(
+    const int trajectory_id, const sensor::OdometryData& odometry_data) {
+  common::MutexLocker locker(&mutex_);
+  AddWorkItem([=]() REQUIRES(mutex_) {
+    optimization_problem_.AddOdometerData(trajectory_id, odometry_data);
+  });
+}
+
 void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
                                         const mapping::SubmapId& submap_id) {
   CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);

cartographer/mapping_2d/sparse_pose_graph.h

@@ -78,6 +78,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Adds new IMU data to be used in the optimization.
   void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddOdometerData(int trajectory_id,
+                       const sensor::OdometryData& odometry_data);
 
   void FreezeTrajectory(int trajectory_id) override;
   void AddSubmapFromProto(int trajectory_id,

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -28,7 +28,9 @@
 #include "cartographer/common/histogram.h"
 #include "cartographer/common/math.h"
 #include "cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h"
+#include "cartographer/sensor/odometry_data.h"
 #include "cartographer/transform/transform.h"
+#include "cartographer/transform/transform_interpolation_buffer.h"
 #include "ceres/ceres.h"
 #include "glog/logging.h"
 
@@ -68,6 +70,14 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
   imu_data_[trajectory_id].push_back(imu_data);
 }
 
+void OptimizationProblem::AddOdometerData(
+    const int trajectory_id, const sensor::OdometryData& odometry_data) {
+  CHECK_GE(trajectory_id, 0);
+  odometry_data_.resize(
+      std::max(odometry_data_.size(), static_cast<size_t>(trajectory_id) + 1));
+  odometry_data_[trajectory_id].Push(odometry_data.time, odometry_data.pose);
+}
+
 void OptimizationProblem::AddTrajectoryNode(
     const int trajectory_id, const common::Time time,
     const transform::Rigid2d& initial_point_cloud_pose,
@@ -170,7 +180,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
       }
     }
   }
-
   // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
     problem.AddResidualBlock(
@@ -192,20 +201,36 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
             .data());
   }
 
-  // Add penalties for changes between consecutive scans.
+  // Add penalties for violating odometry or changes between consecutive scans
+  // if odometry is not available.
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
     for (size_t node_data_index = 1;
          node_data_index < node_data_[trajectory_id].size();
          ++node_data_index) {
-      problem.AddResidualBlock(
+      const bool odometry_available =
-          new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
+          trajectory_id < odometry_data_.size() &&
-              new SpaCostFunction(Constraint::Pose{
+          odometry_data_[trajectory_id].Has(
-                  transform::Embed3D(
+              node_data_[trajectory_id][node_data_index].time) &&
+          odometry_data_[trajectory_id].Has(
+              node_data_[trajectory_id][node_data_index - 1].time);
+      const transform::Rigid3d relative_pose =
+          odometry_available
+              ? odometry_data_[trajectory_id]
+                        .Lookup(
+                            node_data_[trajectory_id][node_data_index - 1].time)
+                        .inverse() *
+                    odometry_data_[trajectory_id].Lookup(
+                        node_data_[trajectory_id][node_data_index].time)
+              : transform::Embed3D(
                     node_data_[trajectory_id][node_data_index - 1]
                         .initial_point_cloud_pose.inverse() *
                     node_data_[trajectory_id][node_data_index]
-                          .initial_point_cloud_pose),
+                        .initial_point_cloud_pose);
+      problem.AddResidualBlock(
+          new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
+              new SpaCostFunction(Constraint::Pose{
+                  relative_pose,
                   options_.consecutive_scan_translation_penalty_factor(),
                   options_.consecutive_scan_rotation_penalty_factor()})),
           nullptr /* loss function */,

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -30,6 +30,8 @@
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
 #include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/odometry_data.h"
+#include "cartographer/transform/transform_interpolation_buffer.h"
 
 namespace cartographer {
 namespace mapping_2d {
@@ -59,6 +61,8 @@ class OptimizationProblem {
   OptimizationProblem& operator=(const OptimizationProblem&) = delete;
 
   void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddOdometerData(int trajectory_id,
+                       const sensor::OdometryData& odometry_data);
   void AddTrajectoryNode(int trajectory_id, common::Time time,
                          const transform::Rigid2d& initial_point_cloud_pose,
                          const transform::Rigid2d& point_cloud_pose);
@@ -87,6 +91,7 @@ class OptimizationProblem {
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   std::vector<std::deque<sensor::ImuData>> imu_data_;
   std::vector<std::deque<NodeData>> node_data_;
+  std::vector<transform::TransformInterpolationBuffer> odometry_data_;
   std::vector<std::deque<SubmapData>> submap_data_;
   std::vector<TrajectoryData> trajectory_data_;
 };