cartographer_ learn_ 18optimization problem 2D introduction and management of connection relationship between tracks

Continued from the previous chapter

Last We discussed how the backend stores nodes and subgraphs, and what is stored in the stored procedures of nodes and subgraphs. In the storage of the subgraph, a pointer to the submap and a set are stored to record the nodes associated with the subgraph. The nodes store the Data structure and the pose of the nodes in the global. At the same time, we also learned the cartographer thread pool, which mainly has two points. One point is how to ensure thread blocking when there are no tasks in the task queue. The author believes that this is realized through condition variables. How to keep the logical order of each task, rather than just relying on the time sequence of task addition to execute the task. The implementation of cartographer is that each task has its own dependent task. Only after the dependent task is executed will it execute its own task.
Let's come back from the thread pool and continue to learn the rest of the AddNode function.

Subgraph join optimization problem (optimization problem 2D)

Look at the code for the rest of AddNode:

NodeId PoseGraph2D::AddNode(
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
......//Calculate the pose of nodes in global
......//Create a node storage node to store subgraphs
//See if the old submap has finished inserting
  const bool newly_finished_submap =
// Add the work of computing constraints to the thread pool
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    return ComputeConstraintsForNode(node_id, insertion_submaps,
  return node_id;

You can see that the task still given to the thread pool is a task called ComputeConstraintsForNode. From the name, it should be the task of calculating node constraints. Let's go in and have a look.

WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap) {
  std::vector<SubmapId> submap_ids;
  std::vector<SubmapId> finished_submap_ids;
  std::set<NodeId> newly_finished_submap_node_ids;
    absl::MutexLock locker(&mutex_);
    // Get node data
    const auto& constant_data =;
    // If the submap does not join the optimization problem, join and return insertion_ id of submap in submaps
    submap_ids = InitializeGlobalSubmapPoses(
        node_id.trajectory_id, constant_data->time, insertion_submaps);
   ......//A bunch of other operations

This function is nothing special at first. Until initializeglobalsubmappositions, let's go in and have a look:

std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
    const int trajectory_id, const common::Time time,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {

  // Get all submaps stored in the optimization problem, submap_ The data type is MapById
  const auto& submap_data = optimization_problem_->submap_data();
  ......//A bunch of other operations

This function encountered an unfamiliar class optimization_problem_, Its type is OptimizationProblem2D. From its name, it must be an optimization class. Let's learn about this class first.


This class is in cartographer / cartographer / mapping / internal / optimization / optimization_ In 2D. H, the above code:

class OptimizationProblem2D
    : public OptimizationProblemInterface<NodeSpec2D, SubmapSpec2D,
                                          transform::Rigid2d> {
......//A bunch of class methods
......//A bunch of class methods
  optimization::proto::OptimizationProblemOptions options_; // Configuration item
  MapById<NodeId, NodeSpec2D> node_data_;                   // Various poses of nodes, including those in local coordinate system and global coordinate system
  MapById<SubmapId, SubmapSpec2D> submap_data_;             // submap's posture under global
  std::map<std::string, transform::Rigid3d> landmark_data_; // landmark data
  sensor::MapByTime<sensor::ImuData> empty_imu_data_;       //imu data
  sensor::MapByTime<sensor::OdometryData> odometry_data_;   // Odometer data
  sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_; // gps data
  std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;   // Trajectory data

Let's first look at the class members of this class and find that they are a pile of MapById, map and MapByTime. Although we haven't discussed this MapByTime before, the author looks a little and finds that it is similar to MapById. We must be able to discuss it later. Here we think so first. It is a variant of MapById. These structures are used to store data, presumably for optimization problems. Let's see how those data are added first. Here we list a few, others are roughly similar.
Addition of Subgraph:

void OptimizationProblem2D::AddSubmap(
    const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
  submap_data_.Append(trajectory_id, SubmapSpec2D{global_submap_pose});

The Append method used here was introduced in Section 2 of the previous article. Note that only the pose of the subgraph is stored here. Interestingly, the two-dimensional mapping optimization does not seem to use imu data, so the function adding imu data here is empty.
Node joining:

void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
                                              const NodeSpec2D& node_data) {
  node_data_.Append(trajectory_id, node_data);
  //If the data of this track has not been stored, apply for space for this track in the track data

Addition of odometer data:

void OptimizationProblem2D::AddOdometryData(
    const int trajectory_id, const sensor::OdometryData& odometry_data) {
  odometry_data_.Append(trajectory_id, odometry_data);

This introduction to optimization problem2d comes first. This class is very important. We must have a chance to continue to learn it in the future.

Addition of subgraphs

Let's go back to the initializeglobalsubmappositions function and see the rest of it:

td::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
    const int trajectory_id, const common::Time time,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {

  // Get all submaps stored in the optimization problem, submap_ The data type is MapById
  const auto& submap_data = optimization_problem_->submap_data();
  //There is only one case for the inserted submap, that is, it has just started
  if (insertion_submaps.size() == 1) {
    //Judge whether the subgraph has been added. If not, add it
    if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
      // If the initial pose is not set, it is 0, and if it is set, it is 1
      if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
        //We can skip here and discuss it in the next section
          trajectory_id, transform::Project2D(
                                 data_.global_submap_poses_2d, trajectory_id) *
    CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));

    const SubmapId submap_id{trajectory_id, 0};
    CHECK( == insertion_submaps.front());
    return {submap_id};
  //The inserted subgraph has two cases
  CHECK_EQ(2, insertion_submaps.size());

  // Endoftractory has been introduced, which is the last data of the track
  const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
  CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
  // prev has also been introduced. In combination with the previous sentence, it is to obtain the last data of the track
  const SubmapId last_submap_id = std::prev(end_it)->id;
  // If it is equal to insertion_ The first element of submaps describes the insertion_ The second element of submaps has not been added to optimization_problem_ in
  if ( ==
      insertion_submaps.front()) {
    // In this case, 'last_submap_id' is the ID of
    // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
    //Take the pose of the first subgraph in global
    const auto& first_submap_pose =;
    //Insert the second subgraph into the optimization problem
        //wT2 = wT1 * 1Tl * lT2
        first_submap_pose *
            constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
    return {last_submap_id,
            SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
  CHECK( ==
  const SubmapId front_submap_id{trajectory_id,
                                 last_submap_id.submap_index - 1};
  CHECK( ==
  return {front_submap_id, last_submap_id};

This function can be divided into two cases. One is when the slam process has just started and there is only one inserted subgraph and two inserted subgraphs. The general process is written in the notes. When there are two subgraphs, how to find the pose of the second subgraph in global? The method is to first find the pose of the first sub graph in global, and then obtain the pose of the second sub graph through the relationship between the first sub graph and the second sub graph (transformation in local coordinate system). At the same time, pay attention to the returned results.

Management of connection relationship between tracks

Let's look back at the thing we skipped above - trajectory_connectivity_state, this is a new class. We saw it in Article 16, but we didn't know what it was at that time. Let's see what this is now.

class TrajectoryConnectivityState {
 ......//A bunch of class methods
//Reserved characters in c + + when mutable
  mutable ConnectedComponents connected_components_;
//Record the connection relationship between tracks, that is, the time when these connections are established
  std::map<std::pair<int, int>, common::Time> last_connection_time_map_;

First look at the class members of this class. One is an unprecedented type. Here we introduce mutable, which marks the scalar. The marked variable can be changed in the class function of const. The second is a map, which records the relationship between tracks. We are going to take a look at this unfamiliar class ConnectedComponents

class ConnectedComponents {
......//A bunch of class methods

......//A bunch of class methods

  absl::Mutex lock_;
  // Tracks transitive connectivity using a disjoint set forest, i.e. each
  // entry points towards the representative for the given trajectory.
  std::map<int, int> forest_ GUARDED_BY(lock_);
  // Tracks the number of direct connections between a pair of trajectories.
  std::map<std::pair<int, int>, int> connection_map_ GUARDED_BY(lock_);

First look at the two class members of this class, the two map s. The first is to record which tracks are connected. At the same time, it also records the connection between each track and itself. For example, there are 1, 2 and 3 tracks, of which 1 and 2 are connected and 1 and 3 are connected. Then forest_ The status in is shown as follows:

The second map records the number of times the two tracks are connected. Because the study is not deep enough, the author can not fully grasp the role of this class, only until part of the role of this class. Therefore, the author may not be complete enough to introduce the role of this class here. Like the chestnuts we held above, there are three tracks 1, 2 and 3. 1 and 2 are connected, and 1 and 3 are connected. Is there any connection between 2 and 3? Yes, so only relying on the TrajectoryConnectivityState class, we can't know the exact relationship between tracks. ConnectedComponents class is to solve this problem. We will call this connection indirect. Since the grasp of these two classes is not comprehensive enough, we introduce their class functions one by one, and gradually deepen our understanding.
Go back to InitializeGlobalSubmapPoses and see the code we skipped:

td::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
    const int trajectory_id, const common::Time time,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {

.......//A bunch of other operations
if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
        //Add a link between two tracks
 ......//A bunch of other operations

Take a look at the Connect function:

void TrajectoryConnectivityState::Connect(const int trajectory_id_a,
                                          const int trajectory_id_b,
                                          const common::Time time) {
  //Use the ConnectedComponents class to check whether the two tracks are connected before. If so, add them directly
  if (TransitivelyConnected(trajectory_id_a, trajectory_id_b)) {
    // The trajectories are transitively connected, i.e. they belong to the same
    // connected component. In this case we only update the last connection time
    // of those two trajectories.
    auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
    if (last_connection_time_map_[sorted_pair] < time) {
      last_connection_time_map_[sorted_pair] = time;
  } else {
    // The connection between these two trajectories is about to join to
    // connected components. Here we update all bipartite trajectory pairs for
    // the two connected components with the connection time. This is to quickly
    // change to a more efficient loop closure search (by constraining the
    // search window) when connected components are joined.
    //If there is no connection, add all indirect connections in addition to the current connection
    std::vector<int> component_a =
    //Find all trajectories_ id_ B other trajectories associated
    std::vector<int> component_b =
    for (const auto id_a : component_a) {
      for (const auto id_b : component_b) {
        auto id_pair = std::minmax(id_a, id_b);
        last_connection_time_map_[id_pair] = time;
  connected_components_.Connect(trajectory_id_a, trajectory_id_b);

Let's first see how to judge whether there is a connection between the two trajectories

bool TrajectoryConnectivityState::TransitivelyConnected(
    const int trajectory_id_a, const int trajectory_id_b) const {
  return connected_components_.TransitivelyConnected(trajectory_id_a,
//Call function 1
bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a,
                                                const int trajectory_id_b) {
  if (trajectory_id_a == trajectory_id_b) {
    return true;

  absl::MutexLock locker(&lock_);

  if (forest_.count(trajectory_id_a) == 0 ||
      forest_.count(trajectory_id_b) == 0) {
    return false;
  return FindSet(trajectory_id_a) == FindSet(trajectory_id_b);
//Call function 2
int ConnectedComponents::FindSet(const int trajectory_id) {
  auto it = forest_.find(trajectory_id);
  CHECK(it != forest_.end());
  if (it->first != it->second) {
    // Path compression for efficiency.
    it->second = FindSet(it->second);
  return it->second;

The process of calling function 2 is a bit convoluted. Let's use the previous chestnut to explain the process of calling function 2. For example, the two parameters of calling function 1 are 2 and 3. Then this function will call FindSet twice, namely FindSet (2) and FindSet (3). Here we explain FindSet (2), another similar one. Here we track the changes of iterator it, as shown in the following figure:

So its return is 1. The same is true for FindSet (3). After we understand the implementation process of the FindSet function, let's see how to find all other tracks related to a track? That is, the GetComponent function. The author is lazy here. Post the code and the readers can understand it for themselves...

std::vector<int> ConnectedComponents::GetComponent(const int trajectory_id) {
  absl::MutexLock locker(&lock_);
  const int set_id = FindSet(trajectory_id);
  std::vector<int> trajectory_ids;
  for (const auto& entry : forest_) {
    if (FindSet(entry.first) == set_id) {
  return trajectory_ids;

Tags: slam cartographer

Posted on Sat, 04 Sep 2021 21:35:20 -0400 by drewbee