Improvement of 2D/3D consistency and cleanup. (#122)

cartographer/mapping_2d/CMakeLists.txt

@@ -118,6 +118,7 @@ google_library(mapping_2d_sparse_pose_graph
     common_fixed_ratio_sampler
     common_make_unique
     common_math
+    common_mutex
     common_thread_pool
     common_time
     kalman_filter_pose_tracker
@@ -131,6 +132,7 @@ google_library(mapping_2d_sparse_pose_graph
     sensor_compressed_point_cloud
     sensor_point_cloud
     sensor_voxel_filter
+    transform_rigid_transform
     transform_transform
 )
 

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -30,7 +30,6 @@
 #include "Eigen/Eigenvalues"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
-#include "cartographer/common/time.h"
 #include "cartographer/mapping/proto/scan_matching_progress.pb.h"
 #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
 #include "cartographer/sensor/compressed_point_cloud.h"
@@ -101,7 +100,7 @@ void SparsePoseGraph::AddScan(
   constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{
       time, laser_fan_in_pose,
       Compress(sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}, {}}), submaps,
-      transform::Rigid3d(tracking_to_pose)});
+      tracking_to_pose});
   trajectory_nodes_.push_back(mapping::TrajectoryNode{
       &constant_node_data_->back(), optimized_pose,
   });
@@ -126,10 +125,19 @@ void SparsePoseGraph::AddScan(
         common::make_unique<common::FixedRatioSampler>(
             options_.global_sampling_ratio());
   }
-  AddWorkItem(
+
-      std::bind(std::mem_fn(&SparsePoseGraph::ComputeConstraintsForScan), this,
+  AddWorkItem([=]() REQUIRES(mutex_) {
-                j, submaps, matching_submap, insertion_submaps, finished_submap,
+    ComputeConstraintsForScan(j, submaps, matching_submap, insertion_submaps,
-                pose, covariance));
+                              finished_submap, pose, covariance);
+  });
+}
+
+void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
+  if (scan_queue_ == nullptr) {
+    work_item();
+  } else {
+    scan_queue_->push_back(work_item);
+  }
 }
 
 void SparsePoseGraph::ComputeConstraintsForOldScans(
@@ -301,14 +309,6 @@ void SparsePoseGraph::WaitForAllComputations() {
   locker.Await([&notification]() { return notification; });
 }
 
-void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
-  if (scan_queue_ == nullptr) {
-    work_item();
-  } else {
-    scan_queue_->push_back(work_item);
-  }
-}
-
 void SparsePoseGraph::RunFinalOptimization() {
   WaitForAllComputations();
   optimization_problem_.SetMaxNumIterations(

cartographer/mapping_2d/sparse_pose_graph.h

@@ -37,6 +37,7 @@
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 #include "cartographer/common/fixed_ratio_sampler.h"
+#include "cartographer/common/mutex.h"
 #include "cartographer/common/thread_pool.h"
 #include "cartographer/common/time.h"
 #include "cartographer/kalman_filter/pose_tracker.h"
@@ -47,6 +48,7 @@
 #include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h"
 #include "cartographer/mapping_2d/submaps.h"
 #include "cartographer/sensor/point_cloud.h"
+#include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 
 namespace cartographer {
@@ -81,7 +83,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void RunFinalOptimization() override;
   bool HasNewOptimizedPoses() override;
   mapping::proto::ScanMatchingProgress GetScanMatchingProgress() override;
-
   std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories()
       override;
   std::vector<transform::Rigid3d> GetSubmapTransforms(
@@ -111,6 +112,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
     const mapping::Submaps* trajectory = nullptr;
   };
 
+  // Handles a new work item.
+  void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
+
   int GetSubmapIndex(const mapping::Submap* submap) const REQUIRES(mutex_) {
     const auto iterator = submap_indices_.find(submap);
     CHECK(iterator != submap_indices_.end());
@@ -145,9 +149,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // optimization being run at a time.
   void RunOptimization() EXCLUDES(mutex_);
 
-  // Handles a new 'work_item'.
-  void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
-
   // Adds extrapolated transforms, so that there are transforms for all submaps.
   std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
       const std::vector<transform::Rigid2d>& submap_transforms,

cartographer/mapping_3d/CMakeLists.txt

@@ -244,6 +244,7 @@ google_library(mapping_3d_sparse_pose_graph
     mapping_sparse_pose_graph
     mapping_sparse_pose_graph_proto_constraint_builder_options
     mapping_trajectory_connectivity
+    sensor_compressed_point_cloud
     sensor_point_cloud
     sensor_voxel_filter
     transform_rigid_transform

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -30,9 +30,9 @@
 #include "Eigen/Eigenvalues"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
-#include "cartographer/common/mutex.h"
 #include "cartographer/mapping/proto/scan_matching_progress.pb.h"
 #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
+#include "cartographer/sensor/compressed_point_cloud.h"
 #include "cartographer/sensor/voxel_filter.h"
 #include "glog/logging.h"
 
@@ -176,7 +176,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
       submap_transforms_[matching_index] *
       matching_submap->local_pose().inverse() * pose;
   CHECK_EQ(scan_index, optimization_problem_.node_data().size());
-  optimization_problem_.AddTrajectoryNode(time, pose, optimized_pose);
+  optimization_problem_.AddTrajectoryNode(time, optimized_pose);
   for (const Submap* submap : insertion_submaps) {
     const int submap_index = GetSubmapIndex(submap);
     CHECK(!submap_states_[submap_index].finished);
@@ -247,7 +247,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     run_loop_closure_ = true;
     // If there is a 'scan_queue_' already, some other thread will take care.
     if (scan_queue_ == nullptr) {
-      scan_queue_.reset(new std::deque<std::function<void()>>);
+      scan_queue_ = common::make_unique<std::deque<std::function<void()>>>();
       HandleScanQueue();
     }
   }
@@ -338,8 +338,7 @@ void SparsePoseGraph::RunOptimization() {
     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 =
+      trajectory_nodes_[i].pose = node_data[i].point_cloud_pose;
-          transform::Rigid3d(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_3d/sparse_pose_graph.h

@@ -41,6 +41,7 @@
 #include "cartographer/common/thread_pool.h"
 #include "cartographer/common/time.h"
 #include "cartographer/kalman_filter/pose_tracker.h"
+#include "cartographer/mapping/proto/scan_matching_progress.pb.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/trajectory_connectivity.h"
 #include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h"

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -84,10 +84,8 @@ void OptimizationProblem::AddImuData(common::Time time,
 }
 
 void OptimizationProblem::AddTrajectoryNode(
-    common::Time time, const transform::Rigid3d& initial_point_cloud_pose,
+    common::Time time, const transform::Rigid3d& point_cloud_pose) {
-    const transform::Rigid3d& point_cloud_pose) {
+  node_data_.push_back(NodeData{time, point_cloud_pose});
-  node_data_.push_back(
-      NodeData{time, initial_point_cloud_pose, point_cloud_pose});
 }
 
 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -37,7 +37,6 @@ namespace sparse_pose_graph {
 
 struct NodeData {
   common::Time time;
-  transform::Rigid3d initial_point_cloud_pose;
   transform::Rigid3d point_cloud_pose;
 };
 
@@ -57,7 +56,6 @@ class OptimizationProblem {
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity);
   void AddTrajectoryNode(common::Time time,
-                         const transform::Rigid3d& initial_point_cloud_pose,
                          const transform::Rigid3d& point_cloud_pose);
 
   void SetMaxNumIterations(int32 max_num_iterations);

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -126,7 +126,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
         AddNoise(node.ground_truth_pose, node.noise);
     optimization_problem_.AddImuData(now, Eigen::Vector3d::UnitZ() * 9.81,
                                      Eigen::Vector3d::Zero());
-    optimization_problem_.AddTrajectoryNode(now, pose, pose);
+    optimization_problem_.AddTrajectoryNode(now, pose);
     now += common::FromSeconds(0.01);
   }
 
@@ -153,29 +153,33 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
 
   std::vector<transform::Rigid3d> submap_transforms = {
       kSubmap0Transform, kSubmap0Transform, kSubmap2Transform};
-  optimization_problem_.Solve(constraints, kSubmap0Transform, trajectories,
-                              &submap_transforms);
 
   double translation_error_before = 0.;
-  double translation_error_after = 0.;
   double rotation_error_before = 0.;
-  double rotation_error_after = 0.;
   const auto& node_data = optimization_problem_.node_data();
   for (int j = 0; j != kNumNodes; ++j) {
-    translation_error_before +=
+    translation_error_before += (test_data[j].ground_truth_pose.translation() -
-        (test_data[j].ground_truth_pose.translation() -
-         node_data[j].initial_point_cloud_pose.translation())
-            .norm();
-    translation_error_after += (test_data[j].ground_truth_pose.translation() -
                                  node_data[j].point_cloud_pose.translation())
                                     .norm();
     rotation_error_before +=
         transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
-                            node_data[j].initial_point_cloud_pose);
+                            node_data[j].point_cloud_pose);
+  }
+
+  optimization_problem_.Solve(constraints, kSubmap0Transform, trajectories,
+                              &submap_transforms);
+
+  double translation_error_after = 0.;
+  double rotation_error_after = 0.;
+  for (int j = 0; j != kNumNodes; ++j) {
+    translation_error_after += (test_data[j].ground_truth_pose.translation() -
+                                node_data[j].point_cloud_pose.translation())
+                                   .norm();
     rotation_error_after +=
         transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
                             node_data[j].point_cloud_pose);
   }
+
   EXPECT_GT(0.8 * translation_error_before, translation_error_after);
   EXPECT_GT(0.8 * rotation_error_before, rotation_error_after);
 }