Use landmarks in optimization (both 2D & 3D). (#884)

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)

cartographer/mapping/pose_graph_interface.h

@@ -19,6 +19,7 @@
 
 #include <vector>
 
+#include "cartographer/common/optional.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/mapping/submaps.h"
 
@@ -58,7 +59,7 @@ class PoseGraphInterface {
       double rotation_weight;
     };
     std::vector<LandmarkObservation> landmark_observations;
-    transform::Rigid3d global_landmark_pose;
+    common::optional<transform::Rigid3d> global_landmark_pose;
   };
 
   struct SubmapPose {

cartographer/mapping_2d/pose_graph.cc

@@ -550,7 +550,8 @@ void PoseGraph::RunOptimization() {
   // frozen_trajectories_ when executing the Solve. Solve is time consuming,
   // so not taking the mutex before Solve to avoid blocking foreground
   // processing.
-  optimization_problem_.Solve(constraints_, frozen_trajectories_);
+  optimization_problem_.Solve(constraints_, frozen_trajectories_,
+                              landmark_nodes_);
   common::MutexLocker locker(&mutex_);
 
   const auto& submap_data = optimization_problem_.submap_data();

cartographer/mapping_2d/pose_graph/optimization_problem.cc

@@ -27,6 +27,8 @@
 #include "cartographer/common/ceres_solver_options.h"
 #include "cartographer/common/histogram.h"
 #include "cartographer/common/math.h"
+#include "cartographer/mapping/pose_graph/ceres_pose.h"
+#include "cartographer/mapping_2d/pose_graph/landmark_cost_function.h"
 #include "cartographer/mapping_2d/pose_graph/spa_cost_function.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/transform/transform.h"
@@ -39,6 +41,9 @@ namespace pose_graph {
 
 namespace {
 
+using ::cartographer::mapping::pose_graph::CeresPose;
+using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
+
 // Converts a pose into the 3 optimization variable format used for Ceres:
 // translation in x and y, followed by the rotation angle representing the
 // orientation.
@@ -52,6 +57,63 @@ transform::Rigid2d ToPose(const std::array<double, 3>& values) {
   return transform::Rigid2d({values[0], values[1]}, values[2]);
 }
 
+// Selects a trajectory node closest in time to the landmark observation and
+// applies a relative transform from it.
+transform::Rigid3d GetInitialLandmarkPose(
+    const LandmarkNode::LandmarkObservation& observation,
+    const NodeData& prev_node, const NodeData& next_node) {
+  const NodeData& closest_node =
+      observation.time - prev_node.time < next_node.time - observation.time
+          ? prev_node
+          : next_node;
+  return transform::Embed3D(closest_node.pose) *
+         transform::Rigid3d::Rotation(closest_node.gravity_alignment) *
+         observation.landmark_to_tracking_transform;
+}
+
+void AddLandmarkCostFunctions(
+    const std::map<std::string, LandmarkNode>& landmark_nodes,
+    const mapping::MapById<mapping::NodeId, NodeData>& node_data,
+    mapping::MapById<mapping::NodeId, std::array<double, 3>>* C_nodes,
+    std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
+  for (const auto& landmark_node : landmark_nodes) {
+    for (const auto& observation : landmark_node.second.landmark_observations) {
+      const std::string& landmark_id = landmark_node.first;
+      // Find the trajectory nodes before and after the landmark observation.
+      auto next =
+          node_data.lower_bound(observation.trajectory_id, observation.time);
+      // The landmark observation was made before the trajectory was created.
+      if (next == node_data.BeginOfTrajectory(observation.trajectory_id)) {
+        continue;
+      }
+      // The landmark observation was made, but the next trajectory node has
+      // not been added yet.
+      if (next == node_data.EndOfTrajectory(observation.trajectory_id)) {
+        continue;
+      }
+      auto prev = std::prev(next);
+      // Add parameter blocks for the landmark ID if they were not added before.
+      if (!C_landmarks->count(landmark_id)) {
+        const transform::Rigid3d starting_point =
+            landmark_node.second.global_landmark_pose.has_value()
+                ? landmark_node.second.global_landmark_pose.value()
+                : GetInitialLandmarkPose(observation, prev->data, next->data);
+        C_landmarks->emplace(
+            landmark_id,
+            CeresPose(starting_point, nullptr /* translation_parametrization */,
+                      common::make_unique<ceres::QuaternionParameterization>(),
+                      problem));
+      }
+      problem->AddResidualBlock(
+          LandmarkCostFunction::CreateAutoDiffCostFunction(
+              observation, prev->data, next->data),
+          nullptr /* loss function */, C_nodes->at(prev->id).data(),
+          C_nodes->at(next->id).data(), C_landmarks->at(landmark_id).rotation(),
+          C_landmarks->at(landmark_id).translation());
+    }
+  }
+}
+
 }  // namespace
 
 OptimizationProblem::OptimizationProblem(
@@ -112,8 +174,10 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
       max_num_iterations);
 }
 
-void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
+void OptimizationProblem::Solve(
-                                const std::set<int>& frozen_trajectories) {
+    const std::vector<Constraint>& constraints,
+    const std::set<int>& frozen_trajectories,
+    const std::map<std::string, LandmarkNode>& landmark_nodes) {
   if (node_data_.empty()) {
     // Nothing to optimize.
     return;
@@ -126,6 +190,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   // TODO(hrapp): Move ceres data into SubmapData.
   mapping::MapById<mapping::SubmapId, std::array<double, 3>> C_submaps;
   mapping::MapById<mapping::NodeId, std::array<double, 3>> C_nodes;
+  std::map<std::string, CeresPose> C_landmarks;
   bool first_submap = true;
   for (const auto& submap_id_data : submap_data_) {
     const bool frozen =
@@ -160,6 +225,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
         C_submaps.at(constraint.submap_id).data(),
         C_nodes.at(constraint.node_id).data());
   }
+  // Add cost  functions for landmarks.
+  AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
+                           &problem);
   // Add penalties for violating odometry or changes between consecutive nodes
   // if odometry is not available.
   for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
@@ -222,6 +290,11 @@ OptimizationProblem::submap_data() const {
   return submap_data_;
 }
 
+const std::map<std::string, transform::Rigid2d>&
+OptimizationProblem::landmark_data() const {
+  return landmark_data_;
+}
+
 const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
     const {
   return imu_data_;

cartographer/mapping_2d/pose_graph/optimization_problem.h

@@ -54,6 +54,7 @@ struct SubmapData {
 class OptimizationProblem {
  public:
   using Constraint = mapping::PoseGraphInterface::Constraint;
+  using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode;
 
   explicit OptimizationProblem(
       const mapping::pose_graph::proto::OptimizationProblemOptions& options);
@@ -84,10 +85,12 @@ class OptimizationProblem {
 
   // Optimizes the global poses.
   void Solve(const std::vector<Constraint>& constraints,
-             const std::set<int>& frozen_trajectories);
+             const std::set<int>& frozen_trajectories,
+             const std::map<std::string, LandmarkNode>& landmark_nodes);
 
   const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
   const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
+  const std::map<std::string, transform::Rigid2d>& landmark_data() const;
   const sensor::MapByTime<sensor::ImuData>& imu_data() const;
   const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
 
@@ -102,6 +105,7 @@ class OptimizationProblem {
   mapping::pose_graph::proto::OptimizationProblemOptions options_;
   mapping::MapById<mapping::NodeId, NodeData> node_data_;
   mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
+  std::map<std::string, transform::Rigid2d> landmark_data_;
   sensor::MapByTime<sensor::ImuData> imu_data_;
   sensor::MapByTime<sensor::OdometryData> odometry_data_;
 };

cartographer/mapping_3d/pose_graph.cc

@@ -578,7 +578,8 @@ void PoseGraph::RunOptimization() {
   // No other thread is accessing the optimization_problem_, constraints_ and
   // frozen_trajectories_ when executing the Solve. Solve is time consuming, so
   // not taking the mutex before Solve to avoid blocking foreground processing.
-  optimization_problem_.Solve(constraints_, frozen_trajectories_);
+  optimization_problem_.Solve(constraints_, frozen_trajectories_,
+                              landmark_nodes_);
   common::MutexLocker locker(&mutex_);
 
   const auto& submap_data = optimization_problem_.submap_data();

cartographer/mapping_3d/pose_graph/optimization_problem.cc

@@ -34,6 +34,7 @@
 #include "cartographer/internal/mapping_3d/rotation_cost_function.h"
 #include "cartographer/mapping/pose_graph/ceres_pose.h"
 #include "cartographer/mapping_3d/imu_integration.h"
+#include "cartographer/mapping_3d/pose_graph/landmark_cost_function.h"
 #include "cartographer/mapping_3d/pose_graph/spa_cost_function.h"
 #include "cartographer/mapping_3d/rotation_parameterization.h"
 #include "cartographer/transform/timestamped_transform.h"
@@ -49,6 +50,7 @@ namespace pose_graph {
 namespace {
 
 using ::cartographer::mapping::pose_graph::CeresPose;
+using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
 
 // For odometry.
 std::unique_ptr<transform::Rigid3d> Interpolate(
@@ -98,6 +100,63 @@ std::unique_ptr<transform::Rigid3d> Interpolate(
   return nullptr;
 }
 
+// Selects a trajectory node closest in time to the landmark observation and
+// applies a relative transform from it.
+transform::Rigid3d GetInitialLandmarkPose(
+    const LandmarkNode::LandmarkObservation& observation,
+    const NodeData& prev_node, const NodeData& next_node) {
+  const NodeData& closest_node =
+      observation.time - prev_node.time < next_node.time - observation.time
+          ? prev_node
+          : next_node;
+  return closest_node.global_pose * observation.landmark_to_tracking_transform;
+}
+
+void AddLandmarkCostFunctions(
+    const std::map<std::string, LandmarkNode>& landmark_nodes,
+    const mapping::MapById<mapping::NodeId, NodeData>& node_data,
+    mapping::MapById<mapping::NodeId, CeresPose>* C_nodes,
+    std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
+  for (const auto& landmark_node : landmark_nodes) {
+    for (const auto& observation : landmark_node.second.landmark_observations) {
+      const std::string& landmark_id = landmark_node.first;
+      // Find the trajectory nodes before and after the landmark observation.
+      auto next =
+          node_data.lower_bound(observation.trajectory_id, observation.time);
+      // The landmark observation was made before the trajectory was created.
+      if (next == node_data.BeginOfTrajectory(observation.trajectory_id)) {
+        continue;
+      }
+      // The landmark observation was made, but the next trajectory node has
+      // not been added yet.
+      if (next == node_data.EndOfTrajectory(observation.trajectory_id)) {
+        continue;
+      }
+      auto prev = std::prev(next);
+      // Add parameter blocks for the landmark ID if they were not added before.
+      if (!C_landmarks->count(landmark_id)) {
+        const transform::Rigid3d starting_point =
+            landmark_node.second.global_landmark_pose.has_value()
+                ? landmark_node.second.global_landmark_pose.value()
+                : GetInitialLandmarkPose(observation, prev->data, next->data);
+        C_landmarks->emplace(
+            landmark_id,
+            CeresPose(starting_point, nullptr /* translation_parametrization */,
+                      common::make_unique<ceres::QuaternionParameterization>(),
+                      problem));
+      }
+      problem->AddResidualBlock(
+          LandmarkCostFunction::CreateAutoDiffCostFunction(
+              observation, prev->data, next->data),
+          nullptr /* loss function */, C_nodes->at(prev->id).rotation(),
+          C_nodes->at(prev->id).translation(), C_nodes->at(next->id).rotation(),
+          C_nodes->at(next->id).translation(),
+          C_landmarks->at(landmark_id).rotation(),
+          C_landmarks->at(landmark_id).translation());
+    }
+  }
+}
+
 }  // namespace
 
 OptimizationProblem::OptimizationProblem(
@@ -169,8 +228,10 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
       max_num_iterations);
 }
 
-void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
+void OptimizationProblem::Solve(
-                                const std::set<int>& frozen_trajectories) {
+    const std::vector<Constraint>& constraints,
+    const std::set<int>& frozen_trajectories,
+    const std::map<std::string, LandmarkNode>& landmark_nodes) {
   if (node_data_.empty()) {
     // Nothing to optimize.
     return;
@@ -192,6 +253,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   CHECK(submap_data_.Contains(mapping::SubmapId{0, 0}));
   mapping::MapById<mapping::SubmapId, CeresPose> C_submaps;
   mapping::MapById<mapping::NodeId, CeresPose> C_nodes;
+  std::map<std::string, CeresPose> C_landmarks;
   bool first_submap = true;
   for (const auto& submap_id_data : submap_data_) {
     const bool frozen =
@@ -251,6 +313,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
         C_nodes.at(constraint.node_id).rotation(),
         C_nodes.at(constraint.node_id).translation());
   }
+  // Add cost  functions for landmarks.
+  AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
+                           &problem);
   // Add constraints based on IMU observations of angular velocities and
   // linear acceleration.
   if (fix_z_ == FixZ::kNo) {
@@ -482,6 +547,11 @@ OptimizationProblem::submap_data() const {
   return submap_data_;
 }
 
+const std::map<std::string, transform::Rigid3d>&
+OptimizationProblem::landmark_data() const {
+  return landmark_data_;
+}
+
 const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
     const {
   return imu_data_;

cartographer/mapping_3d/pose_graph/optimization_problem.h

@@ -54,6 +54,7 @@ struct SubmapData {
 class OptimizationProblem {
  public:
   using Constraint = mapping::PoseGraphInterface::Constraint;
+  using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode;
 
   enum class FixZ { kYes, kNo };
 
@@ -88,10 +89,12 @@ class OptimizationProblem {
 
   // Optimizes the global poses.
   void Solve(const std::vector<Constraint>& constraints,
-             const std::set<int>& frozen_trajectories);
+             const std::set<int>& frozen_trajectories,
+             const std::map<std::string, LandmarkNode>& landmark_nodes);
 
   const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
   const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
+  const std::map<std::string, transform::Rigid3d>& landmark_data() const;
   const sensor::MapByTime<sensor::ImuData>& imu_data() const;
   const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
   const sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
@@ -113,6 +116,7 @@ class OptimizationProblem {
   FixZ fix_z_;
   mapping::MapById<mapping::NodeId, NodeData> node_data_;
   mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
+  std::map<std::string, transform::Rigid3d> landmark_data_;
   sensor::MapByTime<sensor::ImuData> imu_data_;
   sensor::MapByTime<sensor::OdometryData> odometry_data_;
   sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;

cartographer/mapping_3d/pose_graph/optimization_problem_test.cc

@@ -170,7 +170,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
   const std::set<int> kFrozen;
-  optimization_problem_.Solve(constraints, kFrozen);
+  optimization_problem_.Solve(constraints, kFrozen, {});
 
   double translation_error_after = 0.;
   double rotation_error_after = 0.;