Support fixing z during 3D sparse pose adjustment. (#161)

This is in preparation of using the 3D sparse pose graph
optimization for 2D SLAM.

CMakeLists.txt

@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-cmake_minimum_required (VERSION 2.8.7)
+cmake_minimum_required (VERSION 2.8.12)
 
 project(cartographer)
 set(CARTOGRAPHER_MAJOR_VERSION 1)

cartographer/mapping_3d/ceres_pose.cc

@@ -21,13 +21,15 @@ namespace mapping_3d {
 
 CeresPose::CeresPose(
     const transform::Rigid3d& rigid,
+    std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
     std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
     ceres::Problem* problem)
     : translation_({{rigid.translation().x(), rigid.translation().y(),
                      rigid.translation().z()}}),
       rotation_({{rigid.rotation().w(), rigid.rotation().x(),
                   rigid.rotation().y(), rigid.rotation().z()}}) {
-  problem->AddParameterBlock(translation_.data(), 3);
+  problem->AddParameterBlock(translation_.data(), 3,
+                             translation_parametrization.release());
   problem->AddParameterBlock(rotation_.data(), 4,
                              rotation_parametrization.release());
 }

cartographer/mapping_3d/ceres_pose.h

@@ -31,6 +31,7 @@ class CeresPose {
  public:
   CeresPose(
       const transform::Rigid3d& rigid,
+      std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
       std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
       ceres::Problem* problem);
 

cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc

@@ -94,7 +94,7 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose,
                              ceres::Solver::Summary* const summary) {
   ceres::Problem problem;
   CeresPose ceres_pose(
-      initial_pose_estimate,
+      initial_pose_estimate, nullptr /* translation_parameterization */,
       options_.only_optimize_yaw()
           ? std::unique_ptr<ceres::LocalParameterization>(
                 common::make_unique<ceres::AutoDiffLocalParameterization<

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -44,7 +44,8 @@ SparsePoseGraph::SparsePoseGraph(
     common::ThreadPool* thread_pool,
     std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data)
     : options_(options),
-      optimization_problem_(options_.optimization_problem_options()),
+      optimization_problem_(options_.optimization_problem_options(),
+                            sparse_pose_graph::OptimizationProblem::FixZ::kNo),
       constraint_builder_(options_.constraint_builder_options(), thread_pool),
       constant_node_data_(constant_node_data) {}
 

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -72,8 +72,9 @@ struct ConstantYawQuaternionPlus {
 
 OptimizationProblem::OptimizationProblem(
     const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
-        options)
-    : options_(options) {}
+        options,
+    FixZ fix_z)
+    : options_(options), fix_z_(fix_z) {}
 
 OptimizationProblem::~OptimizationProblem() {}
 
@@ -114,13 +115,21 @@ void OptimizationProblem::Solve(
   ceres::Problem::Options problem_options;
   ceres::Problem problem(problem_options);
 
+  const auto translation_parameterization =
+      [this]() -> std::unique_ptr<ceres::LocalParameterization> {
+    return fix_z_ == FixZ::kYes
+               ? common::make_unique<ceres::SubsetParameterization>(
+                     3, std::vector<int>{2})
+               : nullptr;
+  };
+
   // Set the starting point.
   std::deque<CeresPose> C_submaps;
   std::deque<CeresPose> C_point_clouds;
   // Tie the first submap to the origin.
   CHECK(!submap_transforms->empty());
   C_submaps.emplace_back(
-      transform::Rigid3d::Identity(),
+      transform::Rigid3d::Identity(), translation_parameterization(),
       common::make_unique<ceres::AutoDiffLocalParameterization<
           ConstantYawQuaternionPlus, 4, 2>>(),
       &problem);
@@ -128,12 +137,12 @@ void OptimizationProblem::Solve(
 
   for (size_t i = 1; i != submap_transforms->size(); ++i) {
     C_submaps.emplace_back(
-        (*submap_transforms)[i],
+        (*submap_transforms)[i], translation_parameterization(),
         common::make_unique<ceres::QuaternionParameterization>(), &problem);
   }
   for (size_t j = 0; j != node_data_.size(); ++j) {
     C_point_clouds.emplace_back(
-        node_data_[j].point_cloud_pose,
+        node_data_[j].point_cloud_pose, translation_parameterization(),
         common::make_unique<ceres::QuaternionParameterization>(), &problem);
   }
 

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -47,9 +47,12 @@ class OptimizationProblem {
  public:
   using Constraint = mapping::SparsePoseGraph::Constraint;
 
-  explicit OptimizationProblem(
+  enum class FixZ { kYes, kNo };
+
+  OptimizationProblem(
       const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
-          options);
+          options,
+      FixZ fix_z);
   ~OptimizationProblem();
 
   OptimizationProblem(const OptimizationProblem&) = delete;
@@ -72,6 +75,7 @@ class OptimizationProblem {
 
  private:
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
+  FixZ fix_z_;
   std::map<const mapping::Submaps*, std::deque<ImuData>> imu_data_;
   std::vector<NodeData> node_data_;
   double gravity_constant_ = 9.8;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -34,7 +34,8 @@ namespace {
 class OptimizationProblemTest : public ::testing::Test {
  protected:
   OptimizationProblemTest()
-      : optimization_problem_(CreateOptions()), rng_(45387) {}
+      : optimization_problem_(CreateOptions(), OptimizationProblem::FixZ::kNo),
+        rng_(45387) {}
 
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions
   CreateOptions() {

cartographer/sensor/ordered_multi_queue.cc

@@ -48,7 +48,7 @@ OrderedMultiQueue::~OrderedMultiQueue() {
 
 void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
   CHECK_EQ(queues_.count(queue_key), 0);
-  queues_[queue_key].callback = callback;
+  queues_[queue_key].callback = std::move(callback);
 }
 
 void OrderedMultiQueue::MarkQueueAsFinished(const QueueKey& queue_key) {