(02) Cartographer source code analysis without dead ends - (58) 2D backend optimization → PoseGraph2D::AddNode(), PoseGraph2D::AppendNode()

Explanation of a series of articles about slam summary links: the most complete slam in history starts from scratch , for this column to explain (02) Analysis of Cartographer source code without dead ends - the link is as follows:
(02) Analysis of Cartographer source code without dead ends - (00) Catalog_Latest None Dead corner explanation: https://blog.csdn.net/weixin_43013761/article/details/127350885

$certification{\color{blue}{right below the end of the article Center} provides my \color{red} contact information, \color{blue} clicks on my photo to display WX→official certification}$

I. Introduction

Through the previous blog analysis, after explaining some basic classes and structures, let’s look back at PoseGraph2D::AppendNode():

/**
 * @brief 向节点列表中添加一个新的节点, 并保存新生成的submap
 * 
 * @param[in] constant_data 节点数据的指针
 * @param[in] trajectory_id 轨迹id
 * @param[in] insertion_submaps 子地图指针的vector
 * @param[in] optimized_pose 当前节点在global坐标系下的坐标
 * @return NodeId 返回新生成的节点id
 */
NodeId PoseGraph2D::AppendNode(
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps,
    const transform::Rigid3d& optimized_pose) {
    
    
	......
	......
}

Repeat the content of the previous blog here: It should be noted that the submaps stored in insertion_submaps are currently active submaps. Usually, there are only two submaps. If you don’t remember them clearly, you may need to go back and analyze them Subgraph creation and point cloud insertion related code. optimized_pose is the pose of the Robot in the global system.

二、AddTrajectoryIfNeeded(trajectory_id)

$\color{blue}(1)$ PoseGraph2D::AppendNode() first calls the AddTrajectoryIfNeeded(trajectory_id) function. First, if the state of the trajectory corresponding to trajectory_id has not been added to the member variable PoseGraph2D::data_::trajectories_state, a default InternalTrajectoryState object will be created for the trajectory, which will be used to store the state of the trajectory in the future.

$\color{blue}(2)$ Then check the status of the track, the track cannot be in the FINISHED or DELETED state, otherwise an error will be reported. Because the trajectory in these two states cannot call the PoseGraph2D::AppendNode() function to add nodes, naturally the PoseGraph2D::AddTrajectoryIfNeeded() function will not be executed. In addition, the deletion state data_.trajectories_state.at(trajectory_id).deletion_state of the trajectory needs to be marked as normal, that is to say, the trajectory waiting to be deleted or planned to be deleted cannot call the PoseGraph2D::AppendNode() function to add nodes.

$\color{blue}(3)$ Call the data_.trajectory_connectivity_state.Add(trajectory_id) function to add the trajectory, which is essentially to connect with yourself, and add a (trajectory_id, trajectory_id) element to TrajectoryConnectivityState::connected_components_::forest_.

$\color{blue}(4)$ A sampler will be built for each trajectory, that is, an instance object of the FixedRatioSampler type, and then added to the member variable PoseGraph2D::global_localization_samplers_. Note that it will not be added repeatedly.

// 如果轨迹不存在, 则将轨迹添加到连接状态里并添加采样器
void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
    
    
  // 如果不存在就添加map中,调用默认构造函数创建value对象
  data_.trajectories_state[trajectory_id];

  //trajectory_id轨迹不能是完成或者删除状态
  CHECK(data_.trajectories_state.at(trajectory_id).state !=
        TrajectoryState::FINISHED);
  CHECK(data_.trajectories_state.at(trajectory_id).state !=
        TrajectoryState::DELETED);
  //trajectory_id轨迹.deletion_state需要标识为NORMAL
  CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
        InternalTrajectoryState::DeletionState::NORMAL);

  // 将轨迹添加到连接状态里,并与自己做连接
  data_.trajectory_connectivity_state.Add(trajectory_id);

  // Make sure we have a sampler for this trajectory.
  // 为轨迹添加采样器
  if (!global_localization_samplers_[trajectory_id]) {
    
    
    global_localization_samplers_[trajectory_id] =
        absl::make_unique<common::FixedRatioSampler>(
            options_.global_sampling_ratio());
  }
}

3. CanAddWorkItemModifying()

After PoseGraph2D::AppendNode calls the AddTrajectoryIfNeeded() function, it executes the following code:

  // 根据轨迹状态判断是否可以添加修改任务
  if (!CanAddWorkItemModifying(trajectory_id)) {
    
    
    LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
  }

Its main function is to judge whether it is possible to add and modify tasks according to the state of the track, and print if the task cannot be added or modified. The CanAddWorkItemModifying() function is actually relatively simple.

$\color{blue}(1)$ First check whether the trajectory state of trajectory_id has been saved. If there is no record in PoseGraph2D::data_.trajectories_state, it means that it is a new trajectory. Return true, indicating that the trajectory can be added with modification tasks.

$\color{blue}(2)$ For tracks in completed or deleted state (including pending deletion and planned deletion), return false, indicating that the task cannot be added or modified.

$\color{blue}(3)$ All other cases return true, indicating that the track can be added with modification tasks. The code comments of the function are as follows:

// 根据轨迹状态判断是否可以添加任务
bool PoseGraph2D::CanAddWorkItemModifying(int trajectory_id) {
    
    
  auto it = data_.trajectories_state.find(trajectory_id);
  if (it == data_.trajectories_state.end()) {
    
    
    return true;
  }
  if (it->second.state == TrajectoryState::FINISHED) {
    
    
    // TODO(gaschler): Replace all FATAL to WARNING after some testing.
    LOG(FATAL) << "trajectory_id " << trajectory_id
               << " has finished "
                  "but modification is requested, skipping.";
    return false;
  }
  if (it->second.deletion_state !=
      InternalTrajectoryState::DeletionState::NORMAL) {
    
    
    LOG(FATAL) << "trajectory_id " << trajectory_id
               << " has been scheduled for deletion "
                  "but modification is requested, skipping.";
    return false;
  }
  if (it->second.state == TrajectoryState::DELETED) {
    
    
    LOG(FATAL) << "trajectory_id " << trajectory_id
               << " has been deleted "
                  "but modification is requested, skipping.";
    return false;
  }
  return true;
}

From the source code point of view, PoseGraph2D::AppendNode() calls CanAddWorkItemModifying() does not seem to have much effect,

四、data_.trajectory_nodes.Append()

After calling CanAddWorkItemModifying(), execute the following core code:

  // 向节点列表中添加一个新的节点
  const NodeId node_id = data_.trajectory_nodes.Append(
      trajectory_id, TrajectoryNode{
    
    constant_data, optimized_pose});
  // 节点总个数加1
  ++data_.num_trajectory_nodes;

This place is relatively simple, that is, construct a TrajectoryNode instance with constant_data and optimized_pose (optimized global pose), and then store it in data_.trajectory_nodes. Then do the +1 operation count.

Five, the rest of the code

Let's take a look at the code comments first, and then explain in detail:

  // Test if the 'insertion_submap.back()' is one we never saw before.
  // 如果是刚开始的轨迹, 或者insertion_submaps.back()是第一次看到, 就添加新的子图
  if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
              ->data.submap != insertion_submaps.back()) {
    
    
    // We grow 'data_.submap_data' as needed. This code assumes that the first
    // time we see a new submap is as 'insertion_submaps.back()'.

    // 如果insertion_submaps.back()是第一次看到, 也就是新生成的
    // 在data_.submap_data中加入一个空的InternalSubmapData
    const SubmapId submap_id =
        data_.submap_data.Append(trajectory_id, InternalSubmapData());
    
    // 保存后边的地图, 将后边的地图的指针赋值过去
    // 地图是刚生成的, 但是地图会在前端部分通过插入点云数据进行更新, 这里只保存指针
    // tag: 画图说明一下
    data_.submap_data.at(submap_id).submap = insertion_submaps.back();
    LOG(INFO) << "Inserted submap " << submap_id << ".";
    kActiveSubmapsMetric->Increment();
  }

First of all, it is clear that the purpose of this code is to add the submap to PoseGraph2D::data_::submap_data. So the first question is, when do you need to insert subgraphs?

The first point is that it cannot be inserted repeatedly, so this is done in the source code. Insertion will be performed for the following two situations: ① If there is no node data in the track corresponding to the subgraph, then of course the subgraph has not been inserted yet. Insertion is required at this time; ② The last submap of a track in data_.submap_data defaults to be the second submap of insertion_submaps. As analyzed in the submap section above, the maximum capacity of insertion_submaps is two submaps. If data_.submap_data pairs with the last If a submap is not insertion_submaps.back() (the last or second submap), it means that the previous insertion_submaps.back() submap has become the first submap, that is, a new submap is added to insertion_submaps , at this time, this new submap needs to be added to data_.submap_data.

If the above two conditions are met, it means that insertion_submaps.back() needs to be added to data_.submap_data. As can be seen from the above code, first call the data_.submap_data.Append() function to construct a set of (SubmapId, InternalSubmapData) elements and add them to data_.submap_data. SubmapId is constructed based on trajectory_id.

However, it should be noted that the InternalSubmapData in the (SubmapId, InternalSubmapData) element is created by calling the default constructor, which means that there is no real storage or pointing to the submap inside, so the following code is finally executed:

    data_.submap_data.at(submap_id).submap = insertion_submaps.back();

After this code is executed, the last (second) submap of insertion_submaps is added to data_.submap_data.

$\color{red} note$ : insertion_submaps is a shared pointer, indicating that the submap added to data_.submap_data is just a pointer. So when the submap is updated in the front end or other places, the submap corresponding to data_.submap_data will also be updated, because it is essentially the same submap.

6. PoseGraph2D::AddNode()

Through the previous analysis of PoseGraph2D::AppendNode(), we can see that the main purpose of this function is to construct a trajectory node based on the relevant data of scan matching and then add it to the corresponding trajectory, and also record the shared pointer of each subgraph . Now go back to the main calling function PoseGraph2D::AddNode(), and the rest of the code is actually not much, as follows:

  // We have to check this here, because it might have changed by the time we
  // execute the lambda.
  // 获取第一个submap是否是完成状态
  const bool newly_finished_submap =  
      insertion_submaps.front()->insertion_finished();

  // 把计算约束的工作放入workitem中等待执行
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    
    
    return ComputeConstraintsForNode(node_id, insertion_submaps,
                                     newly_finished_submap);
  });

  return node_id;

Get the status (completed or incomplete) of the first subgraph in the front end, and then add a task to the thread pool. Yes, the thread pool is involved next. Here's a brief look at it. A lambda expression is used on it, and its main function is very simple, which is to call the ComputeConstraintsForNode() function. This function is also analyzed later.

It should be noted that the capture list of the lambda expression is captured by =, that is to say, it is an assignment operation, that is, when adding a task, the three parameters node_id, insertion_submaps, and newly_finished_submap will be fixed, and the subsequent thread will execute The task is also the same value. The special one is insertion_submaps, which stores the pointer address and will not change, but the submap it points to may change dynamically.

7. Conclusion

After a series of analysis, the explanation of PoseGraph2D::AddNode() is finally completed. In general: it is to construct a trajectory node based on the relevant scan matching data and the global pose and add it to the corresponding trajectory. At the same time , which is recorded for each subgraph. These data are stored in the member variable PoseGraph2D::data_, whose type is PoseGraphData.

Since the previous question is still unresolved, I have to copy it again here, so as not to forget it later

$\color{red}Question 1:$ global_submap_poses is equivalent to when PoseGraph2D::data_.global_submap_poses_2d is optimized.