# Continued from the previous chapter

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 =
insertion_submaps.front()->insertion_finished();

return ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap);
});
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 =
data_.trajectory_nodes.at(node_id).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) {
CHECK(!insertion_submaps.empty());

// 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.

### OptimizationProblem2D

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
private:
......//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.

```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
trajectory_data_[trajectory_id];
}
```

```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.

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) {
CHECK(!insertion_submaps.empty());

// 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) {
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
data_.trajectory_connectivity_state.Connect(
trajectory_id,
data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
time);
}
//join
trajectory_id, transform::Project2D(
ComputeLocalToGlobalTransform(
data_.global_submap_poses_2d, trajectory_id) *
insertion_submaps->local_pose()));
}
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));

const SubmapId submap_id{trajectory_id, 0};
CHECK(data_.submap_data.at(submap_id).submap == 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 (data_.submap_data.at(last_submap_id).submap ==
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 = submap_data.at(last_submap_id).global_pose;
//Insert the second subgraph into the optimization problem
trajectory_id,
//wT2 = wT1 * 1Tl * lT2
first_submap_pose *
constraints::ComputeSubmapPose(*insertion_submaps).inverse() *
constraints::ComputeSubmapPose(*insertion_submaps));
return {last_submap_id,
SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
}
CHECK(data_.submap_data.at(last_submap_id).submap ==
insertion_submaps.back());
const SubmapId front_submap_id{trajectory_id,
last_submap_id.submap_index - 1};
CHECK(data_.submap_data.at(front_submap_id).submap ==
insertion_submaps.front());
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 {
public:
......//A bunch of class methods
private:
//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 {
public:
......//A bunch of class methods

private:
......//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) {
data_.trajectory_connectivity_state.Connect(
trajectory_id,
data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
time);
}
......//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 =
connected_components_.GetComponent(trajectory_id_a);
//Find all trajectories_ id_ B other trajectories associated
std::vector<int> component_b =
connected_components_.GetComponent(trajectory_id_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,
trajectory_id_b);
}
//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) {
trajectory_ids.push_back(entry.first);
}
}
return trajectory_ids;
}
```

Tags: slam cartographer

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