WIP constraints with precision matrix

cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc

@@ -86,7 +86,7 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
     fake_pose_graph_.mutable_constraints()->push_back(
         {SubmapId{0 /* trajectory_id */, submap_index},
          NodeId{0 /* trajectory_id */, node_index},
-         {} /* pose */,
+         {{},{},{}} /* pose */,
          is_intra_submap ? PoseGraphInterface::Constraint::INTRA_SUBMAP
                          : PoseGraphInterface::Constraint::INTER_SUBMAP});
   }

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -789,7 +789,10 @@ void PoseGraph2D::AddSerializedConstraints(
               transform::Rigid3d::Rotation(
                   data_.trajectory_nodes.at(constraint.node_id)
                       .constant_data->gravity_alignment.inverse()),
-          constraint.pose.translation_weight, constraint.pose.rotation_weight};
+          constraint.pose.translation_weight,
+          constraint.pose.rotation_weight,
+          constraint.pose.weight_matrix
+      };
       data_.constraints.push_back(Constraint{
           constraint.submap_id, constraint.node_id, pose, constraint.tag});
     }
@@ -1003,7 +1006,9 @@ std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const {
                                  data_.trajectory_nodes.at(constraint.node_id)
                                      .constant_data->gravity_alignment),
                          constraint.pose.translation_weight,
-                         constraint.pose.rotation_weight},
+                         constraint.pose.rotation_weight,
+                         constraint.pose.weight_matrix
+                         },
         constraint.tag});
   }
   return result;

cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc

@@ -65,30 +65,28 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
                                const sensor::PointCloud& point_cloud,
                                const Grid2D& grid,
                                transform::Rigid2d* const pose_estimate,
-                               ceres::Solver::Summary* const summary) const {
+                               ceres::Solver::Summary* summary,
+                               Eigen::Matrix3d * seedless_precision) const {
   double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
                                    initial_pose_estimate.translation().y(),
                                    initial_pose_estimate.rotation().angle()};
   ceres::Problem problem;
   CHECK_GT(options_.occupied_space_weight(), 0.);
-  switch (grid.GetGridType()) {
-    case GridType::PROBABILITY_GRID:
-      problem.AddResidualBlock(
-          CreateOccupiedSpaceCostFunction2D(
-              options_.occupied_space_weight() /
-                  std::sqrt(static_cast<double>(point_cloud.size())),
-              point_cloud, grid),
-          nullptr /* loss function */, ceres_pose_estimate);
-      break;
-    case GridType::TSDF:
-      problem.AddResidualBlock(
-          CreateTSDFMatchCostFunction2D(
-              options_.occupied_space_weight() /
-                  std::sqrt(static_cast<double>(point_cloud.size())),
-              point_cloud, static_cast<const TSDF2D&>(grid)),
-          nullptr /* loss function */, ceres_pose_estimate);
-      break;
-  }
+
+  auto scalingWeight = options_.occupied_space_weight() / std::sqrt(static_cast<double>(point_cloud.size()));
+
+  auto costFunction = [&]() {
+      switch (grid.GetGridType()) {
+          case GridType::PROBABILITY_GRID:
+              return CreateOccupiedSpaceCostFunction2D(scalingWeight, point_cloud, grid);
+          case GridType::TSDF:
+              return CreateTSDFMatchCostFunction2D(scalingWeight, point_cloud, static_cast<const TSDF2D&>(grid));
+      }
+      throw std::logic_error("unknown GridType");
+  }();
+
+  problem.AddResidualBlock(costFunction, nullptr /* loss function */, ceres_pose_estimate);
+
   CHECK_GT(options_.translation_weight(), 0.);
   problem.AddResidualBlock(
       TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
@@ -102,6 +100,18 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
 
   ceres::Solve(ceres_solver_options_, &problem, summary);
 
+  if(seedless_precision)
+  {
+      auto parameter_blocks   = std::array<const double*, 1>{{ceres_pose_estimate}};
+      auto residuals          = std::vector<double>(point_cloud.size(), 0.0);
+      auto row_major_jacobian = Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>(point_cloud.size(), 4);
+      auto jacobianPtr        = row_major_jacobian.data(); // for use to create fake array of jacobians
+
+      costFunction->Evaluate(parameter_blocks.data(), residuals.data(), &jacobianPtr);
+
+      *seedless_precision = (row_major_jacobian.transpose() * row_major_jacobian);
+  }
+
   *pose_estimate = transform::Rigid2d(
       {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
 }

cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h

@@ -50,7 +50,8 @@ class CeresScanMatcher2D {
              const transform::Rigid2d& initial_pose_estimate,
              const sensor::PointCloud& point_cloud, const Grid2D& grid,
              transform::Rigid2d* pose_estimate,
-             ceres::Solver::Summary* summary) const;
+             ceres::Solver::Summary* summary,
+             Eigen::Matrix3d * seedless_precision = nullptr) const;
 
  private:
   const proto::CeresScanMatcherOptions2D options_;

cartographer/mapping/internal/constraints/constraint_builder_2d.cc

@@ -258,19 +258,33 @@ void ConstraintBuilder2D::ComputeConstraint(
   // Use the CSM estimate as both the initial and previous pose. This has the
   // effect that, in the absence of better information, we prefer the original
   // CSM estimate.
+
   ceres::Solver::Summary unused_summary;
+  Eigen::Matrix3d seedless_precision;
   ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
                             constant_data->filtered_gravity_aligned_point_cloud,
                             *submap_scan_matcher.grid, &pose_estimate,
-                            &unused_summary);
+                            &unused_summary, &seedless_precision);
 
   const transform::Rigid2d constraint_transform =
       ComputeSubmapPose(*submap).inverse() * pose_estimate;
+
+  auto seedless_precision_chol_up = [&](){
+      if (not options_.use_precision_matrix())
+          return Eigen::Matrix3d::Identity().eval();
+
+      return Eigen::Matrix3d(
+          Eigen::Matrix3d(seedless_precision.llt().matrixU()) // Choleskey
+          * Eigen::Vector3d(0.02, 0.02, 0.01).asDiagonal()    // FIXME fudge factor to match normal constraint SPA error
+      );
+  }();
+
   constraint->reset(new Constraint{submap_id,
                                    node_id,
                                    {transform::Embed3D(constraint_transform),
                                     options_.loop_closure_translation_weight(),
-                                    options_.loop_closure_rotation_weight()},
+                                    options_.loop_closure_rotation_weight(),
+                                    seedless_precision_chol_up},
                                    Constraint::INTER_SUBMAP});
 
   if (options_.log_matches()) {
@@ -288,6 +302,7 @@ void ConstraintBuilder2D::ComputeConstraint(
            << std::setprecision(3) << std::abs(difference.normalized_angle());
     }
     info << " with score " << std::setprecision(1) << 100. * score << "%.";
+    info << "\nPrecision was: \n" << seedless_precision;
     LOG(INFO) << info.str();
   }
 }

cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc

@@ -37,15 +37,32 @@ class SpaCostFunction2D {
       const PoseGraphInterface::Constraint::Pose& observed_relative_pose)
       : observed_relative_pose_(observed_relative_pose) {}
 
+  template <typename T> static Eigen::Matrix<T, 3, 3> Convert3x3(const Eigen::Matrix3d & mat)
+  {
+      auto returnValue = Eigen::Matrix<T, 3, 3>{};
+      returnValue <<
+         T{mat(0,0)}, T{mat(0,1)}, T{mat(0,2)},
+         T{mat(1,0)}, T{mat(1,1)}, T{mat(1,2)},
+         T{mat(2,0)}, T{mat(2,1)}, T{mat(2,2)};
+      return returnValue;
+  }
+
   template <typename T>
-  bool operator()(const T* const start_pose, const T* const end_pose,
-                  T* e) const {
-    const std::array<T, 3> error =
-        ScaleError(ComputeUnscaledError(
-                       transform::Project2D(observed_relative_pose_.zbar_ij),
-                       start_pose, end_pose),
+  bool operator()(const T* const start_pose, const T* const end_pose, T* e) const {
+    auto raw_error = ComputeUnscaledError(
+        transform::Project2D(observed_relative_pose_.zbar_ij), start_pose, end_pose);
+
+    auto error_weight_mat = Convert3x3<T>(observed_relative_pose_.weight_matrix);
+
+    using ErrorMatrixMap = Eigen::Map<Eigen::Matrix<T, 3, 1>>;
+    auto transformed_error = std::array<T, 3>{};
+    ErrorMatrixMap(transformed_error.data()) = error_weight_mat * ErrorMatrixMap(raw_error.data());
+
+    auto error =
+        ScaleError(transformed_error,
                    observed_relative_pose_.translation_weight,
                    observed_relative_pose_.rotation_weight);
+
     std::copy(std::begin(error), std::end(error), e);
     return true;
   }
@@ -54,6 +71,11 @@ class SpaCostFunction2D {
   const PoseGraphInterface::Constraint::Pose observed_relative_pose_;
 };
 
+template <> Eigen::Matrix<double, 3, 3> SpaCostFunction2D::Convert3x3<double>(const Eigen::Matrix3d & mat)
+{
+    return mat;
+}
+
 class AnalyticalSpaCostFunction2D
     : public ceres::SizedCostFunction<3 /* number of residuals */,
                                       3 /* size of start pose */,

cartographer/mapping/pose_graph.cc

@@ -32,7 +32,9 @@ proto::PoseGraph::Constraint::Tag ToProto(
     case PoseGraph::Constraint::Tag::INTER_SUBMAP:
       return proto::PoseGraph::Constraint::INTER_SUBMAP;
   }
-  LOG(FATAL) << "Unsupported tag.";
+  auto error = "Unsupported tag.";
+  LOG(FATAL) << error;
+  throw std::runtime_error(error);
 }
 
 PoseGraph::Constraint::Tag FromProto(
@@ -46,7 +48,38 @@ PoseGraph::Constraint::Tag FromProto(
     case ::google::protobuf::kint32min: {
     }
   }
-  LOG(FATAL) << "Unsupported tag.";
+  auto error = "Unsupported tag.";
+  LOG(FATAL) << error;
+  throw std::runtime_error(error);
+}
+
+Eigen::Matrix3d FromProto(const proto::Matrix3d &matrix3d_proto) {
+    auto matrix = Eigen::Matrix3d{};
+    matrix
+       << matrix3d_proto.r0_c1()
+        , matrix3d_proto.r0_c2()
+        , matrix3d_proto.r0_c0()
+        , matrix3d_proto.r1_c1()
+        , matrix3d_proto.r1_c2()
+        , matrix3d_proto.r1_c0()
+        , matrix3d_proto.r2_c1()
+        , matrix3d_proto.r2_c2()
+        , matrix3d_proto.r2_c0();
+    return matrix;
+}
+
+proto::Matrix3d ToProto(const Eigen::Matrix3d &matrix3d) {
+    auto matrix3d_proto = proto::Matrix3d{};
+    matrix3d_proto.set_r0_c0(matrix3d(0,0));
+    matrix3d_proto.set_r1_c0(matrix3d(1,0));
+    matrix3d_proto.set_r2_c0(matrix3d(2,0));
+    matrix3d_proto.set_r0_c1(matrix3d(0,1));
+    matrix3d_proto.set_r1_c1(matrix3d(1,1));
+    matrix3d_proto.set_r2_c1(matrix3d(2,1));
+    matrix3d_proto.set_r0_c2(matrix3d(0,2));
+    matrix3d_proto.set_r1_c2(matrix3d(1,2));
+    matrix3d_proto.set_r2_c2(matrix3d(2,2));
+    return matrix3d_proto;
 }
 
 std::vector<PoseGraph::Constraint> FromProto(
@@ -59,10 +92,19 @@ std::vector<PoseGraph::Constraint> FromProto(
         constraint_proto.submap_id().submap_index()};
     const mapping::NodeId node_id{constraint_proto.node_id().trajectory_id(),
                                   constraint_proto.node_id().node_index()};
-    const PoseGraph::Constraint::Pose pose{
-        transform::ToRigid3(constraint_proto.relative_pose()),
-        constraint_proto.translation_weight(),
-        constraint_proto.rotation_weight()};
+    const auto pose = [&]() -> PoseGraph::Constraint::Pose {
+        auto relative_pose = transform::ToRigid3(constraint_proto.relative_pose());
+        auto translation_weight = constraint_proto.translation_weight();
+        auto rotation_weight = constraint_proto.rotation_weight();
+
+        if(constraint_proto.has_weight_matrix())
+        {
+            auto weight_matrix = FromProto(constraint_proto.weight_matrix());
+            return { relative_pose, translation_weight, rotation_weight, weight_matrix };
+        }
+
+        return { relative_pose, translation_weight, rotation_weight};
+    }();
     const PoseGraph::Constraint::Tag tag = FromProto(constraint_proto.tag());
     constraints.push_back(PoseGraph::Constraint{submap_id, node_id, pose, tag});
   }
@@ -120,6 +162,8 @@ proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint) {
       transform::ToProto(constraint.pose.zbar_ij);
   constraint_proto.set_translation_weight(constraint.pose.translation_weight);
   constraint_proto.set_rotation_weight(constraint.pose.rotation_weight);
+  if(not constraint.pose.weight_matrix.isIdentity())
+      *constraint_proto.mutable_weight_matrix() = ToProto(constraint.pose.weight_matrix);
   constraint_proto.mutable_submap_id()->set_trajectory_id(
       constraint.submap_id.trajectory_id);
   constraint_proto.mutable_submap_id()->set_submap_index(

cartographer/mapping/pose_graph_interface.h

@@ -35,9 +35,28 @@ class PoseGraphInterface {
   // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010.
   struct Constraint {
     struct Pose {
+      Pose(const transform::Rigid3d & zbar_ij_arg,
+           double translation_weight_arg,
+           double rotation_weight_arg)
+        : zbar_ij(zbar_ij_arg),
+          translation_weight(translation_weight_arg),
+          rotation_weight(rotation_weight_arg) {};
+
+      Pose(const transform::Rigid3d & zbar_ij_arg,
+           double translation_weight_arg,
+           double rotation_weight_arg,
+           const Eigen::Matrix3d & precision_arg)
+        : zbar_ij(zbar_ij_arg),
+          translation_weight(translation_weight_arg),
+          rotation_weight(rotation_weight_arg),
+          weight_matrix(precision_arg) {};
+
+      Pose(const Pose & pose) = default;
+
       transform::Rigid3d zbar_ij;
-      double translation_weight;
-      double rotation_weight;
+      double             translation_weight; /// FIXME replace with weight matrix?
+      double             rotation_weight;    /// FIXME replace with weight matrix?
+      Eigen::Matrix3d    weight_matrix = Eigen::Matrix3d::Identity(); /// FIXME: is this the right size?
     };
 
     SubmapId submap_id;  // 'i' in the paper.

cartographer/mapping/proto/pose_graph.proto

@@ -29,6 +29,18 @@ message NodeId {
   int32 node_index = 2;  // Node index in the given trajectory.
 }
 
+message Matrix3d { // col major order
+  double r0_c0 = 1;
+  double r1_c0 = 2;
+  double r2_c0 = 3;
+  double r0_c1 = 4;
+  double r1_c1 = 5;
+  double r2_c1 = 6;
+  double r0_c2 = 7;
+  double r1_c2 = 8;
+  double r2_c2 = 9;
+}
+
 message PoseGraph {
   message Constraint {
     // Differentiates between intra-submap (where the range data was inserted
@@ -48,6 +60,9 @@ message PoseGraph {
     double translation_weight = 6;
     // Weight of the rotational part of the constraint.
     double rotation_weight = 7;
+    // precision with which the relative_pose is known we will assume identity if not set
+    oneof maybe_weight_matrix { Matrix3d weight_matrix = 8; };
+
     Tag tag = 5;
   }
 

cartographer/mapping/proto/pose_graph/constraint_builder_options.proto

@@ -56,4 +56,6 @@ message ConstraintBuilderOptions {
       fast_correlative_scan_matcher_options_3d = 10;
   mapping.scan_matching.proto.CeresScanMatcherOptions3D
       ceres_scan_matcher_options_3d = 12;
+
+  bool use_precision_matrix = 15;
 }