一、前言
在前述constraint_builder_2d的建立过程中，建立了新节点与旧子图之间的约束，并将这些约束加入了因子图中，这种约束可以认为是cartgrapher的回环检测，在约束建立后，具体的搜索方法使用的是constraint_builder_2d中私有类建立的SubmapScanMatcher，本文将对其中涉及到的分支定界搜索方法进行分析，并对后端优化器optimization_problem_进行分析，至此整个后端优化过程也就完整结束了
二、SubmapScanMatcher方法
constraint_builder_2d中私有类建立的SubmapScanMatcher如下
private:
struct SubmapScanMatcher {
const Grid2D* grid = nullptr;
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher2D>
fast_correlative_scan_matcher;
std::weak_ptr<common::Task> creation_task_handle;
};fast_correlative_scan_matcher的构造来源于fast_correlative_scan_matcher_2d.h中的方法，其构造函数如下
FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
//子图占据的栅格
const Grid2D& grid,
//子图匹配配置
const proto::FastCorrelativeScanMatcherOptions2D& options)
: options_(options),
//子图地图的作用范围
limits_(grid.limits()),
//存储预算图的容器
precomputation_grid_stack_(
absl::make_unique<PrecomputationGridStack2D>(grid, options)) {}
FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {}约束器构建时，通过选项matchfullmap来选择位姿是否与所有地图进行匹配，然后传入FastCorrelativeScanMatcher2D的不同函数，分为定点匹配与全地图匹配两个函数
//定点匹配
bool FastCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const float min_score, float* score,
transform::Rigid2d* pose_estimate) const {
const SearchParameters search_parameters(options_.linear_search_window(),
options_.angular_search_window(),
point_cloud, limits_.resolution());
return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
point_cloud, min_score, score,
pose_estimate);
}
//全地图匹配
bool FastCorrelativeScanMatcher2D::MatchFullSubmap(
const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const {
// Compute a search window around the center of the submap that includes it
// fully.
const SearchParameters search_parameters(
1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction.
M_PI, // Angular search window, 180 degrees in both directions.
point_cloud, limits_.resolution());
const transform::Rigid2d center = transform::Rigid2d::Translation(
limits_.max() - 0.5 * limits_.resolution() *
Eigen::Vector2d(limits_.cell_limits().num_y_cells,
limits_.cell_limits().num_x_cells));
return MatchWithSearchParameters(search_parameters, center, point_cloud,
min_score, score, pose_estimate);
}两种匹配方法均是输入路径节点初始位姿（全地图匹配不需要）、路径节点的点云、以及匹配参数，调用深度优先分支界定搜索MatchWithSearchParameters函数，匹配完成的返回结果装入pose_estimate中
bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
SearchParameters search_parameters,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const {
CHECK(score != nullptr);
CHECK(pose_estimate != nullptr);
//将激光点云转化到世界坐标系下
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
point_cloud,
transform::Rigid3f::Rotation(Eigen::AngleAxisf(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
//调用correlative_scan_matcher_2d.cc函数，将点云经过角分辨率旋转，获得点云组
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
//调用correlative_scan_matcher_2d.cc函数，完成点云离散化,转化为整型栅格单元索引
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
limits_, rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
//缩小搜索窗口的大小，提高搜索效率
search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());
//对搜索空间分割，得到初始子空间点集合
const std::vector<Candidate2D> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
//分支定界搜索
const Candidate2D best_candidate = BranchAndBound(
discrete_scans, search_parameters, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);
if (best_candidate.score > min_score) {
*score = best_candidate.score;
*pose_estimate = transform::Rigid2d(
{initial_pose_estimate.translation().x() + best_candidate.x,
initial_pose_estimate.translation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
return true;
}
return false;
}整个搜索过程也是比较直观的，把点云和位姿扔进去直接利用深度优先分支定界搜索，关于搜索方法的原理有很多大佬已经总结的很好，我就不再献丑了，这里只分析一下算法
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
//离散化后的点云
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters) const {
//对搜索空间进行初始分割，构建候选位置点
std::vector<Candidate2D> lowest_resolution_candidates =
GenerateLowestResolutionCandidates(search_parameters);
//计算各个估计位姿点的分数
ScoreCandidates(
precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()),
discrete_scans, search_parameters, &lowest_resolution_candidates);
return lowest_resolution_candidates;
}
//构建候选位置点
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters) const {
//步长
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
int num_candidates = 0;
//遍历所有搜索方向
for (int scan_index = 0; scan_index != search_parameters.num_scans;
++scan_index) {
const int num_lowest_resolution_linear_x_candidates =
(search_parameters.linear_bounds[scan_index].max_x -
search_parameters.linear_bounds[scan_index].min_x + linear_step_size) /
linear_step_size;
const int num_lowest_resolution_linear_y_candidates =
(search_parameters.linear_bounds[scan_index].max_y -
search_parameters.linear_bounds[scan_index].min_y + linear_step_size) /
linear_step_size;
num_candidates += num_lowest_resolution_linear_x_candidates *
num_lowest_resolution_linear_y_candidates;
}
//遍历不同角度的scan的x方向与y方向的所有可行解
std::vector<Candidate2D> candidates;
candidates.reserve(num_candidates);
for (int scan_index = 0; scan_index != search_parameters.num_scans;
++scan_index) {
for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x;
x_index_offset <= search_parameters.linear_bounds[scan_index].max_x;
x_index_offset += linear_step_size) {
for (int y_index_offset =
search_parameters.linear_bounds[scan_index].min_y;
y_index_offset <= search_parameters.linear_bounds[scan_index].max_y;
y_index_offset += linear_step_size) {
candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
search_parameters);
}
}
}
CHECK_EQ(candidates.size(), num_candidates);
return candidates;
}
void FastCorrelativeScanMatcher2D::ScoreCandidates(
const PrecomputationGrid2D& precomputation_grid,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate2D>* const candidates) const {
//遍历所有候选点
for (Candidate2D& candidate : *candidates) {
int sum = 0;
for (const Eigen::Array2i& xy_index :
discrete_scans[candidate.scan_index]) {
const Eigen::Array2i proposed_xy_index(
xy_index.x() + candidate.x_index_offset,
xy_index.y() + candidate.y_index_offset);
sum += precomputation_grid.GetValue(proposed_xy_index);
}
//一个scan的sum除以这个scan中点的个数，即为得分
candidate.score = precomputation_grid.ToScore(
sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));
}
std::sort(candidates->begin(), candidates->end(),
std::greater<Candidate2D>());
}
Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate2D>& candidates, const int candidate_depth,
float min_score) const {
//如果搜索树高为0，也就是找到了一个叶子结点，把这个结果返回
if (candidate_depth == 0) {
// Return the best candidate.
return *candidates.begin();
}
//候选点容器
Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
best_high_resolution_candidate.score = min_score;
//遍历所有的候选点
for (const Candidate2D& candidate : candidates) {
//如果有一个候选点的分数很低，后面的候选点也不会有更好的结果了
if (candidate.score <= min_score) {
break;
}
//如果当前点的分数比较高，需要进行分支
std::vector<Candidate2D> higher_resolution_candidates;
const int half_width = 1 << (candidate_depth - 1);
//对x、y偏移进行遍历，求出这一个candidate的四个子节点候选
for (int x_offset : {0, half_width}) {
if (candidate.x_index_offset + x_offset >
search_parameters.linear_bounds[candidate.scan_index].max_x) {
break;
}
for (int y_offset : {0, half_width}) {
if (candidate.y_index_offset + y_offset >
search_parameters.linear_bounds[candidate.scan_index].max_y) {
break;
}
higher_resolution_candidates.emplace_back(
candidate.scan_index, candidate.x_index_offset + x_offset,
candidate.y_index_offset + y_offset, search_parameters);
}
}
//对新扩展的候选点定界并排序
ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
discrete_scans, search_parameters,
&higher_resolution_candidates);
best_high_resolution_candidate = std::max(
best_high_resolution_candidate,
//递归调用
BranchAndBound(discrete_scans, search_parameters,
higher_resolution_candidates, candidate_depth - 1,
best_high_resolution_candidate.score));
}
return best_high_resolution_candidate;
}上面是整个分支定界法搜索的流程，我将其理解为一种二维上的二分法，也就是四分法，在对搜索空间进行切割处理后，构建候选的位置点，递归调用分支定界法不停地四分搜索，将预想的xy坐标一直加上半个偏置，逐步缩小搜索范围，直到找到最终的叶子结点，作为位姿估计的结果并返回
三、optimization_problem_后端优化器
前面构建的约束器，以及SubmapScanMatcher方法，实现新旧节点与新旧子图之间的约束关系，仍旧需要根据后端优化问题，考察所有约束，调整子图和路径节点的位姿，来最小化全局估计与局部估计之间的偏差，这就需要std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_后端优化器来起作用
在pose_graph_2d中，私有类optimization_problem_的建立依赖于optimization_problem_2d.h，继承自接口类optimization_problem_interface.h后端优化器是通过一种称为SPA(Sparse Pose Adjustment)的技术，根据节点与子图之间的约束关系，优化路径节点与子图的世界坐标，本质上还是通过列文伯格-马尔夸特（LM）法来寻找最优解
优化动作的触发是由posd_graph_2d.cc中RunOptimization函数执行的
void PoseGraph2D::RunOptimization() {
//没有要优化的对象
if (optimization_problem_->submap_data().empty()) {
return;
}
// No other thread is accessing the optimization_problem_,
// data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
// when executing the Solve. Solve is time consuming, so not taking the mutex
// before Solve to avoid blocking foreground processing.
//程序运行这部分的时候，没有其他待优化问题要解决了，这部分程序比较耗时
//保证constraints约束器、GetTrajectoryStates路径生成、landmark_nodes地标程序都完成了
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes);
//加线程锁
absl::MutexLock locker(&mutex_);
const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data();
//对每一条轨迹遍历
for (const int trajectory_id : node_data.trajectory_ids()) {
//对每一个节点遍历
for (const auto& node : node_data.trajectory(trajectory_id)) {
//用优化后的位姿来更新轨迹点的世界坐标
auto& mutable_trajectory_node = data_.trajectory_nodes.at(node.id);
mutable_trajectory_node.global_pose =
transform::Embed3D(node.data.global_pose_2d) *
transform::Rigid3d::Rotation(
mutable_trajectory_node.constant_data->gravity_alignment);
}
// Extrapolate all point cloud poses that were not included in the
// 'optimization_problem_' yet.
//计算SPA优化后的世界坐标变换关系
const auto local_to_new_global =
ComputeLocalToGlobalTransform(submap_data, trajectory_id);
//计算SPA优化前的世界坐标变换关系
const auto local_to_old_global = ComputeLocalToGlobalTransform(
data_.global_submap_poses_2d, trajectory_id);
//计算旧坐标系到新坐标系的变换关系
const transform::Rigid3d old_global_to_new_global =
local_to_new_global * local_to_old_global.inverse();
const NodeId last_optimized_node_id =
std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
auto node_it =
std::next(data_.trajectory_nodes.find(last_optimized_node_id));
for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id);
++node_it) {
auto& mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id);
//旧坐标系到新坐标系的变换关系，左乘到global_pose上，进行修正
mutable_trajectory_node.global_pose =
old_global_to_new_global * mutable_trajectory_node.global_pose;
}
}
for (const auto& landmark : optimization_problem_->landmark_data()) {
//landmark也一样的处理
data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
}
//记录下当前位姿
data_.global_submap_poses_2d = submap_data;
}optimization_problem_2d.cc中，后端优化的核心就是类OptimizationProblem2D的成员函数Solve，通过Ceres库进行优化，调整子图和路径节点的世界位姿
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;
}
//已经冻结的轨迹
std::set<int> frozen_trajectories;
for (const auto& it : trajectories_state) {
if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) {
frozen_trajectories.insert(it.first);
}
}
//建立ceres问题的优化对象
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
// Set the starting point.
// TODO(hrapp): Move ceres data into SubmapSpec.
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;
//遍历所有的子图，将子图的全局位姿放置到刚刚构建的临时容器C_submaps中
for (const auto& submap_id_data : submap_data_) {
const bool frozen =
frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
C_submaps.Insert(submap_id_data.id,
FromPose(submap_id_data.data.global_pose));
//把要优化的位子问题添加进去
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.
//第一幅子图或者已经冻结的子图，把C_submaps相关数值设成常量
problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
}
}
//遍历所有的路径节点，将他们的全局位姿作为优化参数告知ceres对象problem
for (const auto& node_id_data : node_data_) {
//已经冻结的节点就不用优化了，和上面子图道理一样
const bool frozen =
frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));
//需要优化的节点加进去
problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
if (frozen) {
problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
}
}
// Add cost functions for intra- and inter-submap constraints.
//遍历所有的约束，描述优化问题的残差块
for (const Constraint& constraint : constraints) {
//构建子图位姿与路径位姿的残差方程，ceres传入数据是指针的方式，所以最后优化结果保存在C_submaps和C_nodes里
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(constraint.pose),
// Loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale())
: nullptr,
C_submaps.at(constraint.submap_id).data(),
C_nodes.at(constraint.node_id).data());
}
// Add cost functions for landmarks.
//根据路标点添加残差项
AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
&problem, options_.huber_scale());
// Add penalties for violating odometry or changes between consecutive nodes
// if odometry is not available.
//遍历所有的路径节点，根据Local SLAM以及里程计等局部定位的信息建立相邻的路径节点之间的位姿变换关系
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id;
const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
if (frozen_trajectories.count(trajectory_id) != 0) {
node_it = trajectory_end;
continue;
}
auto prev_node_it = node_it;
for (++node_it; node_it != trajectory_end; ++node_it) {
const NodeId first_node_id = prev_node_it->id;
const NodeSpec2D& first_node_data = prev_node_it->data;
prev_node_it = node_it;
const NodeId second_node_id = node_it->id;
const NodeSpec2D& second_node_data = node_it->data;
if (second_node_id.node_index != first_node_id.node_index + 1) {
continue;
}
// Add a relative pose constraint based on the odometry (if available).
std::unique_ptr<transform::Rigid3d> relative_odometry =
CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
second_node_data);
if (relative_odometry != nullptr) {
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(Constraint::Pose{
*relative_odometry, options_.odometry_translation_weight(),
options_.odometry_rotation_weight()}),
nullptr /* loss function */, C_nodes.at(first_node_id).data(),
C_nodes.at(second_node_id).data());
}
// Add a relative pose constraint based on consecutive local SLAM poses.
const transform::Rigid3d relative_local_slam_pose =
transform::Embed3D(first_node_data.local_pose_2d.inverse() *
second_node_data.local_pose_2d);
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(
Constraint::Pose{relative_local_slam_pose,
options_.local_slam_pose_translation_weight(),
options_.local_slam_pose_rotation_weight()}),
nullptr /* loss function */, C_nodes.at(first_node_id).data(),
C_nodes.at(second_node_id).data());
}
}
//看到一种说法说C_fixed_frames一般指gps信号这样的全局信息，感觉这块可以改编
std::map<int, std::array<double, 3>> C_fixed_frames;
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id;
const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) {
node_it = trajectory_end;
continue;
}
const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
bool fixed_frame_pose_initialized = false;
for (; node_it != trajectory_end; ++node_it) {
const NodeId node_id = node_it->id;
const NodeSpec2D& node_data = node_it->data;
const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);
if (fixed_frame_pose == nullptr) {
continue;
}
const Constraint::Pose constraint_pose{
*fixed_frame_pose, options_.fixed_frame_pose_translation_weight(),
options_.fixed_frame_pose_rotation_weight()};
if (!fixed_frame_pose_initialized) {
transform::Rigid2d fixed_frame_pose_in_map;
if (trajectory_data.fixed_frame_origin_in_map.has_value()) {
fixed_frame_pose_in_map = transform::Project2D(
trajectory_data.fixed_frame_origin_in_map.value());
} else {
fixed_frame_pose_in_map =
node_data.global_pose_2d *
transform::Project2D(constraint_pose.zbar_ij).inverse();
}
C_fixed_frames.emplace(trajectory_id,
FromPose(fixed_frame_pose_in_map));
fixed_frame_pose_initialized = true;
}
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(constraint_pose),
options_.fixed_frame_pose_use_tolerant_loss()
? new ceres::TolerantLoss(
options_.fixed_frame_pose_tolerant_loss_param_a(),
options_.fixed_frame_pose_tolerant_loss_param_b())
: nullptr,
C_fixed_frames.at(trajectory_id).data(), C_nodes.at(node_id).data());
}
}
// Solve.
//描述完优化参数和残差计算方式之后，通过ceres求解优化问题
ceres::Solver::Summary summary;
ceres::Solve(
common::CreateCeresSolverOptions(options_.ceres_solver_options()),
&problem, &summary);
if (options_.log_solver_summary()) {
LOG(INFO) << summary.FullReport();
}
// Store the result.
//存储优化结果
for (const auto& C_submap_id_data : C_submaps) {
submap_data_.at(C_submap_id_data.id).global_pose =
ToPose(C_submap_id_data.data);
}
for (const auto& C_node_id_data : C_nodes) {
node_data_.at(C_node_id_data.id).global_pose_2d =
ToPose(C_node_id_data.data);
}
for (const auto& C_fixed_frame : C_fixed_frames) {
trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map =
transform::Embed3D(ToPose(C_fixed_frame.second));
}
for (const auto& C_landmark : C_landmarks) {
landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
}
}Solve函数在接受子图、节点、轨迹等的位姿信息，确定优化目标后，对他们的各自的位姿信息做出调整，完成全局优化，整个optimization_problem也就完成了
四、结语
整个cartographer_2d的主题流程基本梳理完了，历时半个月，感觉也只是把整个工程理了一遍，里面其实有好多具体的问题和原理笔者并没有展开分析，后续可能会根据这套工程做一些改编和利用，如果有机会的话，再把遇到的问题和思路发上来，欢迎大家多多交流
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
