ScanRegistration源码分析¶
这一节点主要功能实现点云特征点的配准。基本原理：就是根据点云中点的曲率c来将点划分为不同的类别（边/面特征或不是特征），公式如下：
（这里的实现和代码看起来有点不一致,具体实现计算点云中i点与周围10点的距离，求得曲率半径，曲率半径越大，曲率越小）：
刚才我们已经遍历所有点，根据角度计算结果将其划分为不同的线：计算角度–>计算起始和终止位置–>插入IMU数据–>将点插入容器中,最后更新总的点云laserCLoud 我们已经把一帧杂乱的点云数据有条理的放在了容器里，下面就可以开始处理啦！ 我们要在laserCloud中找特征点的候选点，该怎么找呢？没错，就是最暴力的方法：遍历每个点（除了前五个和后五个），计算各点曲率并找到所有线的起点终点位置； extractFeatures()具体代码实现：
void ScanRegistration::extractFeatures(const uint16_t& beginIdx)
{
// extract features from individual scans 遍历我们整理好后的点云，按照激光线束顺序从0-15
size_t nScans = _scanIndices.size();
for (size_t i = beginIdx; i < nScans; i++) {
pcl::PointCloud<pcl::PointXYZI>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<pcl::PointXYZI>);
size_t scanStartIdx = _scanIndices[i].first;
size_t scanEndIdx = _scanIndices[i].second;
// skip empty scans跳过曲率区域太小的区域
if (scanEndIdx <= scanStartIdx + 2 * _config.curvatureRegion) {
continue;
}
// Quick&Dirty fix for relative point time calculation without IMU data
/*float scanSize = scanEndIdx - scanStartIdx + 1;
for (int j = scanStartIdx; j <= scanEndIdx; j++) {
_laserCloud[j].intensity = i + _scanPeriod * (j - scanStartIdx) / scanSize;
}*/
// reset scan buffers 最最重要的就是这个函数了，计算曲率，并排序
setScanBuffersFor(scanStartIdx, scanEndIdx);
// extract features from equally sized scan regions将每条线分成6段，计算每段点云的开头sp，结尾ep位置。sp=s+(e-s)*j/6,ep=s+(e-s)/6+(e-s)*j/6-1
for (int j = 0; j < _config.nFeatureRegions; j++) {
size_t sp = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - j)
+ (scanEndIdx - _config.curvatureRegion) * j) / _config.nFeatureRegions;
size_t ep = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - 1 - j)
+ (scanEndIdx - _config.curvatureRegion) * (j + 1)) / _config.nFeatureRegions - 1;
// skip empty regions
if (ep <= sp) {
continue;
}
size_t regionSize = ep - sp + 1;
// reset region buffers对16束激光中每一束6段中的每一段进行特征挑选
setRegionBuffersFor(sp, ep);
最重要的是setScanBuffersFor()这个函数，实现曲率计算和排序
void ScanRegistration::setRegionBuffersFor(const size_t& startIdx,
const size_t& endIdx)
{
// resize buffers
size_t regionSize = endIdx - startIdx + 1;
_regionCurvature.resize(regionSize);
_regionSortIndices.resize(regionSize);
_regionLabel.assign(regionSize, SURFACE_LESS_FLAT);
// calculate point curvatures and reset sort indices
float pointWeight = -2 * _config.curvatureRegion;
//取i点周围的10个点，计算距离，求出曲率半径，曲率=1/曲率半径
for (size_t i = startIdx, regionIdx = 0; i <= endIdx; i++, regionIdx++) {
float diffX = pointWeight * _laserCloud[i].x;
float diffY = pointWeight * _laserCloud[i].y;
float diffZ = pointWeight * _laserCloud[i].z;
for (int j = 1; j <= _config.curvatureRegion; j++) {
diffX += _laserCloud[i + j].x + _laserCloud[i - j].x;
diffY += _laserCloud[i + j].y + _laserCloud[i - j].y;
diffZ += _laserCloud[i + j].z + _laserCloud[i - j].z;
}
_regionCurvature[regionIdx] = diffX * diffX + diffY * diffY + diffZ * diffZ;
_regionSortIndices[regionIdx] = i;
}
// sort point curvatures把每一激光束求得的曲率半径按照升序排序
for (size_t i = 1; i < regionSize; i++) {
for (size_t j = i; j >= 1; j--) {
if (_regionCurvature[_regionSortIndices[j] - startIdx] < _regionCurvature[_regionSortIndices[j - 1] - startIdx]) {
std::swap(_regionSortIndices[j], _regionSortIndices[j - 1]);
}
}
}
}
setRegionBuffersFor()函数实现特征挑选，参照论文对与点位筛选的条件：1. 平面/直线与激光近似平行的点不能要； 2. 被遮挡的边缘点不能要（(b)中点A右侧部分）
遍历所有点（除去前五个和后六个），判断该点及其周边点是否可以作为特征点位：当某点及其后点间的距离平方大于某阈值a（说明这两点有一定距离），且两向量夹角小于某阈值b时（夹角小就可能存在遮挡），将其一侧的临近6个点设为不可标记为特征点的点；若某点到其前后两点的距离均大于c倍的该点深度，则该点判定为不可标记特征点的点（入射角越小，点间距越大，即激光发射方向与投射到的平面越近似水平）。
void ScanRegistration::setScanBuffersFor(const size_t& startIdx,
const size_t& endIdx)
{
// resize buffers
size_t scanSize = endIdx - startIdx + 1;
_scanNeighborPicked.assign(scanSize, 0);
// mark unreliable points as picked去除这一段中前5个，后6个点
for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) {
const pcl::PointXYZI& previousPoint = (_laserCloud[i - 1]);
const pcl::PointXYZI& point = (_laserCloud[i]);
const pcl::PointXYZI& nextPoint = (_laserCloud[i + 1]);
float diffNext = calcSquaredDiff(nextPoint, point);
if (diffNext > 0.1) {
float depth1 = calcPointDistance(point);
float depth2 = calcPointDistance(nextPoint);
//针对论文b情况
if (depth1 > depth2) {
float weighted_distance = std::sqrt(calcSquaredDiff(nextPoint, point, depth2 / depth1)) / depth2;
//Xi+1-Xi*(|Xi+1|/|Xi|)
if (weighted_distance < 0.1) {//根据等腰三角形的性质，如果Xi与Xi+1的夹角小于阈值0.1即对应5.732°
std::fill_n(&_scanNeighborPicked[i - startIdx - _config.curvatureRegion], _config.curvatureRegion + 1, 1);//_scanNeighborPicked表示一个点的周围不能再设置为特征点
continue;
}
} else {
float weighted_distance = std::sqrt(calcSquaredDiff(point, nextPoint, depth1 / depth2)) / depth1;
if (weighted_distance < 0.1) {
std::fill_n(&_scanNeighborPicked[i - startIdx + 1], _config.curvatureRegion + 1, 1);//std::fill_n()是赋值函数，起始指针，大小，值
}
}
}
float diffPrevious = calcSquaredDiff(point, previousPoint);
float dis = calcSquaredPointDistance(point);
//针对论文a情况
if (diffNext > 0.0002 * dis && diffPrevious > 0.0002 * dis) {
_scanNeighborPicked[i - startIdx] = 1;
}
}
}
现在我们得到了一组有条理的点云，且把那些与我们想提取的特征不符的点标记了出来，那我们只需要从剩下的点中选择最好的那一波不就行了！为了保证特征点均匀的分布在环境中，将一次扫描划分为4个独立的子区域，每个子区域最多提供2个边缘点和4个平面点。我们只需要预先设定好阈值，就可以轻松加随意的将这些点分类了。代码中，把每个线分为6段进行处理，每段都将曲率按照升序排列。看代码：
// extract corner features抽取边特征点最多2个边缘点
int largestPickedNum = 0;
for (size_t k = regionSize; k > 0 && largestPickedNum < _config.maxCornerLessSharp;) {
size_t idx = _regionSortIndices[--k];//排序好的每段中抽取
size_t scanIdx = idx - scanStartIdx;
size_t regionIdx = idx - sp;
//判断条件1.没有别标记为不不可选的点。2.大于阈值
if (_scanNeighborPicked[scanIdx] == 0 &&
_regionCurvature[regionIdx] > _config.surfaceCurvatureThreshold) {//这个变量有点奇怪？？
largestPickedNum++;
if (largestPickedNum <= _config.maxCornerSharp) {
_regionLabel[regionIdx] = CORNER_SHARP;
_cornerPointsSharp.push_back(_laserCloud[idx]);
} else {//大于2个边缘点后情况，存入备选区
_regionLabel[regionIdx] = CORNER_LESS_SHARP;
}
_cornerPointsLessSharp.push_back(_laserCloud[idx]);
markAsPicked(idx, scanIdx);//对选过的点标记为不可再选
}
}
// extract flat surface features抽取面特征点
int smallestPickedNum = 0;
for (int k = 0; k < regionSize && smallestPickedNum < _config.maxSurfaceFlat; k++) {
size_t idx = _regionSortIndices[k];
size_t scanIdx = idx - scanStartIdx;
size_t regionIdx = idx - sp;
if (_scanNeighborPicked[scanIdx] == 0 &&
_regionCurvature[regionIdx] < _config.surfaceCurvatureThreshold) {
smallestPickedNum++;
_regionLabel[regionIdx] = SURFACE_FLAT;
_surfacePointsFlat.push_back(_laserCloud[idx]);
markAsPicked(idx, scanIdx);
}
}
// extract less flat surface features剩下归平面特征点备选
for (int k = 0; k < regionSize; k++) {
if (_regionLabel[k] <= SURFACE_LESS_FLAT) {
surfPointsLessFlatScan->push_back(_laserCloud[sp + k]);
}
}
}
// down size less flat surface point cloud of current scan 下采样surPointsLessFlatScan
pcl::PointCloud<pcl::PointXYZI> surfPointsLessFlatScanDS;
pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(_config.lessFlatFilterSize, _config.lessFlatFilterSize, _config.lessFlatFilterSize);
downSizeFilter.filter(surfPointsLessFlatScanDS);
_surfacePointsLessFlat += surfPointsLessFlatScanDS;
}
}
终于把这波特征点找到了，后面就是把他们简简单单地发布出去了。