在AddAccumulatedRangeData函数中通过调用ScanMatch获取位置
// local map frame <- gravity-aligned frame
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
if (pose_estimate_2d == nullptr) {
LOG(WARNING) << "Scan matching failed.";
return nullptr;
}
1、 ScanMatch 代码解析
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
const common::Time time, //最后一个点云对应的时刻
const transform::Rigid2d& pose_prediction,//重力矫正后的最新预测位姿
const sensor::PointCloud& filtered_gravity_aligned_point_cloud)//过滤后的点云数据
{
if (active_submaps_.submaps().empty()) { //暂无active_submaps_时
return absl::make_unique<transform::Rigid2d>(pose_prediction);
}
//存在active_submaps_，取子图
std::shared_ptr<const Submap2D> matching_submap =
active_submaps_.submaps().front();
// The online correlative scan matcher will refine the initial estimate for
// the Ceres scan matcher.
transform::Rigid2d initial_ceres_pose = pose_prediction;
//如果实时匹配参数为true
if (options_.use_online_correlative_scan_matching()) {
const double score = real_time_correlative_scan_matcher_.Match(
pose_prediction, filtered_gravity_aligned_point_cloud,
*matching_submap->grid(), &initial_ceres_pose);
kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
}
auto pose_observation = absl::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary;
ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
filtered_gravity_aligned_point_cloud,
*matching_submap->grid(), pose_observation.get(),
&summary);
if (pose_observation) {
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
const double residual_distance =
(pose_observation->translation() - pose_prediction.translation())
.norm();
kScanMatcherResidualDistanceMetric->Observe(residual_distance);
const double residual_angle =
std::abs(pose_observation->rotation().angle() -
pose_prediction.rotation().angle());
kScanMatcherResidualAngleMetric->Observe(residual_angle);
}
return pose_observation;
}
1.1 RealTimeCorrelativeScanMatcher2D
double RealTimeCorrelativeScanMatcher2D::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());
//旋转点云数据
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
//旋转后的点云加平移
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
probability_grid.limits(), rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
// 生成所有的候选解
std::vector<Candidate2D> candidates =
GenerateExhaustiveSearchCandidates(search_parameters);
//计算候选解得分
ScoreCandidates(probability_grid, discrete_scans, search_parameters,
&candidates);
//筛选出最优解
const Candidate2D& 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; //返回得分
}
1.1.1 SearchParameters
构建搜索框参数
搜索角度步长论文中计算公式
SearchParameters::SearchParameters(const double linear_search_window,
const double angular_search_window,
const sensor::PointCloud& point_cloud,
const double resolution)
: resolution(resolution) {
// We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined.
float max_scan_range = 3.f * resolution;
for (const Eigen::Vector3f& point : point_cloud) {
const float range = point.head<2>().norm();
max_scan_range = std::max(range, max_scan_range);
}
const double kSafetyMargin = 1. - 1e-3;
//角度分辨率计算 根据论文公式计算
angular_perturbation_step_size =
kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
(2. * common::Pow2(max_scan_range)));
//计算搜索角度的个数
num_angular_perturbations =
std::ceil(angular_search_window / angular_perturbation_step_size);
//搜索扩大2倍
num_scans = 2 * num_angular_perturbations + 1;
//x,y搜索栅格数的计算
const int num_linear_perturbations =
std::ceil(linear_search_window / resolution);
linear_bounds.reserve(num_scans);
/*
struct LinearBounds {
int min_x;
int max_x;
int min_y;
int max_y;
};
*/
for (int i = 0; i != num_scans; ++i) {
//根据num_linear_perturbations 设定区间范围
linear_bounds.push_back(
LinearBounds{-num_linear_perturbations, num_linear_perturbations,
-num_linear_perturbations, num_linear_perturbations});
}
}
1.1.2 GenerateRotatedScans
根据search_parameters生成旋转后的点云
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);//设置旋转点云大小
//设定搜索起始角度
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(sensor::TransformPointCloud(
point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
delta_theta, Eigen::Vector3f::UnitZ()))));
}
return rotated_scans;
}
按照角度旋转后的旋转向量
transform::Rigid3f::Rotation(Eigen::AngleAxisf(
delta_theta, Eigen::Vector3f::UnitZ()))
将点云按照旋转向量进行旋转得到旋转后的点云数据
sensor::TransformPointCloud(
point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
delta_theta, Eigen::Vector3f::UnitZ())))
1.1.3 DiscretizeScans
将旋转后的点云集合按照预测出的平移量进行平移, 获取平移后的点在地图中的索引typedef std::vector<Eigen::Array2i> DiscreteScan2D;
std::vector<DiscreteScan2D> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation) {
std::vector<DiscreteScan2D> discrete_scans;
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.GetCellIndex(translated_point));
}
}
return discrete_scans; //返回的就直接是 num_scans 帧各个点云在栅格地图中的索引。
}
每个旋转后的点云加上平移得到world的坐标
const Eigen::Vector2f translated_point =
Eigen::Affine2f(initial_translation) * point.head<2>();
获取激光点在栅格下的索引值
map_limits.GetCellIndex(translated_point)
Eigen::Array2i GetCellIndex(const Eigen::Vector2f& point) const {
// Index values are row major and the top left has Eigen::Array2i::Zero()
// and contains (centered_max_x, centered_max_y). We need to flip and
// rotate.
return Eigen::Array2i(
common::RoundToInt((max_.y() - point.y()) / resolution_ - 0.5),
common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5));
}
1.1.4 GenerateExhaustiveSearchCandidates
std::vector<Candidate2D>
RealTimeCorrelativeScanMatcher2D::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<Candidate2D> candidates;
candidates.reserve(num_candidates);
//遍历所有点云数据
for (int scan_index = 0; scan_index != search_parameters.num_scans;
++scan_index) {
//一个xy位置上对应一个点云数据
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;
}
1.1.5ScoreCandidates
计算候选者得分，暴力匹配
void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
const ProbabilityGrid& probability_grid,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate2D>* const candidates) const {
//遍历所有候选解
for (Candidate2D& candidate : *candidates) {
candidate.score = 0.f;
for (const Eigen::Array2i& xy_index :
discrete_scans[candidate.scan_index]) { //discrete_scans存储的是对应栅格索引
const Eigen::Array2i proposed_xy_index( //激光索引值+候选者在xy上的平移
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);
}
1.2 CeresScanMatcher2D::Match
内容在 cartographer代码学习笔记-CeresScanMatcher2D
参考
Cartographer源码阅读2D-前端暴力匹配-RealTimeCorrelativeScanMatcher2D
cartographer_real_time_correlative_scan_matcher_2d
945
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
