Cartographer源码阅读-2D前端暴力匹配-RealTimeCorrelativeScanMatcher2D
使用暴力匹配的思想,解决当前激光帧和submap匹配的问题:
假定在SLAM的过程中,车体的运动是连续有界的,即运动速度是不可能达到无穷大,根据车体的运动速度,可以预测当前帧和上一帧之间的最大相对运动大小是多少,假设我们已经知道上一帧经过CSM解算过的位姿,我们将所有可能的位姿和当前激光帧组合到一起,生成可能解。然后,遍历所有可能解,把所有的可能解投射到submap上:计算当前激光帧通过其位姿转换后,得到这些hit点落在submap上的位置,并计算这些位置处的占据概率之和,如果占据概率之和越大,则表示该可能解的位姿越可靠。通过暴力匹配,得到最高得分的解即为可能解。
RealTimeCorrelativeScanMatcher2D
class RealTimeCorrelativeScanMatcher2D {
public:
explicit RealTimeCorrelativeScanMatcher2D(
const proto::RealTimeCorrelativeScanMatcherOptions& options);
RealTimeCorrelativeScanMatcher2D(const RealTimeCorrelativeScanMatcher2D&) =
delete;
RealTimeCorrelativeScanMatcher2D& operator=(
const RealTimeCorrelativeScanMatcher2D&) = delete;
// Aligns 'point_cloud' within the 'probability_grid' given an
// 'initial_pose_estimate' then updates 'pose_estimate' with the result and
// returns the score.
double Match(const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const;
// Computes the score for each Candidate2D in a collection. The cost is
// computed as the sum of probabilities, different from the Ceres
// CostFunctions: http://ceres-solver.org/modeling.html
//
// Visible for testing.
void ScoreCandidates(const ProbabilityGrid& probability_grid,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate2D>* candidates) const;
private:
// 根据预设的窗口大小,生成Candidate2D数据
std::vector<Candidate2D> GenerateExhaustiveSearchCandidates(
const SearchParameters& search_parameters) const;
const proto::RealTimeCorrelativeScanMatcherOptions options_;
};
根据调用接口可以知道,scan匹配的是前端的active_submaps的第一个submap。
入口函数:
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()));
// 根据搜索框,生成candidates,即为候选解
std::vector<Candidate2D> candidates =
GenerateExhaustiveSearchCandidates(search_parameters);
// 对candidates打分
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;
}
// 构建搜索框
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);
}
// 计算角度增长step
const double kSafetyMargin = 1. - 1e-3;
// 角度非常小,计算角分辨率的方法在论文中有体现,推导方法如下:
angular_perturbation_step_size =
kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
(2. * common::Pow2(max_scan_range)));
// 计算角度有多少个增长的step
num_angular_perturbations =
std::ceil(angular_search_window / angular_perturbation_step_size);
// 数量为2*num_angular_perturbations + 1,因为在根据角度生成点云时,是从-angular_search_window到+angular_search_window生成的。
num_scans = 2 * num_angular_perturbations + 1;
// 计算线性搜索框有多少个step,step的size是resolution
const int num_linear_perturbations =
std::ceil(linear_search_window / resolution);
// 得到线搜索框的边界
linear_bounds.reserve(num_scans);
for (int i = 0; i != num_scans; ++i) {
linear_bounds.push_back(
LinearBounds{-num_linear_perturbations, num_linear_perturbations,
-num_linear_perturbations, num_linear_perturbations});
}
}
// 根据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);
// 按照-angular->+angular区间,生成2 * num_angular_perturbations + 1个点云
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;
}
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());
// 点云中的每个点加上平移,转换到世界坐标系,并得到在地图中对应的id
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;
}
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)
{
// 获取x方向offset数量
const int num_linear_x_candidates =
(search_parameters.linear_bounds[scan_index].max_x -
search_parameters.linear_bounds[scan_index].min_x + 1);
// 获取y方向offset数量
const int num_linear_y_candidates =
(search_parameters.linear_bounds[scan_index].max_y -
search_parameters.linear_bounds[scan_index].min_y + 1);
// x*y为offset总量
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)
{
// 生成candidates,一个offset(一个新的位姿)对应一个点云,所以有点云数量*x方向offset数量*y方向offset数量个可能解
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;
}
// 运用暴力匹配的方法,
void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
const ProbabilityGrid &probability_grid,
const std::vector<DiscreteScan2D> &discrete_scans,
const SearchParameters &search_parameters,
std::vector<Candidate2D> *const candidates) const
{
// 遍历所有的candidates可能解
for (Candidate2D &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());
// std::hypot:计算两点之间的距离
// 最终的score=平均score*exp^(-x^2):x=距离*权重+角度*权重
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);
}
}
角分辨率公式推导:
在很远的地方,一个网格分辨率r和激光雷达中心连线,如下图所示:
该三角形可以看做是等腰三角形。由余弦定理: cos θ = a 2 + b 2 − c 2 2 a b \cos\theta=\frac{a^2+b^2-c^2}{2ab} cosθ=2aba2+b2−c2,对于本三角形有: a = b = d m a x , c = r a=b=dmax, c=r a=b=dmax,c=r,带入公式有: cos θ = 2 d m a x 2 − r 2 2 d m a x 2 = 1 − r 2 2 d m a x 2 \cos\theta=\frac{2dmax^2-r^2}{2dmax^2}=1-\frac{r^2}{2dmax^2} cosθ=2dmax22dmax2−r2=1−2dmax2r2。
看一下可能解的定义:Candidate2D:
struct Candidate2D {
Candidate2D(const int init_scan_index, const int init_x_index_offset,
const int init_y_index_offset,
const SearchParameters& search_parameters)
: scan_index(init_scan_index),
x_index_offset(init_x_index_offset),
y_index_offset(init_y_index_offset),
// 得到真实的xy数据
x(-y_index_offset * search_parameters.resolution),
y(-x_index_offset * search_parameters.resolution),
// scan_index包含了角度信息,计算方式如下
orientation((scan_index - search_parameters.num_angular_perturbations) *
search_parameters.angular_perturbation_step_size) {}
// Index into the rotated scans vector.
int scan_index = 0;
// Linear offset from the initial pose.
int x_index_offset = 0;
int y_index_offset = 0;
// Pose of this Candidate2D relative to the initial pose.
double x = 0.;
double y = 0.;
double orientation = 0.;
// Score, higher is better.
float score = 0.f;
// 排序方法
bool operator<(const Candidate2D& other) const { return score < other.score; }
bool operator>(const Candidate2D& other) const { return score > other.score; }
};