Unify 2D/3D constraints. (#119)

This removes Constraint2D and instead embeds 2D constraints in a
Constraint3D which is renamed to Constraint.

cartographer/mapping/sparse_pose_graph.cc

@@ -28,22 +28,11 @@ namespace cartographer {
 namespace mapping {
 
 proto::SparsePoseGraph::Constraint::Tag ToProto(
-    const SparsePoseGraph::Constraint2D::Tag& tag) {
+    const SparsePoseGraph::Constraint::Tag& tag) {
   switch (tag) {
-    case SparsePoseGraph::Constraint2D::Tag::INTRA_SUBMAP:
+    case SparsePoseGraph::Constraint::Tag::INTRA_SUBMAP:
       return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP;
-    case SparsePoseGraph::Constraint2D::Tag::INTER_SUBMAP:
+    case SparsePoseGraph::Constraint::Tag::INTER_SUBMAP:
-      return proto::SparsePoseGraph::Constraint::INTER_SUBMAP;
-  }
-  LOG(FATAL) << "Unsupported tag.";
-}
-
-proto::SparsePoseGraph::Constraint::Tag ToProto(
-    const SparsePoseGraph::Constraint3D::Tag& tag) {
-  switch (tag) {
-    case SparsePoseGraph::Constraint3D::Tag::INTRA_SUBMAP:
-      return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP;
-    case SparsePoseGraph::Constraint3D::Tag::INTER_SUBMAP:
       return proto::SparsePoseGraph::Constraint::INTER_SUBMAP;
   }
   LOG(FATAL) << "Unsupported tag.";
@@ -85,26 +74,7 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
 
 proto::SparsePoseGraph SparsePoseGraph::ToProto() {
   proto::SparsePoseGraph proto;
-  for (const auto& constraint : constraints_2d()) {
+  for (const auto& constraint : constraints()) {
-    auto* const constraint_proto = proto.add_constraint();
-    *constraint_proto->mutable_relative_pose() =
-        transform::ToProto(transform::Embed3D(constraint.pose.zbar_ij));
-    for (int i = 0; i != 36; ++i) {
-      constraint_proto->mutable_sqrt_lambda()->Add(0.);
-    }
-    constexpr double kFakePositionCovariance = 1.;
-    constexpr double kFakeOrientationCovariance = 1.;
-    Eigen::Map<Eigen::Matrix<double, 6, 6>>(
-        constraint_proto->mutable_sqrt_lambda()->mutable_data()) =
-        kalman_filter::Embed3D(constraint.pose.sqrt_Lambda_ij,
-                               kFakePositionCovariance,
-                               kFakeOrientationCovariance);
-    // TODO(whess): Support multi-trajectory.
-    constraint_proto->mutable_submap_id()->set_submap_index(constraint.i);
-    constraint_proto->mutable_scan_id()->set_scan_index(constraint.j);
-    constraint_proto->set_tag(mapping::ToProto(constraint.tag));
-  }
-  for (const auto& constraint : constraints_3d()) {
     auto* const constraint_proto = proto.add_constraint();
     *constraint_proto->mutable_relative_pose() =
         transform::ToProto(constraint.pose.zbar_ij);

cartographer/mapping/sparse_pose_graph.h

@@ -42,28 +42,7 @@ class SparsePoseGraph {
   // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
   // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS),
   // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010.
-  struct Constraint2D {
+  struct Constraint {
-    struct Pose {
-      transform::Rigid2d zbar_ij;
-      Eigen::Matrix<double, 3, 3> sqrt_Lambda_ij;
-    };
-
-    // Submap index.
-    int i;
-
-    // Scan index.
-    int j;
-
-    // Pose of the scan 'j' relative to submap 'i'.
-    Pose pose;
-
-    // Differentiates between intra-submap (where scan 'j' was inserted into
-    // submap 'i') and inter-submap constraints (where scan 'j' was not inserted
-    // into submap 'i').
-    enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
-  };
-
-  struct Constraint3D {
     struct Pose {
       transform::Rigid3d zbar_ij;
       Eigen::Matrix<double, 6, 6> sqrt_Lambda_ij;
@@ -116,11 +95,8 @@ class SparsePoseGraph {
   // Returns the current optimized trajectory.
   virtual std::vector<TrajectoryNode> GetTrajectoryNodes() = 0;
 
-  // Returns the collection of 2D constraints.
+  // Returns the collection of constraints.
-  virtual std::vector<Constraint2D> constraints_2d() = 0;
+  virtual std::vector<Constraint> constraints() = 0;
-
-  // Returns the collection of 3D constraints.
-  virtual std::vector<Constraint3D> constraints_3d() = 0;
 
   // Serializes the constraints and the computed trajectory.
   //

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -172,14 +172,18 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     // Unchanged covariance as (submap <- map) is a translation.
     const transform::Rigid2d constraint_transform =
         sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
-    constraints_.push_back(Constraint2D{
+    constexpr double kFakePositionCovariance = 1.;
+    constexpr double kFakeOrientationCovariance = 1.;
+    constraints_.push_back(Constraint{
         submap_index,
         scan_index,
-        {constraint_transform,
+        {transform::Embed3D(constraint_transform),
          common::ComputeSpdMatrixSqrtInverse(
-             covariance, options_.constraint_builder_options()
+             kalman_filter::Embed3D(covariance, kFakePositionCovariance,
-                             .lower_covariance_eigenvalue_bound())},
+                                    kFakeOrientationCovariance),
-        Constraint2D::INTRA_SUBMAP});
+             options_.constraint_builder_options()
+                 .lower_covariance_eigenvalue_bound())},
+        Constraint::INTRA_SUBMAP});
   }
 
   // Determine if this scan should be globally localized.
@@ -389,14 +393,10 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
-std::vector<SparsePoseGraph::Constraint2D> SparsePoseGraph::constraints_2d() {
+std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   return constraints_;
 }
 
-std::vector<SparsePoseGraph::Constraint3D> SparsePoseGraph::constraints_3d() {
-  return {};
-}
-
 transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
     const mapping::Submaps& submaps) {
   return GetSubmapTransforms(submaps).back() *

cartographer/mapping_2d/sparse_pose_graph.h

@@ -90,8 +90,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_) override;
   std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
       EXCLUDES(mutex_);
-  std::vector<Constraint2D> constraints_2d() override;
+  std::vector<Constraint> constraints() override;
-  std::vector<Constraint3D> constraints_3d() override;
 
  private:
   struct SubmapState {
@@ -179,7 +178,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Current optimization problem.
   sparse_pose_graph::OptimizationProblem optimization_problem_;
   sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
-  std::vector<Constraint2D> constraints_;
+  std::vector<Constraint> constraints_;
   std::vector<transform::Rigid2d> initial_point_cloud_poses_;
   std::vector<transform::Rigid2d> point_cloud_poses_;  // (map <- point cloud)
   std::vector<transform::Rigid2d> submap_transforms_;  // (map <- submap)

cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt

@@ -31,10 +31,10 @@ google_library(mapping_2d_sparse_pose_graph_constraint_builder
     mapping_2d_scan_matching_fast_correlative_scan_matcher
     mapping_2d_scan_matching_proto_ceres_scan_matcher_options
     mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options
-    mapping_2d_sparse_pose_graph_optimization_problem
     mapping_2d_submaps
     mapping_3d_scan_matching_ceres_scan_matcher
     mapping_3d_scan_matching_fast_correlative_scan_matcher
+    mapping_sparse_pose_graph
     mapping_sparse_pose_graph_proto_constraint_builder_options
     mapping_trajectory_connectivity
     sensor_point_cloud
@@ -70,6 +70,8 @@ google_library(mapping_2d_sparse_pose_graph_spa_cost_function
     spa_cost_function.h
   DEPENDS
     common_math
+    kalman_filter_pose_tracker
     mapping_sparse_pose_graph
     transform_rigid_transform
+    transform_transform
 )

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -176,7 +176,7 @@ void ConstraintBuilder::ComputeConstraint(
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const sensor::PointCloud* const point_cloud,
     const transform::Rigid2d& initial_relative_pose,
-    std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
+    std::unique_ptr<ConstraintBuilder::Constraint>* constraint) {
   const transform::Rigid2d initial_pose =
       ComputeSubmapPose(*submap) * initial_relative_pose;
   const SubmapScanMatcher* const submap_scan_matcher =
@@ -226,13 +226,17 @@ void ConstraintBuilder::ComputeConstraint(
 
   const transform::Rigid2d constraint_transform =
       ComputeSubmapPose(*submap).inverse() * pose_estimate;
-  constraint->reset(new OptimizationProblem::Constraint{
+  constexpr double kFakePositionCovariance = 1.;
+  constexpr double kFakeOrientationCovariance = 1.;
+  constraint->reset(new Constraint{
       submap_index,
       scan_index,
-      {constraint_transform,
+      {transform::Embed3D(constraint_transform),
        common::ComputeSpdMatrixSqrtInverse(
-           covariance, options_.lower_covariance_eigenvalue_bound())},
+           kalman_filter::Embed3D(covariance, kFakePositionCovariance,
-      OptimizationProblem::Constraint::INTER_SUBMAP});
+                                  kFakeOrientationCovariance),
+           options_.lower_covariance_eigenvalue_bound())},
+      Constraint::INTER_SUBMAP});
 
   if (options_.log_matches()) {
     const transform::Rigid2d difference =
@@ -261,8 +265,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
     if (pending_computations_.empty()) {
       CHECK_EQ(submap_queued_work_items_.size(), 0);
       if (when_done_ != nullptr) {
-        for (const std::unique_ptr<OptimizationProblem::Constraint>&
+        for (const std::unique_ptr<Constraint>& constraint : constraints_) {
-                 constraint : constraints_) {
           if (constraint != nullptr) {
             result.push_back(*constraint);
           }

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h

@@ -30,11 +30,11 @@
 #include "cartographer/common/math.h"
 #include "cartographer/common/mutex.h"
 #include "cartographer/common/thread_pool.h"
+#include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
 #include "cartographer/mapping/trajectory_connectivity.h"
 #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
 #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
-#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h"
 #include "cartographer/mapping_2d/submaps.h"
 #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
 #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
@@ -59,7 +59,7 @@ transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap);
 // This class is thread-safe.
 class ConstraintBuilder {
  public:
-  using Constraint = mapping::SparsePoseGraph::Constraint2D;
+  using Constraint = mapping::SparsePoseGraph::Constraint;
   using Result = std::vector<Constraint>;
 
   ConstraintBuilder(

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -134,12 +134,17 @@ void OptimizationProblem::Solve(
     // This pose has a predecessor.
     if (last_pose_indices.count(trajectory) != 0) {
       const int last_pose_index = last_pose_indices[trajectory];
+      constexpr double kUnusedPositionPenalty = 1.;
+      constexpr double kUnusedOrientationPenalty = 1.;
       problem.AddResidualBlock(
           new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
               new SpaCostFunction(Constraint::Pose{
-                  initial_point_cloud_poses[last_pose_index].inverse() *
+                  transform::Embed3D(
-                      initial_point_cloud_poses[j],
+                      initial_point_cloud_poses[last_pose_index].inverse() *
-                  consecutive_pose_change_penalty_matrix})),
+                      initial_point_cloud_poses[j]),
+                  kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
+                                         kUnusedPositionPenalty,
+                                         kUnusedOrientationPenalty)})),
           nullptr /* loss function */, C_point_clouds[last_pose_index].data(),
           C_point_clouds[j].data());
     }

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -36,7 +36,7 @@ namespace sparse_pose_graph {
 // Implements the SPA loop closure method.
 class OptimizationProblem {
  public:
-  using Constraint = mapping::SparsePoseGraph::Constraint2D;
+  using Constraint = mapping::SparsePoseGraph::Constraint;
 
   explicit OptimizationProblem(
       const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&

cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h

@@ -22,8 +22,10 @@
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 #include "cartographer/common/math.h"
+#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
 #include "ceres/jet.h"
 
@@ -33,7 +35,7 @@ namespace sparse_pose_graph {
 
 class SpaCostFunction {
  public:
-  using Constraint = mapping::SparsePoseGraph::Constraint2D;
+  using Constraint = mapping::SparsePoseGraph::Constraint;
 
   explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
 
@@ -62,9 +64,10 @@ class SpaCostFunction {
   static void ComputeScaledError(const Constraint::Pose& pose,
                                  const T* const c_i, const T* const c_j,
                                  T* const e) {
-    std::array<T, 3> e_ij = ComputeUnscaledError(pose.zbar_ij, c_i, c_j);
+    std::array<T, 3> e_ij =
+        ComputeUnscaledError(transform::Project2D(pose.zbar_ij), c_i, c_j);
     (Eigen::Map<Eigen::Matrix<T, 3, 1>>(e)) =
-        pose.sqrt_Lambda_ij.cast<T>() *
+        kalman_filter::Project2D(pose.sqrt_Lambda_ij).cast<T>() *
         Eigen::Map<Eigen::Matrix<T, 3, 1>>(e_ij.data());
   }
 

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -184,14 +184,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     // Unchanged covariance as (submap <- map) is a translation.
     const transform::Rigid3d constraint_transform =
         submap->local_pose().inverse() * pose;
-    constraints_.push_back(Constraint3D{
+    constraints_.push_back(
-        submap_index,
+        Constraint{submap_index,
-        scan_index,
+                   scan_index,
-        {constraint_transform,
+                   {constraint_transform,
-         common::ComputeSpdMatrixSqrtInverse(
+                    common::ComputeSpdMatrixSqrtInverse(
-             covariance, options_.constraint_builder_options()
+                        covariance, options_.constraint_builder_options()
-                             .lower_covariance_eigenvalue_bound())},
+                                        .lower_covariance_eigenvalue_bound())},
-        Constraint3D::INTRA_SUBMAP});
+                   Constraint::INTRA_SUBMAP});
   }
 
   // Determine if this scan should be globally localized.
@@ -393,11 +393,7 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
-std::vector<SparsePoseGraph::Constraint2D> SparsePoseGraph::constraints_2d() {
+std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
-  return {};
-}
-
-std::vector<SparsePoseGraph::Constraint3D> SparsePoseGraph::constraints_3d() {
   return constraints_;
 }
 

cartographer/mapping_3d/sparse_pose_graph.h

@@ -92,8 +92,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_) override;
   std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
       EXCLUDES(mutex_);
-  std::vector<Constraint2D> constraints_2d() override;
+  std::vector<Constraint> constraints() override;
-  std::vector<Constraint3D> constraints_3d() override;
 
  private:
   struct SubmapState {
@@ -179,7 +178,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Current optimization problem.
   sparse_pose_graph::OptimizationProblem optimization_problem_;
   sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
-  std::vector<Constraint3D> constraints_;
+  std::vector<Constraint> constraints_;
   std::vector<transform::Rigid3d> submap_transforms_;  // (map <- submap)
 
   // Submaps get assigned an index and state as soon as they are seen, even

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -55,7 +55,7 @@ namespace sparse_pose_graph {
 // This class is thread-safe.
 class ConstraintBuilder {
  public:
-  using Constraint = mapping::SparsePoseGraph::Constraint3D;
+  using Constraint = mapping::SparsePoseGraph::Constraint;
   using Result = std::vector<Constraint>;
 
   ConstraintBuilder(

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -44,7 +44,7 @@ struct NodeData {
 // Implements the SPA loop closure method.
 class OptimizationProblem {
  public:
-  using Constraint = mapping::SparsePoseGraph::Constraint3D;
+  using Constraint = mapping::SparsePoseGraph::Constraint;
 
   explicit OptimizationProblem(
       const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&

cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h

@@ -33,7 +33,7 @@ namespace sparse_pose_graph {
 
 class SpaCostFunction {
  public:
-  using Constraint = mapping::SparsePoseGraph::Constraint3D;
+  using Constraint = mapping::SparsePoseGraph::Constraint;
 
   explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}