cartographer详解(三)—— Real Time Correlative Scan Matcher(2d模式)
摘要:关于real time 匹配的主要原理、代码详解,上一章:cartographer详解(二)—— Real Time Correlative Scan Matcher(3d模式)做了详细的介绍。这一章主要分析2d模式下的做法。
主要区别:
3d: 暴力搜索
2d: 预投影法:先通过角度切片,然后在在xy方向上暴力搜索,为了减少计算量。
减少计算量的原理:
一、主要工作逻辑
- 以初始位置(角度)为基础,角度搜索范围内,按照搜索步长进行旋转当前scan。将这一系列的离散化的scan,按照旋转顺序存储起来。也就是切片
- 在每个切片内,以初始位置(xy)为基础,xy搜索搜索范围内,按照搜索步长,进行离散,生成候选位置。
- 求得每个候选位置的score,score最高的,就为最佳位置。
二、代码详解
下面通过代码,展示具体实现细节。代码里关键的地方有注释,顺着代码应该可以比较清楚的理清具体工作逻辑。
- 代码总入口
Match
double RealTimeCorrelativeScanMatcher::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const {
CHECK_NOTNULL(pose_estimate);
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())));
const SearchParameters search_parameters(
options_.linear_search_window(), options_.angular_search_window(),
rotated_point_cloud, probability_grid.limits().resolution());
//切片旋转,按照搜索角度/步长,以初始值为基础,生成一些列的旋转后的点云,按顺序存放在vector里
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
//将旋转点云,从旋转点云坐标中,转化到地图坐标中去。根据坐标,获得 cell index
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
probability_grid.limits(), rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
//生成所有候选位置
std::vector<Candidate> candidates =
GenerateExhaustiveSearchCandidates(search_parameters);
//对每个候选位置进行打分
ScoreCandidates(probability_grid, discrete_scans, search_parameters,
&candidates);
//找到score最高的候选位置,就为所求位置。
const Candidate& best_candidate =
*std::max_element(candidates.begin(), candidates.end());
*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 best_candidate.score;
}
- 切片旋转
GenerateRotatedScans
std::vector<sensor::PointCloud> GenerateRotatedScans(
const sensor::PointCloud& point_cloud,
const SearchParameters& search_parameters) {
std::vector<sensor::PointCloud> rotated_scans;
rotated_scans.reserve(search_parameters.num_scans); //num_scans切片数目
//旋转角度步长
double delta_theta = -search_parameters.num_angular_perturbations *
search_parameters.angular_perturbation_step_size;
for (int scan_index = 0; scan_index < search_parameters.num_scans;
++scan_index,
delta_theta += search_parameters.angular_perturbation_step_size) {
rotated_scans.push_back(
//将点云point_cloud 按照旋转角度delta_theta 进行旋转
sensor::TransformPointCloud(
point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
delta_theta, Eigen::Vector3f::UnitZ()))));
}
return rotated_scans;
}
- 离散化点云
DiscretizeScans
std::vector<DiscreteScan> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation) {
std::vector<DiscreteScan> discrete_scans;
// typedef std::vector<Eigen::Array2i> DiscreteScan;
discrete_scans.reserve(scans.size());
for (const sensor::PointCloud& scan : scans) {
discrete_scans.emplace_back();
discrete_scans.back().reserve(scan.size());
for (const Eigen::Vector3f& point : scan) {
const Eigen::Vector2f translated_point = //从旋转点云坐标中,转化到地图坐标中去
Eigen::Affine2f(initial_translation) * point.head<2>();
discrete_scans.back().push_back(
//获得该点,对应的栅格索引
map_limits.GetXYIndexOfCellContainingPoint(translated_point.x(),
translated_point.y()));
}
}
return discrete_scans;
}
- 生成所有候选位置
GenerateExhaustiveSearchCandidates
std::vector<Candidate>
RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates(
const SearchParameters& search_parameters) const {
int num_candidates = 0;
for (int scan_index = 0; scan_index != search_parameters.num_scans;
++scan_index) {
const int num_linear_x_candidates =
(search_parameters.linear_bounds[scan_index].max_x -
search_parameters.linear_bounds[scan_index].min_x + 1);
const int num_linear_y_candidates =
(search_parameters.linear_bounds[scan_index].max_y -
search_parameters.linear_bounds[scan_index].min_y + 1);
num_candidates += num_linear_x_candidates * num_linear_y_candidates;
}
std::vector<Candidate> 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) {
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) {
candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
search_parameters);
}
}
}
CHECK_EQ(candidates.size(), num_candidates);
return candidates;
}
- 对每个候选位置进行打分
ScoreCandidates
void RealTimeCorrelativeScanMatcher::ScoreCandidates(
const ProbabilityGrid& probability_grid,
const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate>* const candidates) const {
for (Candidate& candidate : *candidates) {
candidate.score = 0.f;
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);
const float probability =
probability_grid.GetProbability(proposed_xy_index);
candidate.score += probability;
}
candidate.score /=
static_cast<float>(discrete_scans[candidate.scan_index].size());
candidate.score *=
//权重处理
std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) *
options_.translation_delta_cost_weight() +
std::abs(candidate.orientation) *
options_.rotation_delta_cost_weight()));
CHECK_GT(candidate.score, 0.f);
}
}