Explain the summary link of a series of articles about slam: the most complete slam in history starts from scratch, and explain (02) Cartographer source code without dead ends analysis for this column – the link is as follows:
(02) Analysis of Cartographer source code without dead ends – (00) Directory_ Latest explanation without dead ends: https://blog.csdn.net/weixin_43013761/article/details/127350885
At the end of the article, the center provides my own
Contact information,
Click on my photo to display
W
x
→
Official certification
{\color{blue}{The center at the bottom of the article} provides my \color{red} contact information, \color{blue} clicks on my photo to display WX→official certification}
At the end of the article, the center provides my contact information, click on my photo to display WX→official certification
1. Foreword
In the previous blog, I analyzed how to load the .pbstream file. The following is the code in node_main.cc:
// Initialize the Node class, pass the topic of ROS to SLAM, that is, MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
// If the pbstream file is loaded, execute this function
if (!FLAGS_load_state_filename.empty()) {<!-- -->
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
// start track with default topic
if (FLAGS_start_trajectory_with_default_topics) {<!-- -->
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
After the analysis of node.LoadState() is completed, it is the analysis of node.StartTrajectoryWithDefaultTopics(trajectory_options). In fact, this function has been explained before, but the form of explanation is different this time. It is a pure positioning mode. First of all, the input configuration is different. For example, the command line I executed is as follows:
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag
ros launch cartographer_ros offline_backpack_2d.launch bag_filenames:=${<!-- -->HOME}/Downloads/b2-2016-04-05-14-44-52.bag
ros launch cartographer_ros demo_backpack_2d_localization. launch \
load_state_filename:=${<!-- -->HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \
bag_filename:=${<!-- -->HOME}/Downloads/b2-2016-04-27-12-31-41.bag
Among them, demo_backpack_2d_localization.launch has the following contents:
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d_localization.lua
-load_state_filename $(arg load_state_filename)"
output="screen">
<remap from="echoes" to="horizontal_laser_2d" />
</node>
It can be seen that the configuration file it uses is backpack_2d_localization.lua, which contains the main configuration parameters of the pure positioning mode.
2. Add track
Most of the content has been analyzed before, so I won’t repeat it here. Only the functions related to the pure positioning model are analyzed. First, call:
// Use the default topic name to start a track, that is, start slam
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions & amp; options) {<!-- -->
absl::MutexLock lock( & amp; mutex_);
// Check TrajectoryOptions for 2d or 3d trajectory configuration information
CHECK(ValidateTrajectoryOptions(options));
// add a track
AddTrajectory(options);
}
Add a trajectory according to the configuration parameters, and the Node::AddTrajectory() function on it is quite important, as shown below:
/**
* @brief add a new track
*
* @param[in] options parameter configuration of track
* @return int The id of the newly generated track
*/
int Node::AddTrajectory(const TrajectoryOptions & options) {<!-- -->
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options);
// Call AddTrajectory of map_builder_bridge to add a trajectory
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
// Add a new pose estimator
AddExtrapolator(trajectory_id, options);
// Generate a new sensor data sampler
AddSensorSamplers(trajectory_id, options);
// Subscribe topic and register callback function
LaunchSubscribers(options, trajectory_id);
// Created a timer that executes once in 3s, since oneshot=true, it only executes once
// Check whether the set topic name exists in ros, and report an error if it does not exist
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
& amp;Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
// Save the topic name to check whether the topic name is repeated when creating a new track later
for (const auto & sensor_id : expected_sensor_ids) {<!-- -->
subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;
}
It first adds a trajectory through the interface map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); according to the configuration parameters, obtains the id of the trajectory, and binds a pose inferencer AddExtrapolator and sampling to the trajectory. One thing to note here is that the trajectory loaded from .bpstream starts from 0 by default. If .bpstream only stores one trajectory, then the trajectory_id here is 1. The trajectory id can be directly viewed through the visualization of ROS.
3. Configuration files and parameters
The content of the src/cartographer_ros/cartographer_ros/configuration_files/backpack_2d_localization.lua file is as follows:
include "backpack_2d.lua"
TRAJECTORY_BUILDER.pure_localization_trimmer = {<!-- -->
max_submaps_to_keep = 3,
}
POSE_GRAPH. optimize_every_n_nodes = 20
return options
You can see the configuration on it, max_submaps_to_keep means that the maximum number of submaps it can store is 3, and POSE_GRAPH.optimize_every_n_nodes means that the backend optimization will be performed every time 20 nodes are added. It contains backpack_2d.lua which in turn contains trajectory_builder.lua which in turn contains trajectory_builder_2d.lua, trajectory_builder_3d.lua. In this way, all the required configuration files are included. In general, there seems to be no big difference between the pure positioning mode and the mapping positioning mode. The main core difference is max_submaps_to_keep = 3. Search for this parameter in the source code, and you can see the following code:
// Check whether it is a pure positioning mode, support 2 kinds of pure positioning parameter names, the old parameters have been deprecated, a warning will be reported but the program will not terminate
void MaybeAddPureLocalizationTrimmer(
const int trajectory_id,
const proto::TrajectoryBuilderOptions & trajectory_options,
PoseGraph* pose_graph) {<!-- -->
if (trajectory_options.pure_localization()) {<!-- -->
LOG(WARNING)
<< "'TrajectoryBuilderOptions::pure_localization' field is deprecated. "
"Use 'TrajectoryBuilderOptions::pure_localization_trimmer' instead.";
pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
trajectory_id, 3 /* max_submaps_to_keep */));
return;
}
if (trajectory_options.has_pure_localization_trimmer()) {<!-- -->
pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
trajectory_id,
trajectory_options.pure_localization_trimmer().max_submaps_to_keep()));
}
}
Here is a trimmer that adds a pure localization mode, the old configuration parameter trajectory_options.pure_localization() has been deprecated, we should use the new configuration parameter trajectory_options.pure_localization_trimmer().max_submaps_to_keep(). Changing the function is actually relatively simple, that is, building a subgraph trimmer and passing it to the backend through the pose_graph->AddTrimmer() function. Let’s take a look at where MaybeAddPureLocalizationTrimmer() is called? When building a new trajectory, that is, the function MapBuilder::AddTrajectoryBuilder(), you can see the following code:
?… MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options, pose_graph_.get()); ?…
It should be noted that the map pruner is only configured in the pure positioning mode, and there is no mapping mode.
4. PureLocalization Trimmer
The pure position subgraph trimmer is declared in src/cartographer/cartographer/mapping/pose_graph_trimmer.h, as follows:
class PureLocalizationTrimmer : public PoseGraphTrimmer {<!-- -->
public:
PureLocalizationTrimmer(int trajectory_id, int num_submaps_to_keep);
~PureLocalizationTrimmer() override {<!-- -->}
void Trim(Trimmable* pose_graph) override;
bool IsFinished() override;
private:
const int trajectory_id_;
int num_submaps_to_keep_;
bool finished_ = false;
};
It looks very simple. As mentioned earlier, it creates a submap pruner for each required trajectory, so a trajectory_id_ is needed for binding, and num_submaps_to_keep_ needless to say is the parameter max_submaps_to_keep in the configuration file. finished_ Indicates whether the work of the pruner has been completed. Usually, when the track is completed, the corresponding subgraph pruner will also be marked as completed, and no pruning will be performed. The implementation of its member functions is generally relatively simple, as follows:
PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id,
const int num_submaps_to_keep)
: trajectory_id_(trajectory_id), num_submaps_to_keep_(num_submaps_to_keep) {<!-- -->
CHECK_GE(num_submaps_to_keep, 2) << "Cannot trim with less than 2 submaps";
}
//Crop the subgraph
void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {<!-- -->
// If the trajectory is in the completed state, set num_submaps_to_keep_ to 0
if (pose_graph->IsFinished(trajectory_id_)) {<!-- -->
num_submaps_to_keep_ = 0;
}
// Get all SubmapIds
auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_);
// Crop the submap from serial number 0, and delete related constraints and nodes
for (std::size_t i = 0; i + num_submaps_to_keep_ < submap_ids.size(); + + i) {<!-- -->
pose_graph->TrimSubmap(submap_ids.at(i));
}
// Set track state to DELETED
if (num_submaps_to_keep_ == 0) {<!-- -->
finished_ = true;
pose_graph->SetTrajectoryState(
trajectory_id_, PoseGraphInterface::TrajectoryState::DELETED);
}
}
// get the state of the clipper
bool PureLocalizationTrimmer::IsFinished() {<!-- --> return finished_; }
Its core is to call the pose_graph->TrimSubmap() function of the backend. Before explaining, let’s take a look at the aforementioned:
// called in map_builder.cc, add PureLocalizationTrimmer for pure positioning
void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {<!-- -->
// C++11 does not allow us to move a unique_ptr into a lambda.
PoseGraphTrimmer* const trimmer_ptr = trimmer. release();
AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {<!-- -->
absl::MutexLock locker( & amp; mutex_);
trimmers_.emplace_back(trimmer_ptr);
return WorkItem::Result::kDoNotRunOptimization;
});
}
It is to pass the actual parameter of the subgraph trimmer created in the MaybeAddPureLocalizationTrimmer() function to the formal parameter trimmer of this function. Then assign the pointer to PoseGraph2D::PoseGraphTrimmer, and then add the pointer to PoseGraph2D::trimmers_. Let’s take a look at where this private member variable is used.
5. TrimmingHandle construction
Search for trimmers_ in the source code, and you can see the following fragment in the PoseGraph2D::HandleWorkQueue() function:
?…
RunOptimization();
?…
TrimmingHandle trimming_handle(this);
// Crop the sub-picture, if there is no cropper, it will not be cropped
for (auto & amp; trimmer : trimmers_) {<!-- -->
trimmer->Trim( & amp;trimming_handle); // PureLocalizationTrimmer::Trim()
}
// If the clipper is in the completed state, delete the clipper
trimmers_.erase(
// c++11: std::remove_if If the callback function returns true, move the parameter currently pointed to to the end, and the return value is the first element of the moved area
std::remove_if(trimmers_.begin(), trimmers_.end(),
[](std::unique_ptr<PoseGraphTrimmer> & amp; trimmer) {<!-- -->
return trimmer->IsFinished(); // call PureLocalizationTrimmer::IsFinished()
}),
trimmers_.end());
?…
It first constructs an object of type TrimmingHandle. Interestingly, its constructor still accepts a this pointer object. The specific reasons will be analyzed later. Then you can see that it traverses all the subgraph trimmers, and calls the trimmer->Trim( & amp;trimming_handle) function of the subgraph trimmer, and what is passed to this function is the TrimmingHandle type instance trimming_handle, which we know earlier has Bind the this pointer of the PoseGraph2D instance. Essentially, the subgraph of each trajectory is pruned. After trimming the subgraph, it also performs an erase operation. If the subgraph is already in the completed state, the trimmer is directly erased from trimmers_.
TrimmingHandle needs to pay attention to passing a this pointer during construction, such as TrimmingHandle trimming_handle(this), where this represents an instance of the PoseGraph2D type. The constructor of TrimmingHandle is declared in src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.h, which inherits from Trimmable, which is relatively simple and skipped here. It should be noted that there is a private member PoseGraph2D* const parent_, about The member functions of TrimmingHandle are implemented in pose_graph_2d.cc. Its constructor is as follows:
PoseGraph2D::TrimmingHandle::TrimmingHandle(PoseGraph2D* const parent)
: parent_(parent) {<!-- -->}
The simpler one is to save the parent(this) pointer. Go back and look at the previous code:
for (auto & amp; trimmer : trimmers_) {<!-- -->
trimmer->Trim( & amp;trimming_handle); // PureLocalizationTrimmer::Trim()
}
It passes the constructed trimming_handle object to the Trim function of the PoseGraphTrimmer instance trimmer, which is the PureLocalizationTrimmer::Trim() corresponding to the current trajectory. The code has been analyzed earlier, the core is:
// Crop the submap from serial number 0, and delete related constraints and nodes
for (std::size_t i = 0; i + num_submaps_to_keep_ < submap_ids.size(); + + i) {<!-- -->
pose_graph->TrimSubmap(submap_ids.at(i));
}
The pose_graph->TrimSubmap() function is called for each subgraph. Trimmable* const pose_graph is TrimmingHandle trimming_handle(this), which contains PoseGraph2D instance inside. If you think you understand it, think of TrimmingHandle as an encapsulation of some functions of PoseGraph2D. TrimmingHandle can only access some functions of PoseGraph2D, which can be isolated. as follows
6. TrimmingHandle member function
You can first look at some member functions of TrimmingHandle:
?…
const MapById<NodeId, TrajectoryNode> &
PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const {<!-- -->
return parent_->data_.trajectory_nodes;
}
\t
const std::vector<PoseGraphInterface::Constraint> &
PoseGraph2D::TrimmingHandle::GetConstraints() const {<!-- -->
return parent_->data_. constraints;
}
\t
// track is over, clipper is over
bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const {<!-- -->
return parent_->IsTrajectoryFinished(trajectory_id);
}
?…
It is known that parent_ is an instance of PoseGraph2D, so it essentially calls a member function of PoseGraph2D or a member function of its PoseGraph2D member variable. The general process is as follows:
PoseGraph2D::HandleWorkQueue() PureLocalizationTrimmer::Trim(this) //Each track corresponds to this instance trimmer PoseGraph2D::TrimmingHandle::TrimSubmap(submap_ids.at(i)); //Execute this function for each submap of each track PoseGraph2D::.... PoseGraph2D::.... PoseGraph2D::....
In other words, in the end, the function of PoseGraph2D is called through the TrimmingHandle::TrimSubmap() function to realize the cutting of the submap. Regarding PoseGraph2D::TrimmingHandle::TrimSubmap(), let’s talk about their process first, and then give the overall notes:
the core
\color{red} core
If the core max_submaps_to_keep is 3, it means that there are at most three submaps. If a fourth submap is created, the first submap needs to be deleted. To delete a subgraph, it is necessary to delete the constraints related to the subgraph (within the subgraph, between the subgraphs), as well as the corresponding multi-resolution map, branch and bound scan matcher, etc. of the subgraph, and finally delete the subgraph Map pointers and raster data, etc.
(
1
)
\color{blue}(1)
(1) First, TrimSubmap(submap_ids.at(i)) processes a submap, and only processes completed submaps, so the max_submaps_to_keep parameter for pure positioning must be greater than or equal to 3, because the two submaps are active submaps, Used to insert point cloud data.
(
2
)
\color{blue}(2)
(2) Obtain all node ids of all subgraphs except submap_id, store them in the variable nodes_to_retain, find the nodes that are inside the submap_id subgraph and not in other subgraphs, these nodes need to be deleted, and store them in nodes_to_remove. That is to say, these nodes only belong to submap_id.
(
3
)
\color{blue}(3)
(3) The next step is to find the constraints related to the nodes_to_remove (belonging to submap_id) node, that is, to traverse all the constraints. If the node_id corresponding to the constraint does not belong to nodes_to_remove, it means that it does not need to be deleted. It is recorded in constraints and assigned to parent_-> data_.constraints (equivalent to having removed constraints belonging to nodes_to_remove). If the traversed constraint belongs to nodes_to_remove, first record the constraint.submap_id to which this constraint belongs in other_submap_ids_losing_constraints. Note that at this point the constraints stored in parent_->data_.constraints have nothing to do with the submap_id submap.
(
4
)
\color{blue}(4)
(4) Consider whether it is necessary to delete the submap in other_submap_ids_losing_constraints. At this time, traverse the parent_->data_.constraints after a deletion operation. As long as the submap in other_submap_ids_losing_constraints and parent_->data_.constraints have subgraph connections, Then the subgraph will not be deleted. Otherwise, the matchers of these subgraphs will be deleted, but this subgraph cannot be deleted because there are still constraints within the subgraph.
(
5
)
\color{blue}(5)
(5) Delete the current submap submap_id pointer, matcher, multi-resolution map, and delete this submap from the backend optimization problem optimization_problem_. Finally, remove the nodes corresponding to parent_->data_ and parent_->optimization_problem_ according to the nodes_to_remove parameter. The code as a whole is commented as follows:
// Delete the subgraph with the specified id, and delete related constraints, matchers, and nodes
void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId & amp; submap_id) {<!-- -->
// TODO(hrapp): We have to make sure that the trajectory has been finished
// if we want to delete the last submaps.
// Only submaps in the kFinished state can be cropped
CHECK(parent_->data_.submap_data.at(submap_id).state ==
SubmapState::kFinished);
// Compile all nodes that are still INTRA_SUBMAP constrained to other submaps
// once the submap with 'submap_id' is gone.
// We need to use node_ids instead of constraints here to be also compatible
// with frozen trajectories that don't have intra-constraints.
// Get the ids of all nodes of all submaps except submap_id
std::set<NodeId> nodes_to_retain;
for (const auto & amp; submap_data : parent_->data_.submap_data) {<!-- -->
if (submap_data.id != submap_id) {<!-- -->
nodes_to_retain.insert(submap_data.data.node_ids.begin(),
submap_data.data.node_ids.end());
}
}
// Remove all nodes that are exlusively associated to 'submap_id'.
// Find the nodes that are inside the submap of submap_id and not in other subgraphs, these nodes need to be deleted
std::set<NodeId> nodes_to_remove;
// c ++ 11: std::set_difference finds the difference set, elements that appear in first_set and do not appear in second_set
std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(),
parent_->data_.submap_data.at(submap_id).node_ids.end(),
nodes_to_retain.begin(), nodes_to_retain.end(),
std::inserter(nodes_to_remove, nodes_to_remove.begin()));
// Remove all 'data_. constraints' related to 'submap_id'.
{<!-- -->
// Step: 1 Delete the constraints related to submap_id
std::vector<Constraint> constraints;
for (const Constraint & amp; constraint : parent_->data_. constraints) {<!-- -->
if (constraint.submap_id != submap_id) {<!-- -->
constraints. push_back(constraint);
}
}
parent_->data_. constraints = std::move(constraints);
}
// Remove all 'data_. constraints' related to 'nodes_to_remove'.
// If the removal lets other submaps lose all their inter-submap constraints,
// delete their corresponding constraint submap matchers to save memory.
{<!-- -->
std::vector<Constraint> constraints;
std::set<SubmapId> other_submap_ids_losing_constraints;
// Step: 2 Delete the constraints associated with the nodes in nodes_to_remove, and mark the submap_id
for (const Constraint & amp; constraint : parent_->data_. constraints) {<!-- -->
if (nodes_to_remove.count(constraint.node_id) == 0) {<!-- -->
constraints. push_back(constraint);
} else {<!-- -->
// A constraint to another submap will be removed, mark it as affected.
other_submap_ids_losing_constraints.insert(constraint.submap_id);
}
}
parent_->data_. constraints = std::move(constraints);
// Go through the remaining constraints to ensure we only delete scan
// matchers of other submaps that have no inter-submap constraints left.
// check remaining constraints to make sure we only remove scan matchers for other subgraphs that don't have inter-subgraph constraints
for (const Constraint & amp; constraint : parent_->data_. constraints) {<!-- -->
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) {<!-- -->
continue;
}
// As long as there are other submap constraints in submap_id in other_submap_ids_losing_constraints
// Just delete this submap id from other_submap_ids_losing_constraints, you can keep it
else if (other_submap_ids_losing_constraints.count(
constraint. submap_id)) {<!-- -->
// This submap still has inter-submap constraints - ignore it.
other_submap_ids_losing_constraints.erase(constraint.submap_id);
}
}
// Delete scan matchers of the submaps that lost all constraints.
// TODO(wohe): An improvement to this implementation would be to add the
// caching logic at the constraint builder which could keep around only
// recently used scan matchers.
// Step: 3 Delete the matchers for these subgraph ids
for (const SubmapId & submap_id : other_submap_ids_losing_constraints) {<!-- -->
parent_->constraint_builder_.DeleteScanMatcher(submap_id);
}
}
// Mark the submap with 'submap_id' as trimmed and remove its data.
CHECK(parent_->data_.submap_data.at(submap_id).state ==
SubmapState::kFinished);
// Step: 4 Delete the pointer of this submap
parent_->data_.submap_data.Trim(submap_id);
// Step: 5 Delete the matcher for this submap, and the multiresolution map
parent_->constraint_builder_.DeleteScanMatcher(submap_id);
// Step: 6 Delete the subgraph in optimization_problem_
parent_->optimization_problem_->TrimSubmap(submap_id);
// We have one submap less, update the gauge metrics.
kDeletedSubmapsMetric->Increment();
if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id)) {<!-- -->
kFrozenSubmapsMetric->Decrement();
} else {<!-- -->
kActiveSubmapsMetric->Decrement();
}
// Remove the 'nodes_to_remove' from the pose graph and the optimization
// problem.
// Step: 7 delete node
for (const NodeId & amp; node_id : nodes_to_remove) {<!-- -->
parent_->data_.trajectory_nodes.Trim(node_id);
parent_->optimization_problem_->TrimTrajectoryNode(node_id);
}
}
6. Conclusion
Through this blog, I have a deeper understanding of Cartographer’s pure positioning mode. Its main core is the pruning of subgraphs. The steps are as follows:
the core
\color{red} core
If the core max_submaps_to_keep is 3, it means that there are at most three submaps. If a fourth submap is created, the first submap needs to be deleted. To delete a subgraph, it is necessary to delete the constraints related to the subgraph (within the subgraph, between the subgraphs), as well as the corresponding multi-resolution map, branch and bound scan matcher, etc. of the subgraph, and finally delete the subgraph Shange data for maps, etc.