Karto Correlative Scan Matching
karto slam中scanmatch的方法是暴力匹配的方法，其中我觉得有两点比较重要。
-
对于激光队列中的激光点，在当前观测下需要去除当前观测看不到的点。如下图：
在墙后面的激光点，历史激光是可以观测的到的，但是当前激光观测不到，需要去除，不然会出现匹配到墙对面的情况。
去除的方法是对激光点分段，每段认为是直线段，激光观测位置和当前激光观测位置在直线段的同一边则认为是有效激光点。
-
为了防止多个最优匹配，添加了惩罚机制。原理是以里程计初值为中心，权重最大，边缘权重逐渐减小，对于长走廊问题有比较大的优化。
核心代码实现如下：
void CorrelativeScanMatcher::search(const boost::shared_ptr<ICorrelativeGrid> reference, const mrpt::slam::CSimplePointsMap& points_map,
const mrpt::poses::CPose2D& init, const Eigen::Vector2f& half_range, const Eigen::Vector2f& resolution, mrpt::poses::CPose2D& estimate, float& response)
{
float xy_resolution = resolution[0];
float angle_resolution = resolution[1];
float angle_search_begin = init.phi() - half_range[1];
float angle_search_end = init.phi() + half_range[1] + angle_resolution;
float x_search_begin = init.x() - half_range[0];
float x_search_end = init.x() + half_range[0] + xy_resolution;
float y_search_begin = init.y() - half_range[0];
float y_search_end = init.y() + half_range[0] + xy_resolution;
float best_score = -1.0f; Eigen::Vector3d best;
std::list<Search> bests;
for (float angle = angle_search_begin; angle < angle_search_end; angle += angle_resolution) {
float dsangle = mrpt::math::square(mrpt::math::wrapToPi(angle - init.phi()));
Eigen::Vector3d pose(0., 0., mrpt::math::wrapToPi(angle));
Eigen::Rotation2Df rotation(pose.z());
//std::printf("%8.5f ", pose.z());
std::vector<Eigen::Vector2f> points;
for (int pos = 0; pos < points_map.size(); pos++) {
Eigen::Vector2f point;
points_map.getPoint(pos, point.x(), point.y());
points.push_back(rotation * point);
}
for (float y = y_search_begin; y < y_search_end; y += xy_resolution) {
float dsy = mrpt::math::square(y - init.y());
for (float x = x_search_begin; x < x_search_end; x += xy_resolution) {
float dsx = mrpt::math::square(x - init.x());
pose.x() = x; pose.y() = y;
float score = 0.;
for (int pos = 0; pos < points.size(); pos++) {
auto& point = points.at(pos);
int mx = reference->x2idx(x + point.x());
int my = reference->y2idx(y + point.y());
auto cell = reference->getCell(mx, my);
score += cell;
}
score /= (points_map.size() * CorrelativeGridOccupied);
// do penalize
if (penalize_enabled_) {
float dss = dsx + dsy;
float pd = std::max<float>((1. - (options_.penalty_gain[0] * dss / options_.penalty_variance[0])), options_.penalty_minimum[0]);
float pa = std::max<float>((1. - (options_.penalty_gain[1] * dsangle / options_.penalty_variance[1])), options_.penalty_minimum[1]);
score *= (pd * pa);
}
Search search(pose, score);
searchs_.push_back(search);
if (score > best_score) {
best_score = score;
best = pose;
bests.clear();
//std::printf("%8.5f ", best_score);
}
if (std::abs(score - best_score) < std::numeric_limits<float>::epsilon()) {
bests.push_back(search);
}
}
}
}
//std::printf("\n");
estimate = mrpt::poses::CPose2D(best.x(), best.y(), best.z());
response = best_score;
if (bests.size() > 1) {
Eigen::Vector4d average = Eigen::Vector4d::Zero();
for (auto iter = bests.begin(); iter != bests.end(); iter++) {
average[0] += iter->pose[0];
average[1] += iter->pose[1];
average[2] += std::cos(iter->pose[2]);
average[3] += std::sin(iter->pose[2]);
//std::printf("find %d best: %8.5f,%8.5f,%8.5f.\n", bests.size(), iter->pose[0], iter->pose[1], iter->pose[2]);
}
average = average / bests.size();
estimate = mrpt::poses::CPose2D(average[0], average[1], std::atan2(average[3], average[2]));
}
/*mrpt::poses::CPose2D diff = estimate - init;
std::printf("response:%8.5f diff:(%8.5f,%8.5f,%8.5f) mean:(%8.5f,%8.5f,%8.5f)\n",
response, diff.x(), diff.y(), diff.phi(), estimate.x(), estimate.y(), estimate.phi());*/
}
同样，该方法不仅可以在建图阶段使用，在定位阶段同样可以使用，测试如下视频：
Hector SLAM Scan Matching
Map Access
占用格网图的离散性质限制了可实现的精度，也不允许直接计算插值值或导数。为此，采用双线性滤波的插值方案来计算占用概率和导数。直观地说，网格图单元格值可以看作是一个潜在连续概率分布的样本。给定一个连续的坐标\(P_m\), 则占用概率为\(M(P_m)\)。占用概率对占用坐标的导数\(\Delta M(P_M) = \left( \frac{\partial M}{\partial x}(P_m), \frac{\partial M}{\partial y}(P_m) \right)\)可以通过使用四个最近的整数坐标（\(P_{00} - P_{11}\)）来近似， 如下：
Scan Matching
对于变换到栅格的一个激光点，希望\(M(P_M)\)越大越好（趋于1），在获得t时刻的激光数据后，计算的是t-1时刻到t时刻的位姿增量，在t-1的附近去迭代匹配得到最好的匹配结果。因此可以构造优化问题，使用Gauss-Newton求解。使用这种方法，就不需要在波束端点之间进行数据关联搜索或穷举式搜索。 我们的目的是求解帧间刚体变换\(\boldsymbol{\xi}=\left(p_{x}, p_{y}, \psi\right)^{\mathrm{T}}\), 可以构造代价函数如下：
此代价函数的意思是找到最优的刚体变换，使得激光与地图最匹配，也就是占用概率趋向于1. 其中：
对代价函数一阶泰勒展开并求导：
通过高斯牛顿求解\(\Delta \mathbf{\xi}\):
改进
采用双线性插值具有一些不连续的问题，因此可以做一些改进。
在数值分析这个数学分支中，双三次插值（英語：Bicubic interpolation）是二维空间中最常用的插值方法。在这种方法中，函数 f 在点 (x, y) 的值可以通过矩形网格中最近的十六个采样点的加权平均得到，在这里需要使用两个多项式插值三次函数，每个方向使用一个。
通过双三次插值可以得到一个连续的插值函数，它的一阶偏导数连续，并且交叉导数处处连续。
三次插值函数可以写成：
其中：
Reference
[1] A Flexible and Scalable SLAM System with Full 3D Motion [2] Real-time correlative scan matching. In International Conference on Robotics and Automation [3] Real-Time Loop Closure in 2D LIDAR SLAM [4] M3RSM: Many-to-many multi-resolution scan matching [5] https://github.com/ros-perception/slam_karto.git [6] https://github.com/ros-perception/open_karto.git [7] https://github.com/skasperski/navigation_2d [8] https://github.com/skasperski/OpenKarto [9] Robust and Efficient Robotic Mapping