Remove the 'log_residual_histograms' option. (#166)

This option only existed in 2D. Also other refactoring to make the 2D
sparse pose graph optimization more similar to 3D.

cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc

@@ -38,8 +38,6 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
           "consecutive_scan_rotation_penalty_factor"));
   options.set_log_solver_summary(
       parameter_dictionary->GetBool("log_solver_summary"));
-  options.set_log_residual_histograms(
-      parameter_dictionary->GetBool("log_residual_histograms"));
   *options.mutable_ceres_solver_options() =
       common::CreateCeresSolverOptionsProto(
           parameter_dictionary->GetDictionary("ceres_solver_options").get());

cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto

@@ -36,9 +36,5 @@ message OptimizationProblemOptions {
   // If true, the Ceres solver summary will be logged for every optimization.
   optional bool log_solver_summary = 5;
 
-  // If true, histograms of the final residual values after optimization will be
-  // logged for every optimization.
-  optional bool log_residual_histograms = 10;
-
   optional common.proto.CeresSolverOptions ceres_solver_options = 7;
 }

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -54,6 +54,8 @@ void GlobalTrajectoryBuilder::AddImuData(
     const Eigen::Vector3d& angular_velocity) {
   local_trajectory_builder_.AddImuData(time, linear_acceleration,
                                        angular_velocity);
+  sparse_pose_graph_->AddImuData(local_trajectory_builder_.submaps(), time,
+                                 linear_acceleration, angular_velocity);
 }
 
 void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -127,7 +127,7 @@ void SparsePoseGraph::AddScan(
   }
 
   AddWorkItem([=]() REQUIRES(mutex_) {
-    ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
+    ComputeConstraintsForScan(time, j, matching_submap, insertion_submaps,
                               finished_submap, pose, covariance);
   });
 }
@@ -140,11 +140,22 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
   }
 }
 
+void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
+                                 common::Time time,
+                                 const Eigen::Vector3d& linear_acceleration,
+                                 const Eigen::Vector3d& angular_velocity) {
+  common::MutexLocker locker(&mutex_);
+  AddWorkItem([=]() REQUIRES(mutex_) {
+    optimization_problem_.AddImuData(trajectory, time, linear_acceleration,
+                                     angular_velocity);
+  });
+}
+
 void SparsePoseGraph::ComputeConstraint(const int scan_index,
                                         const int submap_index) {
   const transform::Rigid2d relative_pose =
       submap_transforms_[submap_index].inverse() *
-      point_cloud_poses_[scan_index];
+      optimization_problem_.node_data().at(scan_index).point_cloud_pose;
 
   const mapping::Submaps* const scan_trajectory =
       trajectory_nodes_[scan_index].constant_data->trajectory;
@@ -176,9 +187,10 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
 void SparsePoseGraph::ComputeConstraintsForOldScans(
     const mapping::Submap* submap) {
   const int submap_index = GetSubmapIndex(submap);
-  CHECK_GT(point_cloud_poses_.size(), 0);
+  const auto& node_data = optimization_problem_.node_data();
-  CHECK_LT(point_cloud_poses_.size(), std::numeric_limits<int>::max());
+  CHECK_GT(node_data.size(), 0);
-  const int num_nodes = point_cloud_poses_.size();
+  CHECK_LT(node_data.size(), std::numeric_limits<int>::max());
+  const int num_nodes = node_data.size();
   for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
     if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) {
       ComputeConstraint(scan_index, submap_index);
@@ -187,7 +199,8 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
 }
 
 void SparsePoseGraph::ComputeConstraintsForScan(
-    int scan_index, const mapping::Submap* matching_submap,
+    const common::Time time, const int scan_index,
+    const mapping::Submap* matching_submap,
     std::vector<const mapping::Submap*> insertion_submaps,
     const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
     const kalman_filter::Pose2DCovariance& covariance) {
@@ -196,9 +209,11 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   const transform::Rigid2d optimized_pose =
       submap_transforms_[matching_index] *
       sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
-  CHECK_EQ(scan_index, point_cloud_poses_.size());
+  CHECK_EQ(scan_index, optimization_problem_.node_data().size());
-  initial_point_cloud_poses_.push_back(pose);
+  const mapping::Submaps* const scan_trajectory =
-  point_cloud_poses_.push_back(optimized_pose);
+      trajectory_nodes_[scan_index].constant_data->trajectory;
+  optimization_problem_.AddTrajectoryNode(scan_trajectory, time, pose,
+                                          optimized_pose);
   for (const mapping::Submap* submap : insertion_submaps) {
     const int submap_index = GetSubmapIndex(submap);
     CHECK(!submap_states_[submap_index].finished);
@@ -206,8 +221,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     // Unchanged covariance as (submap <- map) is a translation.
     const transform::Rigid2d constraint_transform =
         sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
-    constexpr double kFakePositionCovariance = 1.;
+    constexpr double kFakePositionCovariance = 1e-6;
-    constexpr double kFakeOrientationCovariance = 1.;
+    constexpr double kFakeOrientationCovariance = 1e-6;
     constraints_.push_back(Constraint{
         submap_index,
         scan_index,
@@ -318,24 +333,15 @@ void SparsePoseGraph::RunFinalOptimization() {
 
 void SparsePoseGraph::RunOptimization() {
   if (!submap_transforms_.empty()) {
-    std::vector<const mapping::Submaps*> trajectories;
+    optimization_problem_.Solve(constraints_, &submap_transforms_);
-    {
-      common::MutexLocker locker(&mutex_);
-      CHECK(!submap_states_.empty());
-      for (const auto& trajectory_node : trajectory_nodes_) {
-        trajectories.push_back(trajectory_node.constant_data->trajectory);
-      }
-    }
-
-    optimization_problem_.Solve(constraints_, trajectories,
-                                initial_point_cloud_poses_, &point_cloud_poses_,
-                                &submap_transforms_);
     common::MutexLocker locker(&mutex_);
     has_new_optimized_poses_ = true;
-    const size_t num_optimized_poses = point_cloud_poses_.size();
+
+    const auto& node_data = optimization_problem_.node_data();
+    const size_t num_optimized_poses = node_data.size();
     for (size_t i = 0; i != num_optimized_poses; ++i) {
       trajectory_nodes_[i].pose =
-          transform::Rigid3d(transform::Embed3D(point_cloud_poses_[i]));
+          transform::Rigid3d(transform::Embed3D(node_data[i].point_cloud_pose));
     }
     // Extrapolate all point cloud poses that were added later.
     std::unordered_map<const mapping::Submaps*, transform::Rigid3d>

cartographer/mapping_2d/sparse_pose_graph.h

@@ -80,6 +80,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                const std::vector<const mapping::Submap*>& insertion_submaps)
       EXCLUDES(mutex_);
 
+  // Adds new IMU data to be used in the optimization.
+  void AddImuData(const mapping::Submaps* trajectory, common::Time time,
+                  const Eigen::Vector3d& linear_acceleration,
+                  const Eigen::Vector3d& angular_velocity);
+
   void RunFinalOptimization() override;
   bool HasNewOptimizedPoses() override;
   mapping::proto::ScanMatchingProgress GetScanMatchingProgress() override;
@@ -127,7 +132,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Adds constraints for a scan, and starts scan matching in the background.
   void ComputeConstraintsForScan(
-      int scan_index, const mapping::Submap* matching_submap,
+      common::Time time, int scan_index, const mapping::Submap* matching_submap,
       std::vector<const mapping::Submap*> insertion_submaps,
       const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
       const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
@@ -183,8 +188,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   sparse_pose_graph::OptimizationProblem optimization_problem_;
   sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
   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)
 
   // Submaps get assigned an index and state as soon as they are seen, even

cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt

@@ -55,8 +55,10 @@ google_library(mapping_2d_sparse_pose_graph_optimization_problem
     common_histogram
     common_math
     common_port
+    common_time
     mapping_2d_sparse_pose_graph_spa_cost_function
     mapping_2d_submaps
+    mapping_3d_imu_integration
     mapping_sparse_pose_graph
     mapping_sparse_pose_graph_proto_optimization_problem_options
     transform_transform

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -228,8 +228,8 @@ void ConstraintBuilder::ComputeConstraint(
 
   const transform::Rigid2d constraint_transform =
       ComputeSubmapPose(*submap).inverse() * pose_estimate;
-  constexpr double kFakePositionCovariance = 1.;
+  constexpr double kFakePositionCovariance = 1e-6;
-  constexpr double kFakeOrientationCovariance = 1.;
+  constexpr double kFakeOrientationCovariance = 1e-6;
   constraint->reset(new Constraint{
       submap_index,
       scan_index,

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -59,6 +59,22 @@ OptimizationProblem::OptimizationProblem(
 
 OptimizationProblem::~OptimizationProblem() {}
 
+void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory,
+                                     const common::Time time,
+                                     const Eigen::Vector3d& linear_acceleration,
+                                     const Eigen::Vector3d& angular_velocity) {
+  imu_data_[trajectory].push_back(
+      mapping_3d::ImuData{time, linear_acceleration, angular_velocity});
+}
+
+void OptimizationProblem::AddTrajectoryNode(
+    const mapping::Submaps* const trajectory, const common::Time time,
+    const transform::Rigid2d& initial_point_cloud_pose,
+    const transform::Rigid2d& point_cloud_pose) {
+  node_data_.push_back(
+      NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose});
+}
+
 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
   options_.mutable_ceres_solver_options()->set_max_num_iterations(
       max_num_iterations);
@@ -66,11 +82,8 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
 
 void OptimizationProblem::Solve(
     const std::vector<Constraint>& constraints,
-    const std::vector<const mapping::Submaps*>& trajectories,
+    std::vector<transform::Rigid2d>* const submap_transforms) {
-    const std::vector<transform::Rigid2d>& initial_point_cloud_poses,
+  if (node_data_.empty()) {
-    std::vector<transform::Rigid2d>* point_cloud_poses,
-    std::vector<transform::Rigid2d>* submap_transforms) {
-  if (point_cloud_poses->empty()) {
     // Nothing to optimize.
     return;
   }
@@ -80,13 +93,13 @@ void OptimizationProblem::Solve(
 
   // Set the starting point.
   std::vector<std::array<double, 3>> C_submaps(submap_transforms->size());
-  std::vector<std::array<double, 3>> C_point_clouds(point_cloud_poses->size());
+  std::vector<std::array<double, 3>> C_point_clouds(node_data_.size());
   for (size_t i = 0; i != submap_transforms->size(); ++i) {
     C_submaps[i] = FromPose((*submap_transforms)[i]);
     problem.AddParameterBlock(C_submaps[i].data(), 3);
   }
-  for (size_t j = 0; j != point_cloud_poses->size(); ++j) {
+  for (size_t j = 0; j != node_data_.size(); ++j) {
-    C_point_clouds[j] = FromPose((*point_cloud_poses)[j]);
+    C_point_clouds[j] = FromPose(node_data_[j].point_cloud_pose);
     problem.AddParameterBlock(C_point_clouds[j].data(), 3);
   }
 
@@ -100,7 +113,7 @@ void OptimizationProblem::Solve(
     CHECK_GE(constraint.i, 0);
     CHECK_LT(constraint.i, submap_transforms->size());
     CHECK_GE(constraint.j, 0);
-    CHECK_LT(constraint.j, point_cloud_poses->size());
+    CHECK_LT(constraint.j, node_data_.size());
     constraints_residual_blocks.emplace_back(
         constraint.tag,
         problem.AddResidualBlock(
@@ -115,22 +128,19 @@ void OptimizationProblem::Solve(
   }
 
   // Add penalties for changes between consecutive scans.
-  CHECK(!point_cloud_poses->empty());
   const Eigen::DiagonalMatrix<double, 3> consecutive_pose_change_penalty_matrix(
       options_.consecutive_scan_translation_penalty_factor(),
       options_.consecutive_scan_translation_penalty_factor(),
       options_.consecutive_scan_rotation_penalty_factor());
-  CHECK_GE(initial_point_cloud_poses.size(), point_cloud_poses->size());
-  CHECK_GE(trajectories.size(), point_cloud_poses->size());
 
-  // The poses in initial_point_cloud_poses and point_cloud_poses are
+  // The poses in 'node_data_' are interleaved from multiple trajectories
-  // interleaved from multiple trajectories (although the points from a given
+  // (although the points from a given trajectory are in time order).
-  // trajectory are in time order). 'last_pose_indices[trajectory]' is the index
+  // 'last_pose_indices[trajectory]' is the index of the most-recent pose on
-  // into 'initial_point_cloud_poses' of the most-recent pose on 'trajectory'.
+  // 'trajectory'.
   std::map<const mapping::Submaps*, int> last_pose_indices;
 
-  for (size_t j = 0; j != point_cloud_poses->size(); ++j) {
+  for (size_t j = 0; j != node_data_.size(); ++j) {
-    const mapping::Submaps* trajectory = trajectories[j];
+    const mapping::Submaps* trajectory = node_data_[j].trajectory;
     // This pose has a predecessor.
     if (last_pose_indices.count(trajectory) != 0) {
       const int last_pose_index = last_pose_indices[trajectory];
@@ -139,9 +149,9 @@ void OptimizationProblem::Solve(
       problem.AddResidualBlock(
           new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
               new SpaCostFunction(Constraint::Pose{
-                  transform::Embed3D(
+                  transform::Embed3D(node_data_[last_pose_index]
-                      initial_point_cloud_poses[last_pose_index].inverse() *
+                                         .initial_point_cloud_pose.inverse() *
-                      initial_point_cloud_poses[j]),
+                                     node_data_[j].initial_point_cloud_pose),
                   kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
                                          kUnusedPositionPenalty,
                                          kUnusedOrientationPenalty)})),
@@ -157,41 +167,6 @@ void OptimizationProblem::Solve(
       common::CreateCeresSolverOptions(options_.ceres_solver_options()),
       &problem, &summary);
 
-  if (options_.log_residual_histograms()) {
-    common::Histogram intra_submap_xy_residuals;
-    common::Histogram intra_submap_theta_residuals;
-    common::Histogram inter_submap_xy_residuals;
-    common::Histogram inter_submap_theta_residuals;
-    for (auto constraint_residual_block : constraints_residual_blocks) {
-      ceres::Problem::EvaluateOptions options;
-      options.apply_loss_function = false;
-      options.residual_blocks = {constraint_residual_block.second};
-      std::vector<double> residuals;
-      problem.Evaluate(options, nullptr, &residuals, nullptr, nullptr);
-      CHECK_EQ(3, residuals.size());
-      switch (constraint_residual_block.first) {
-        case Constraint::INTRA_SUBMAP:
-          intra_submap_xy_residuals.Add(common::Pow2(residuals[0]) +
-                                        common::Pow2(residuals[1]));
-          intra_submap_theta_residuals.Add(common::Pow2(residuals[2]));
-          break;
-        case Constraint::INTER_SUBMAP:
-          inter_submap_xy_residuals.Add(common::Pow2(residuals[0]) +
-                                        common::Pow2(residuals[1]));
-          inter_submap_theta_residuals.Add(common::Pow2(residuals[2]));
-          break;
-      }
-    }
-    LOG(INFO) << "Intra-submap x^2 + y^2 residual histogram:\n"
-              << intra_submap_xy_residuals.ToString(10);
-    LOG(INFO) << "Intra-submap theta^2 residual histogram:\n"
-              << intra_submap_theta_residuals.ToString(10);
-    LOG(INFO) << "Inter-submap x^2 + y^2 residual histogram:\n"
-              << inter_submap_xy_residuals.ToString(10);
-    LOG(INFO) << "Inter-submap theta^2 residual histogram:\n"
-              << inter_submap_theta_residuals.ToString(10);
-  }
-
   if (options_.log_solver_summary()) {
     LOG(INFO) << summary.FullReport();
   }
@@ -200,11 +175,15 @@ void OptimizationProblem::Solve(
   for (size_t i = 0; i != submap_transforms->size(); ++i) {
     (*submap_transforms)[i] = ToPose(C_submaps[i]);
   }
-  for (size_t j = 0; j != point_cloud_poses->size(); ++j) {
+  for (size_t j = 0; j != node_data_.size(); ++j) {
-    (*point_cloud_poses)[j] = ToPose(C_point_clouds[j]);
+    node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]);
   }
 }
 
+const std::vector<NodeData>& OptimizationProblem::node_data() const {
+  return node_data_;
+}
+
 }  // namespace sparse_pose_graph
 }  // namespace mapping_2d
 }  // namespace cartographer

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -24,14 +24,24 @@
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 #include "cartographer/common/port.h"
+#include "cartographer/common/time.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
 #include "cartographer/mapping_2d/submaps.h"
+#include "cartographer/mapping_3d/imu_integration.h"
 
 namespace cartographer {
 namespace mapping_2d {
 namespace sparse_pose_graph {
 
+struct NodeData {
+  // TODO(whess): Keep nodes per trajectory instead.
+  const mapping::Submaps* trajectory;
+  common::Time time;
+  transform::Rigid2d initial_point_cloud_pose;
+  transform::Rigid2d point_cloud_pose;
+};
+
 // Implements the SPA loop closure method.
 class OptimizationProblem {
  public:
@@ -45,19 +55,25 @@ class OptimizationProblem {
   OptimizationProblem(const OptimizationProblem&) = delete;
   OptimizationProblem& operator=(const OptimizationProblem&) = delete;
 
+  void AddImuData(const mapping::Submaps* trajectory, common::Time time,
+                  const Eigen::Vector3d& linear_acceleration,
+                  const Eigen::Vector3d& angular_velocity);
+  void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
+                         const transform::Rigid2d& initial_point_cloud_pose,
+                         const transform::Rigid2d& point_cloud_pose);
+
   void SetMaxNumIterations(int32 max_num_iterations);
 
-  // Computes the optimized poses. The point cloud at 'point_cloud_poses[i]'
+  // Computes the optimized poses.
-  // belongs to 'trajectories[i]'. Within a given trajectory, scans are expected
-  // to be contiguous.
   void Solve(const std::vector<Constraint>& constraints,
-             const std::vector<const mapping::Submaps*>& trajectories,
-             const std::vector<transform::Rigid2d>& initial_point_cloud_poses,
-             std::vector<transform::Rigid2d>* point_cloud_poses,
              std::vector<transform::Rigid2d>* submap_transforms);
 
+  const std::vector<NodeData>& node_data() const;
+
  private:
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
+  std::map<const mapping::Submaps*, std::deque<mapping_3d::ImuData>> imu_data_;
+  std::vector<NodeData> node_data_;
 };
 
 }  // namespace sparse_pose_graph

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -125,7 +125,6 @@ class SparsePoseGraphTest : public ::testing::Test {
               consecutive_scan_translation_penalty_factor = 0.,
               consecutive_scan_rotation_penalty_factor = 0.,
               log_solver_summary = true,
-              log_residual_histograms = true,
               ceres_solver_options = {
                 use_nonmonotonic_steps = false,
                 max_num_iterations = 200,

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -30,7 +30,6 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping_3d/acceleration_cost_function.h"
 #include "cartographer/mapping_3d/ceres_pose.h"
-#include "cartographer/mapping_3d/imu_integration.h"
 #include "cartographer/mapping_3d/rotation_cost_function.h"
 #include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h"
 #include "cartographer/transform/transform.h"

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -47,7 +47,6 @@ class OptimizationProblemTest : public ::testing::Test {
           consecutive_scan_translation_penalty_factor = 1e-2,
           consecutive_scan_rotation_penalty_factor = 1e-2,
           log_solver_summary = true,
-          log_residual_histograms = true,
           ceres_solver_options = {
             use_nonmonotonic_steps = false,
             max_num_iterations = 200,

configuration_files/sparse_pose_graph.lua

@@ -71,7 +71,6 @@ SPARSE_POSE_GRAPH = {
     consecutive_scan_translation_penalty_factor = 1e5,
     consecutive_scan_rotation_penalty_factor = 1e5,
     log_solver_summary = false,
-    log_residual_histograms = false,
     ceres_solver_options = {
       use_nonmonotonic_steps = false,
       max_num_iterations = 50,