Explain the summary link of a series of articles about slam: the most complete slam in history starts from scratch , and explain for this column (02) Analysis of Cartographer source code without dead ends-links are as follows:
(02) Cartographer source code analysis without dead ends- (00) Catalog_Latest None Dead corner explanation: https://blog.csdn.net/weixin_43013761/article/details/127350885
The center at the bottom of the article provides my contact information, click on my photo to display WX → official 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}At the bottom of the article, the center provides my contact information. Click on my photo to display W X→Official certification
I. Introduction
In the previous blog, the interaction process between PoseGraph2D and OptimizationProblem2D was analyzed. And know that the last instruction to perform optimization is the following code in PoseGraph2D::RunOptimization():
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),data_.landmark_nodes);
In addition, we should answer:
Question 1: \color{red}Question 1:Question 1: global_submap_poses is equivalent to when PoseGraph2D::data_.global_submap_poses_2d is optimized.
In other words, all the previous confusions have been answered in the source code. Of course, a new question arises at this time, that is how optimization_problem_->Solve() implements optimization. According to the previous analysis, it can be known that the received actual parameters are relatively simple, namely the constraints data_.constraints of nodes and subgraphs, the state of the trajectory GetTrajectoryStates(), and the landmark data data_.landmark_nodes.
2. Relative pose
Before formally analyzing OptimizationProblem2D, you need to review the previous content, that is, the transformation relationship, such as the coordinate transformation from local system to robot system, global system to robot system, submap system to robot system, or global system to local system, or is the relative pose between multiple coordinate systems.
1. The pose of the node in the global system
// 将节点在local坐标系下的坐标转成global坐标系下的坐标
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
GetLocalToGlobalTransform returns a pose transformation from the local system to the global system, but note that constant_data->local_pose is optimized at the front end. As mentioned before, GetLocalToGlobalTransform(), when there is no submap global pose in the trajectory data_.global_submap_poses_2d, returns the unit transformation transform::Rigid3d::Identity(), which can be simply understood as the global system of the first submap The origin is the same as lcoal.
2. The pose of the submap in the global coordinate system
It should be noted that after the submap is added, after data_.global_submap_poses_2d has data, the global pose of the last submap is multiplied by the local pose of the last submap to obtain the pose transformation from global to local .
After this analysis, it seems that a question arises, that is, the pose of the first sub-image of the global system and the local system is the same, so will there be a gap in the pose of the sub-images behind them? As mentioned earlier in the analysis of PoseGraph2D::InitializeGlobalSubmapPoses(), the global pose of the subgraph is based on the pose of the node in the adjacent subgraph, and the pose transformation of the two subgraphs is solved, and then according to the global of the previous subgraph Pose (the global pose of the first subgraph is the same as its local pose), and the technology produces the global pose of the new subgraph. It can be seen here that the global pose of the subgraph has a multiplicative relationship.
Although the global pose of the subgraph and the local pose have the same starting point, the global pose of the subgraph has been optimized at the back end, so as time goes by and the number of subgraphs increases, the pose between them is There is a gap.
3. Constraints within the subgraph (the subgraph to which the node belongs belongs to the pose transformation of the node)
In the PoseGraph2D::ComputeConstraintsForNode() function, you can see the following code:
// 计算该Node投影到平面后的位姿 gravity_alignment是机器人的姿态
const transform::Rigid2d local_pose_2d =
transform::Project2D(constant_data->local_pose * // 三维转平面
transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse()));
// 计算该Node在global坐标系下的二维位姿
// global_pose * constraints::ComputeSubmapPose().inverse() = globla指向local的坐标变换
const transform::Rigid2d global_pose_2d =
optimization_problem_->submap_data().at(matching_id).global_pose *
constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
local_pose_2d;
It first obtains the coordinate transformation from the local system to the global system of the subgraph, and then transforms the pose of the node in the local system to the global system. The optimization_problem_->submap_data().at(matching_id).global_pose pose here may or may not be optimized, but local_pose_2d is optimized by front-end scan matching, we think it is more accurate
4. Constraints between subgraphs (the pose transformation of a node from a non-belonging subgraph to the node)
The calculation of constraints between subgraphs is divided into two types, one is global, that is, the node and all completed subgraphs calculate the constraint relationship (only used in relocation mode), the other is local, the node is only a certain distance away from Constraints are established for subgraphs in the subgraph (only used in the drawing mode), which mainly involves the following codes:
// submap原点在global坐标系下的坐标的逆 * 节点在global坐标系下的坐标 = submap原点指向节点的坐标变换
const transform::Rigid2d initial_relative_pose =
optimization_problem_->submap_data()
.at(submap_id)
.global_pose.inverse() *
optimization_problem_->node_data().at(node_id).global_pose_2d;
// Step:1 得到节点在local frame下的位姿，如果是全局匹配 initial_relative_pose=transform::Rigid2d::Identity(),
const transform::Rigid2d initial_pose =ComputeSubmapPose(*submap) * initial_relative_pose;
// Step:4 获取节点到submap坐标系原点间的坐标变换
// pose_estimate 是 节点在 loacl frame 下的坐标
const transform::Rigid2d constraint_transform =ComputeSubmapPose(*submap).inverse() * pose_estimate;
The constraints here are roughly solved through branch and bound and violent matching of multi-resolution maps, and then optimized through ceres, so that all point clouds can be hit on obstacles as much as possible. So it can be said to be an exact solution.
5. Summary
（ 1 ）： \color{blue}（1）： ( 1 ): The node calculates the global pose through GetLocalToGlobalTransform * constant_data->local_pose
(2): \color{blue} (2):( 2 ): The sub-image accumulates the coordinate transformation from the previous sub-image to the next sub-image to obtain the pose of the sub-image in the global coordinate system
(3): \color{blue} (3):( 3 ): Under the constrained local coordinate system in the subgraph, the origin of the subgraph points to the coordinate transformation between nodes
(4): \color{blue} (4):( 4 ): The constraints between subgraphs calculate the initial value according to the global coordinates, and then use the branch and bound algorithm for rough matching and fine matching of ceres to obtain the calibrated pose, and finally calculate the local coordinate system, and the origin of the subgraph points to the calibrated The coordinate transformation between the nodes of
3. The core theory of back-end optimization
Personal understanding: \color{red} Personal understanding:personal understanding: Here, I will throw bricks and cite as an introduction, and talk about my understanding of back-end optimization. First of all, the back-end optimization is considered from a collation, which is based on the global system. The transformation relationship between subgraphs and subgraphs is used, but in the front-end (based on the local system), the pose of the subgraph is a single pose relative to the local system. It can be said that there is no direct connection between multiple subgraphs. linked together. In addition to the subgraph, the same is true for the nodes. The front-end nodes calculate the pose relative to the local system based on the active subgraph, but they are not connected with the previously completed subgraph, and they appear relatively isolated.
However, in the back-end optimization, these are all taken into account. The estimated node pose not only hopes that its pose in the active subgraph is relatively accurate, but also its pose relative to other subgraphs is also expected to be relatively accurate. And the pose between the sub-images is also relatively accurate. At the same time, these accurate poses must be passed to the front end, because the global optimization is optimized at a certain frequency, not just once.
The optimization process is to adjust the pose, so that the constraint (accurate solution) remains unchanged, adjust and optimize the global pose of the node and the subgraph, the optimized pose node pose and the global pose of the subgraph They are stored in PoseGraph2D::data_::trajectory_nodes and PoseGraph2D::data_::global_submap_poses_2d respectively. In addition, the back-end optimization can also combine OdometryData, FixedFramePoseData, ImuData, LandmarkData and other data to optimize the pose, which can also be understood as constraints.
In general, under the premise of ensuring that the constraints remain unchanged, adjust other poses, just like another fishing net, the relationship is intricate, and now we want to unfold this net. What should I do? You can find characteristic places, such as the four corners, or places that have been marked before, and fix these positions first, and then adjust other positions little by little after fixing. Until the net is paved and unfolded.
Finally, in order to verify what I mentioned earlier, the initial poses of the local system and the global system are the same. Although there will be some errors in the follow-up, it will not be too large, so I added in the code section of the ResultPoseGraph2D::ComputeConstraintsForNode() function The cout output:
// 计算该Node在global坐标系下的二维位姿
// global_pose * constraints::ComputeSubmapPose().inverse() = globla指向local的坐标变换
const transform::Rigid2d global_pose_2d =
optimization_problem_->submap_data().at(matching_id).global_pose *
constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
local_pose_2d;
std::cout << optimization_problem_->submap_data().at(matching_id).global_pose *
constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse()
<< std::endl;
What is printed is the transformation of the global system and the local system. When the subgraphs are the same, they are the same when winning. I will filter the excerpts as follows:
{
t: [0, 0], r: [0] } submap (0, 0).
{
t: [0, 0], r: [0] } submap (0, 1).
......
{
t: [0.0942178, -0.166682], r: [-0.00452089] } submap (0, 7)
......
It can be seen that these numerical differences are very small, t represents the distance difference between the x and y axes, and r represents the difference in rotation radians. This should be in units of rasters, or pixels.
Four, OptimizationProblem2D::Solve() input
Based on the previous theories, it will not be so confusing to analyze the code related to optimization. Look at the OptimizationProblem2D::Solve() function in the optimization_problem_2d.cc file. This function is a bit scary, with more than 200 lines of code. I can only explain bit by bit, so I won't comment on the whole for the time being. First see the following code:
/**
* @brief 搭建优化问题并进行求解
*
* @param[in] constraints 所有的约束数据
* @param[in] trajectories_state 轨迹的状态
* @param[in] landmark_nodes landmark数据
*/
void OptimizationProblem2D::Solve(
const std::vector<Constraint>& constraints,
const std::map<int, PoseGraphInterface::TrajectoryState>&
trajectories_state,
const std::map<std::string, LandmarkNode>& landmark_nodes) {
if (node_data_.empty()) {
// Nothing to optimize.
return;
}
// 记录下所有FROZEN状态的轨迹id
std::set<int> frozen_trajectories;
for (const auto& it : trajectories_state) {
if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) {
frozen_trajectories.insert(it.first);
}
}
The input parameters are the constraints from PoseGraph2D::data_::constraints, the trajectory state of PoseGraph2D::GetTrajectoryStates(), and the landmark data of PoseGraph2D::data_::landmark_nodes. Then judge whether the current node data node_data_ is empty, if it is empty, no optimization will be performed. At the same time, record the frozen trajectory di in the frozen_trajectories variable.
Five, OptimizationProblem2D::Solve() build Problem
// 创建优化问题对象
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
It constructs a ceres::Problem object according to the optimization_problem configuration parameter in the src/cartographer/configuration_files/pose_graph.lua file for subsequent nonlinear optimization. The main parameters are as follows
-- 优化残差方程的相关参数
optimization_problem = {
huber_scale = 1e1, -- 值越大,（潜在）异常值的影响就越大
acceleration_weight = 1.1e2, -- 3d里imu的线加速度的权重
rotation_weight = 1.6e4, -- 3d里imu的旋转的权重
-- 前端结果残差的权重
local_slam_pose_translation_weight = 1e5,
local_slam_pose_rotation_weight = 1e5,
-- 里程计残差的权重
odometry_translation_weight = 1e5,
odometry_rotation_weight = 1e5,
-- gps残差的权重
fixed_frame_pose_translation_weight = 1e1,
fixed_frame_pose_rotation_weight = 1e2,
fixed_frame_pose_use_tolerant_loss = false,
fixed_frame_pose_tolerant_loss_param_a = 1,
fixed_frame_pose_tolerant_loss_param_b = 1,
log_solver_summary = false,
use_online_imu_extrinsics_in_3d = true,
fix_z_in_3d = false,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 50,
num_threads = 7,
},
},
Six, OptimizationProblem2D::Solve() set optimization parameters
After constructing the ceres::Problem instance problem, it is necessary to tell problem which parameters need to be optimized. The following three variables are created in the source code (the first three lines of code)
// Set the starting point.
// TODO(hrapp): Move ceres data into SubmapSpec.
// ceres需要double的指针, std::array能转成原始指针的形式
MapById<SubmapId, std::array<double, 3>> C_submaps;
MapById<NodeId, std::array<double, 3>> C_nodes;
std::map<std::string, CeresPose> C_landmarks;
bool first_submap = true;
Because ceres optimization can only pass pointers of double type, these three variables are constructed for temporary storage and conversion. first_submap indicates whether it is the first submap, if it is the first submap, its global pose does not need to be optimized (it has been proved that the global pose and local pose of the first submap are equal ),code show as below:
// 将需要优化的子图位姿设置为优化参数
for (const auto& submap_id_data : submap_data_) {
// submap_id的轨迹 是否是 已经冻结的轨迹
const bool frozen =
frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
// 将子图的global_pose放入C_submaps中
C_submaps.Insert(submap_id_data.id,
FromPose(submap_id_data.data.global_pose));
// c++11: std::array::data() 返回指向数组对象中第一个元素的指针
// Step: 添加需要优化的数据 这里显式添加参数块,会进行额外的参数块正确性检查
problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);
if (first_submap || frozen) {
first_submap = false;
// Fix the pose of the first submap or all submaps of a frozen
// trajectory.
// Step: 如果是第一幅子图, 或者是已经冻结的轨迹中的子图, 不优化这个子图位姿
problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
}
}
It first traverses the submap_data_ added to OptimizationProblem2D to determine whether its trajectory is frozen, whether it is frozen or not, it will be converted into std::array<double, 3> format, and then insert its global pose into C_submaps and call problem.AddParameterBlock() is added to the parameter block, but if it is frozen, it will call problem.SetParameterBlockConstant() to tell the problem that it is frozen and does not need to be optimized. The problem.AddParameterBlock() function call is also very simple, just give a start pointer and the number of elements. After adding the global pose of the subgraph to the parameter block, it is to add the global pose of the node node to the parameter block. The code is as follows:
// Step: 第一种残差 将节点与子图原点在global坐标系下的相对位姿 与 约束 的差值作为残差项
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
problem.AddResidualBlock(
// 根据SPA论文中的公式计算出的残差的CostFunction
CreateAutoDiffSpaCostFunction(constraint.pose),
// Loop closure constraints should have a loss function.
// 为闭环约束提供一个Huber的核函数,用于降低错误的闭环检测对最终的优化结果带来的负面影响
constraint.tag == Constraint::INTER_SUBMAP // 核函数
? new ceres::HuberLoss(options_.huber_scale()) // param: huber_scale
: nullptr,
C_submaps.at(constraint.submap_id).data(), // 2个优化变量
C_nodes.at(constraint.node_id).data());
}
All nodes will be optimized here, that is, they will be added to the parameter block, and the problem.SetParameterBlockConstant() function will not be called to fix the pose. Of course, if the trajectory to which the node belongs is frozen, it will also be added to the parameter block. Fixed, not optimized.
7. Conclusion
Through the previous series of operations, the basic work has been completed. The next step is to construct the residual block. Needless to say, this is definitely the core part. The next blog will start a detailed analysis.