Towards localization in 3D. (#417)

Adds trimmer handling (but no trimming yet) and frozen trajectories
to 3D. Also freezes nodes, not just submaps.

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -24,7 +24,6 @@
 #include <iostream>
 #include <limits>
 #include <memory>
-#include <set>
 #include <sstream>
 #include <string>
 

cartographer/mapping_2d/sparse_pose_graph.h

@@ -22,6 +22,7 @@
 #include <limits>
 #include <map>
 #include <memory>
+#include <set>
 #include <unordered_map>
 #include <vector>
 
@@ -166,7 +167,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   common::Mutex mutex_;
 
   // If it exists, further scans must be added to this queue, and will be
-  // considered later
+  // considered later.
   std::unique_ptr<std::deque<std::function<void()>>> scan_queue_
       GUARDED_BY(mutex_);
 

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -161,12 +161,16 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   }
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
+    const bool frozen = frozen_trajectories.count(trajectory_id);
     // Reserve guarantees that data does not move, so the pointers for Ceres
     // stay valid.
     C_nodes[trajectory_id].reserve(node_data_[trajectory_id].size());
     for (const NodeData& node_data : node_data_[trajectory_id]) {
       C_nodes[trajectory_id].push_back(FromPose(node_data.point_cloud_pose));
       problem.AddParameterBlock(C_nodes[trajectory_id].back().data(), 3);
+      if (frozen) {
+        problem.SetParameterBlockConstant(C_nodes[trajectory_id].back().data());
+      }
     }
   }
 

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -20,6 +20,7 @@
 #include <array>
 #include <deque>
 #include <map>
+#include <set>
 #include <vector>
 
 #include "Eigen/Core"

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -368,6 +368,23 @@ void SparsePoseGraph::WaitForAllComputations() {
   locker.Await([&notification]() { return notification; });
 }
 
+void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
+  common::MutexLocker locker(&mutex_);
+  AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
+    CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0);
+    frozen_trajectories_.insert(trajectory_id);
+  });
+}
+
+void SparsePoseGraph::AddTrimmer(
+    std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
+  common::MutexLocker locker(&mutex_);
+  // C++11 does not allow us to move a unique_ptr into a lambda.
+  mapping::PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
+  AddWorkItem([this, trimmer_ptr]()
+                  REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); });
+}
+
 void SparsePoseGraph::RunFinalOptimization() {
   WaitForAllComputations();
   optimization_problem_.SetMaxNumIterations(
@@ -383,7 +400,7 @@ void SparsePoseGraph::RunOptimization() {
   if (optimization_problem_.submap_data().empty()) {
     return;
   }
-  optimization_problem_.Solve(constraints_);
+  optimization_problem_.Solve(constraints_, frozen_trajectories_);
   common::MutexLocker locker(&mutex_);
 
   const auto& node_data = optimization_problem_.node_data();
@@ -418,6 +435,11 @@ void SparsePoseGraph::RunOptimization() {
       reverse_connected_components_.emplace(trajectory_id, i);
     }
   }
+
+  TrimmingHandle trimming_handle(this);
+  for (auto& trimmer : trimmers_) {
+    trimmer->Trim(&trimming_handle);
+  }
 }
 
 std::vector<std::vector<mapping::TrajectoryNode>>
@@ -513,5 +535,18 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
                       submap->local_pose()};
 }
 
+SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
+    : parent_(parent) {}
+
+int SparsePoseGraph::TrimmingHandle::num_submaps(
+    const int trajectory_id) const {
+  LOG(FATAL) << "Not yet implemented for 3D.";
+}
+
+void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
+    const mapping::SubmapId& submap_id) {
+  LOG(FATAL) << "Not yet implemented for 3D.";
+}
+
 }  // namespace mapping_3d
 }  // namespace cartographer

cartographer/mapping_3d/sparse_pose_graph.h

@@ -31,6 +31,7 @@
 #include "cartographer/common/mutex.h"
 #include "cartographer/common/thread_pool.h"
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/pose_graph_trimmer.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/trajectory_connectivity.h"
 #include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h"
@@ -77,17 +78,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                   const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity);
 
-  void FreezeTrajectory(const int trajectory_id) override {
-    LOG(FATAL) << "Not yet implemented for 3D.";
-  }
-  void AddSubmapFromProto(const int trajectory_id,
+  void FreezeTrajectory(int trajectory_id) override;
+  void AddSubmapFromProto(int trajectory_id,
                           const transform::Rigid3d& initial_pose,
                           const mapping::proto::Submap& submap) override {
     LOG(FATAL) << "Not yet implemented for 3D";
   }
-  void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override {
-    LOG(FATAL) << "Not yet implemented for 3D.";
-  }
+  void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
   int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
@@ -208,6 +205,27 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Current submap transforms used for displaying data.
   std::vector<std::vector<sparse_pose_graph::SubmapData>>
       optimized_submap_transforms_ GUARDED_BY(mutex_);
+
+  // List of all trimmers to consult when optimizations finish.
+  std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_
+      GUARDED_BY(mutex_);
+
+  // Set of all frozen trajectories not being optimized.
+  std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
+
+  // Allows querying and manipulating the pose graph by the 'trimmers_'. The
+  // 'mutex_' of the pose graph is held while this class is used.
+  class TrimmingHandle : public mapping::Trimmable {
+   public:
+    TrimmingHandle(SparsePoseGraph* parent);
+    ~TrimmingHandle() override {}
+
+    int num_submaps(int trajectory_id) const override;
+    void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id) override;
+
+   private:
+    SparsePoseGraph* const parent_;
+  };
 };
 
 }  // namespace mapping_3d

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -112,7 +112,8 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
       max_num_iterations);
 }
 
-void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
+void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
+                                const std::set<int>& frozen_trajectories) {
   if (node_data_.empty()) {
     // Nothing to optimize.
     return;
@@ -137,6 +138,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
   std::vector<std::deque<CeresPose>> C_nodes(node_data_.size());
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
        ++trajectory_id) {
+    const bool frozen = frozen_trajectories.count(trajectory_id);
     for (size_t submap_index = 0;
          submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
       if (trajectory_id == 0 && submap_index == 0) {
@@ -154,16 +156,29 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
             translation_parameterization(),
             common::make_unique<ceres::QuaternionParameterization>(), &problem);
       }
+      if (frozen) {
+        problem.SetParameterBlockConstant(
+            C_submaps[trajectory_id].back().rotation());
+        problem.SetParameterBlockConstant(
+            C_submaps[trajectory_id].back().translation());
+      }
     }
   }
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
+    const bool frozen = frozen_trajectories.count(trajectory_id);
     for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
          ++node_index) {
       C_nodes[trajectory_id].emplace_back(
           node_data_[trajectory_id][node_index].point_cloud_pose,
           translation_parameterization(),
           common::make_unique<ceres::QuaternionParameterization>(), &problem);
+      if (frozen) {
+        problem.SetParameterBlockConstant(
+            C_nodes[trajectory_id].back().rotation());
+        problem.SetParameterBlockConstant(
+            C_nodes[trajectory_id].back().translation());
+      }
     }
   }
 

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -20,6 +20,7 @@
 #include <array>
 #include <deque>
 #include <map>
+#include <set>
 #include <vector>
 
 #include "Eigen/Core"
@@ -69,7 +70,8 @@ class OptimizationProblem {
   void SetMaxNumIterations(int32 max_num_iterations);
 
   // Computes the optimized poses.
-  void Solve(const std::vector<Constraint>& constraints);
+  void Solve(const std::vector<Constraint>& constraints,
+             const std::set<int>& frozen_trajectories);
 
   const std::vector<std::vector<NodeData>>& node_data() const;
   const std::vector<std::vector<SubmapData>>& submap_data() const;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -167,7 +167,8 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
-  optimization_problem_.Solve(constraints);
+  const std::set<int> kFrozen;
+  optimization_problem_.Solve(constraints, kFrozen);
 
   double translation_error_after = 0.;
   double rotation_error_after = 0.;