PoseGraph位姿圖

mapping2D::PoseGraph類的註釋：

1.繼承關係mapping，mapping2D，mapping3D （以前叫sparse_pose_graph.h，目前名稱被修改了）

類中一個很重要的類OptimizationProblem物件optimization_problem_，該類的註釋是Implements the SPA loop closure method.即實現了SPA方法的閉環。

類的主要成員變數和成員函式如類圖所示，其中重要的方法PoseGraph::RunOptimization()。

pose_graph::OptimizationProblem optimization_problem_;
pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);

接上一篇在PoseGraph::AddNode方法中呼叫了 AddTrajectoryIfNeeded() 和 PoseGraph::ComputeConstraintsForNode()方法。

 1 void PoseGraph::ComputeConstraintsForNode(
 2     const mapping::NodeId& node_id,
 3     std::vector<std::shared_ptr<const Submap>> insertion_submaps,
 4     const bool newly_finished_submap) {
 
 5   const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
 6   const std::vector<mapping::SubmapId> submap_ids = InitializeGlobalSubmapPoses(
 7       node_id.trajectory_id, constant_data->time, insertion_submaps);
 8   CHECK_EQ(submap_ids.size(), insertion_submaps.size());
 9   const mapping::SubmapId matching_id = submap_ids.front();
10   const transform::Rigid2d pose = transform::Project2D(
11       constant_data->local_pose *
12       transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
13   const transform::Rigid2d optimized_pose =
14       optimization_problem_.submap_data().at(matching_id).global_pose *
15       pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
16       pose;
17   optimization_problem_.AddTrajectoryNode(
18       matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
19       constant_data->gravity_alignment);
20   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
21     const mapping::SubmapId submap_id = submap_ids[i];
22     // Even if this was the last node added to 'submap_id', the submap will only
23     // be marked as finished in 'submap_data_' further below.
24     CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
25     submap_data_.at(submap_id).node_ids.emplace(node_id);
26     const transform::Rigid2d constraint_transform =
27         pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose;
28     constraints_.push_back(Constraint{submap_id,
29                                       node_id,
30                                       {transform::Embed3D(constraint_transform),
31                                        options_.matcher_translation_weight(),
32                                        options_.matcher_rotation_weight()},
33                                       Constraint::INTRA_SUBMAP});
34   }
35 
36   for (const auto& submap_id_data : submap_data_) {
37     if (submap_id_data.data.state == SubmapState::kFinished) {
38       CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
39       ComputeConstraint(node_id, submap_id_data.id);
40     }
41   }
42 
43   if (newly_finished_submap) {
44     const mapping::SubmapId finished_submap_id = submap_ids.front();
45     SubmapData& finished_submap_data = submap_data_.at(finished_submap_id);
46     CHECK(finished_submap_data.state == SubmapState::kActive);
47     finished_submap_data.state = SubmapState::kFinished;
48     // We have a new completed submap, so we look into adding constraints for
49     // old nodes.
50     ComputeConstraintsForOldNodes(finished_submap_id);
51   }
52   constraint_builder_.NotifyEndOfNode();
53   ++num_nodes_since_last_loop_closure_;
54   if (options_.optimize_every_n_nodes() > 0 &&
55       num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
56     CHECK(!run_loop_closure_);
57     run_loop_closure_ = true;
58     // If there is a 'work_queue_' already, some other thread will take care.
59     if (work_queue_ == nullptr) {
60       work_queue_ = common::make_unique<std::deque<std::function<void()>>>();
61       HandleWorkQueue();
62     }
63   }
64 }

可以看到對本類中ComputeConstraint、ComputeConstraintsForOldNodes、HandleWorkQueue方法的呼叫。這裡有個pose_graph::ComputeSubmapPose方法在cartographer/mapping_2d/pose_graph/constraint_builder.h檔案中。子圖submap位姿？

transform::Rigid2d ComputeSubmapPose(const Submap& submap)
{
      return transform::Project2D(submap.local_pose());
}

注意HandleWorkQueue()方法中呼叫了RunOptimization方法。

 1 void PoseGraph::HandleWorkQueue() {
 2   constraint_builder_.WhenDone(
 3       [this](const pose_graph::ConstraintBuilder::Result& result) {
 4         {
 5           common::MutexLocker locker(&mutex_);
 6           constraints_.insert(constraints_.end(), result.begin(), result.end());
 7         }
 8         RunOptimization();
 9 
10         common::MutexLocker locker(&mutex_);
11         for (const Constraint& constraint : result) {
12           UpdateTrajectoryConnectivity(constraint);
13         }
14         TrimmingHandle trimming_handle(this);
15         for (auto& trimmer : trimmers_) {
16           trimmer->Trim(&trimming_handle);
17         }
18         trimmers_.erase(
19             std::remove_if(
20                 trimmers_.begin(), trimmers_.end(),
21                 [](std::unique_ptr<mapping::PoseGraphTrimmer>& trimmer) {
22                   return trimmer->IsFinished();
23                 }),
24             trimmers_.end());
25 
26         num_nodes_since_last_loop_closure_ = 0;
27         run_loop_closure_ = false;
28         while (!run_loop_closure_) {
29           if (work_queue_->empty()) {
30             work_queue_.reset();
31             return;
32           }
33           work_queue_->front()();
34           work_queue_->pop_front();
35         }
36         LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
37         // We have to optimize again.
38         HandleWorkQueue();
39       });
40 }

檢視PoseGraph::RunOptimization方法， 

 1 void PoseGraph::RunOptimization()
 2 {
 3   if (optimization_problem_.submap_data().empty())
 4   {
 5     return;
 6   }
 7 
 8   // No other thread is accessing the optimization_problem_, constraints_ and
 9   // frozen_trajectories_ when executing the Solve. Solve is time consuming, so
10   // not taking the mutex before Solve to avoid blocking foreground processing.
11   optimization_problem_.Solve(constraints_, frozen_trajectories_);
12   common::MutexLocker locker(&mutex_);
13 
14   const auto& submap_data = optimization_problem_.submap_data();
15   const auto& node_data = optimization_problem_.node_data();
16   for (const int trajectory_id : node_data.trajectory_ids())
17   {
18     for (const auto& node : node_data.trajectory(trajectory_id)) 
19     {
20       auto& mutable_trajectory_node = trajectory_nodes_.at(node.id);
21       mutable_trajectory_node.global_pose =
22           transform::Embed3D(node.data.pose) *
23           transform::Rigid3d::Rotation(mutable_trajectory_node.constant_data->gravity_alignment);
24     }
25 
26     // Extrapolate all point cloud poses that were not included in the
27     // 'optimization_problem_' yet.
28     const auto local_to_new_global =
29         ComputeLocalToGlobalTransform(submap_data, trajectory_id);
30     const auto local_to_old_global =
31         ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
32     const transform::Rigid3d old_global_to_new_global =
33         local_to_new_global * local_to_old_global.inverse();
34 
35     const mapping::NodeId last_optimized_node_id =
36         std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
37     auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id));
38     for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id); ++node_it) 
39     {
40       auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id);
41       mutable_trajectory_node.global_pose =
42           old_global_to_new_global * mutable_trajectory_node.global_pose;
43     }
44   }
45   global_submap_poses_ = submap_data;
46 }

 檢視Constraint 結構體，該結構體在PoseGraphInterface中定義。

 1   // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
 2   // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS),
 3   // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010.
 4   struct Constraint {
 5     struct Pose {
 6       transform::Rigid3d zbar_ij;
 7       double translation_weight;
 8       double rotation_weight;
 9     };
10 
11     SubmapId submap_id;  // 'i' in the paper.
12     NodeId node_id;      // 'j' in the paper.
13 
14     // Pose of the node 'j' relative to submap 'i'.
15     Pose pose;
16 
17     // Differentiates between intra-submap (where node 'j' was inserted into
18     // submap 'i') and inter-submap constraints (where node 'j' was not inserted
19     // into submap 'i').
20     enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
21   };