Karto slam源码阅读(三) 基于相关方法的scan-to-map匹配
http://pic1.zhimg.com/v2-df81be8f6fdf5bcd3479f1cadd5d4aec_r.jpgKarto slam源码阅读(三) 基于相关方法的scan-to-map匹配
scan-to-map匹配是激光雷达中一个基础的概念，scan是当前帧，map是当前帧时空维度上临近的帧组成的集合。map已经经过不断地优化，有一定精度了，而当前帧只有一个初始的估计位姿。至于怎么来的，可以是用前一帧位姿+运动计算，也可以是有外部传感器IMU、轮式里程计等得到的，总之它是不准确的。那么我们可以将当前帧的激光点，随意变换一下，再与map的激光点进行匹配。如果匹配的特别好，我们认为这个随意变换的pose就是我们需要的校正位姿。当然，随意变换是有个限制的，不能变换的特别特别大吧，所以定义了一个变化范围，称之为搜索空间。包括平移（x、y）、旋转（theta）维度上的搜索。
如何判断scan与map匹配的好不好呢？很简单，scan的激光点变换之后的位置，有map点接盘，不管是什么点，只要有个点就算匹配上了。统计一下匹配上的比例，用它来评判匹配好不好。为啥有效呢？可以想想。
另外要提一点，搜索空间中的角度问题。如果每平移一个（dx, dy）之后，再计算旋转dtheta角度对应的激光点位置，那计算DX*DY*DTheta次。能不能精简一下呢，可以的。我们知道旋转这个东西跟在哪没有关系，所以一开始就先计算好旋转一个dtheta角之后，激光点的偏移量，注意是偏移量。后面对于任何平移（dx, dy）之后的位置，加上这个偏移量就可以了。整个计算量就是DX*DY + DTheta次，少了很多。
scan-to-map匹配在很多地方都会用到。当前帧与局部map匹配，当前帧与闭环候选帧集合匹配，当前帧与广搜临近帧的chain匹配，都会调用这个方法。
相关函数包括：MatchScan、CorrelateScan、ComputeOffsets、GetResponse、
ComputePositionalCovariance、ComputeAngularCovariance。
主函数
/**
* 基于相关方法的scan-to-map匹配
* 1、用前一帧优化前后的位姿变换，来校正当前帧优化前的位姿，得到一个初始位姿
* 2、提取局部map中当前帧也可以看到的点，对应栅格设置为占用
* 剔除相对于当前帧属于物体背面的点，也就是近邻帧与当前帧在物体两面
* 3、scan-to-map匹配
* 1) 创建旋转角度-激光点旋转后相对于当前帧位置的偏移量
* 创建索引表，计算当前帧激光点，在各个旋转角度变换下，新的激光点位置相对于当前帧位置（机器人的位置）的坐标偏移量
* 不管当前帧在什么位置，只要旋转角度一定，那么坐标偏移量也是确定的。
* 目的是后面在搜索空间中对当前帧位姿施加不同的x平移、y平移之后再施加一个旋转的操作，不用重新计算每个搜索位置处旋转某个角度之后对应的点坐标，只需要加上对应旋转角度下算好的坐标偏移量即可。
* 2) 遍历pose搜索空间计算响应值
* a. 当前帧激光点集合在某个pose变换下得到新的位置，统计在局部map栅格中这些新位置处占用的数量，占用越多响应值越高，表示匹配越好
* b. 惩罚一下搜索偏移量，越远响应值折扣越多（当前帧搜索距离、角度偏移）
* 3) 找到响应值最大的pose，如果有多个最佳响应pose，那么对pose求个均值，得到最终pose
* 4) 计算位姿的协方差
* 4、如果响应值是0，没匹配上，扩大角度搜索范围，增加20°、40°、60°，再执行一次scan-to-map匹配
* 5、在scan-to-map优化之后的位姿基础上，再缩小搜索空间优化一次，搜索范围减半，角度区间减半，执行scan-to-map匹配
* @param pScan 当前帧
* @param rBaseScans 局部map（当前帧时空维度上相邻的帧集合）
* @param rMean 输出优化后位姿
* @param rCovariance 输出协方差矩阵，dx，dy，dtheta
* @param doPenalize 是否对响应值做搜索距离上的惩罚
* @param doRefineMatch 是否细化搜索空间，执行第二次优化
* @return 返回响应值
*/
kt_double ScanMatcher::MatchScan(LocalizedRangeScan* pScan, const LocalizedRangeScanVector& rBaseScans, Pose2& rMean,
Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch)
{
// 用前一帧优化前后的位姿变换，来校正当前帧优化前的位姿，得到一个初始位姿
Pose2 scanPose = pScan->GetSensorPose();
// 当前帧激光点数为零
if (pScan->GetNumberOfRangeReadings() == 0)
{
rMean = scanPose;
// 协方差越大，位姿的不确定度越大
rCovariance(0, 0) = MAX_VARIANCE;// XX
rCovariance(1, 1) = MAX_VARIANCE;// YY
rCovariance(2, 2) = 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue());// TH*TH
return 0.0;
}
// 2. get size of grid
Rectangle2<kt_int32s> roi = m_pCorrelationGrid->GetROI();
// 3. compute offset (in meters - lower left corner)
Vector2<kt_double> offset;
offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * m_pCorrelationGrid->GetResolution()));
offset.SetY(scanPose.GetY() - (0.5 * (roi.GetHeight() - 1) * m_pCorrelationGrid->GetResolution()));
// 4. set offset
m_pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset);
// 提取局部map中当前帧也可以看到的点，对应栅格设置为占用
AddScans(rBaseScans, scanPose.GetPosition());
// 搜索矩形框
Vector2<kt_double> searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight());
Vector2<kt_double> coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(),
0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution());
// 搜索步长，2个栅格
Vector2<kt_double> coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(),
2 * m_pCorrelationGrid->GetResolution());
// 基于相关方法的scan-to-map匹配
// 1、创建旋转角度-激光点旋转后相对于当前帧位置的偏移量
// 创建索引表，计算当前帧激光点，在各个旋转角度变换下，新的激光点位置相对于当前帧位置（机器人的位置）的坐标偏移量
// 不管当前帧在什么位置，只要旋转角度一定，那么坐标偏移量也是确定的。
// 目的是后面在搜索空间中对当前帧位姿施加不同的x平移、y平移之后再施加一个旋转的操作，不用重新计算每个搜索位置处旋转某个角度之后对应的点坐标，只需要加上对应旋转角度下算好的坐标偏移量即可。
// 2、遍历pose搜索空间计算响应值
// 1) 当前帧激光点集合在某个pose变换下得到新的位置，统计在局部map栅格中这些新位置处占用的数量，占用越多响应值越高，表示匹配越好
// 2) 惩罚一下搜索偏移量，越远响应值折扣越多（当前帧搜索距离、角度偏移）
// 3、找到响应值最大的pose，如果有多个最佳响应pose，那么对pose求个均值，得到最终pose
// 4、计算位姿的协方差
kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
m_pMapper->m_pCoarseSearchAngleOffset->GetValue(),
m_pMapper->m_pCoarseAngleResolution->GetValue(),
doPenalize, rMean, rCovariance, false);
if (m_pMapper->m_pUseResponseExpansion->GetValue() == true)
{
// 如果响应值是0，没匹配上，扩大角度搜索范围
if (math::DoubleEqual(bestResponse, 0.0))
{
// 扩大搜索角度范围，增加20°、40°、60°
kt_double newSearchAngleOffset = m_pMapper->m_pCoarseSearchAngleOffset->GetValue();
for (kt_int32u i = 0; i < 3; i++)
{
newSearchAngleOffset += math::DegreesToRadians(20);
// 再执行一次scan-to-map匹配
bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(),
doPenalize, rMean, rCovariance, false);
// 响应值还是0，退出
if (math::DoubleEqual(bestResponse, 0.0) == false)
{
break;
}
}
}
}
// 在scan-to-map优化之后的位姿基础上，再缩小搜索空间优化一次
if (doRefineMatch)
{
// 搜索范围减半，角度区间减半
Vector2<kt_double> fineSearchOffset(coarseSearchResolution * 0.5);
Vector2<kt_double> fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution());
// 执行一次scan-to-map匹配
bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution,
0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(),
m_pMapper->m_pFineSearchAngleOffset->GetValue(),
doPenalize, rMean, rCovariance, true);
}
assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
return bestResponse;
}scan-to-map匹配（ScanMatcher::CorrelateScan）
/**
* 基于相关方法的scan-to-map匹配
* 1、创建旋转角度-激光点旋转后相对于当前帧位置的偏移量
* 创建索引表，计算当前帧激光点，在各个旋转角度变换下，新的激光点位置相对于当前帧位置（机器人的位置）的坐标偏移量
* 不管当前帧在什么位置，只要旋转角度一定，那么坐标偏移量也是确定的。
* 目的是后面在搜索空间中对当前帧位姿施加不同的x平移、y平移之后再施加一个旋转的操作，不用重新计算每个搜索位置处旋转某个角度之后对应的点坐标，只需要加上对应旋转角度下算好的坐标偏移量即可。
* 2、遍历pose搜索空间计算响应值
* 1) 当前帧激光点集合在某个pose变换下得到新的位置，统计在局部map栅格中这些新位置处占用的数量，占用越多响应值越高，表示匹配越好
* 2) 惩罚一下搜索偏移量，越远响应值折扣越多（当前帧搜索距离、角度偏移）
* 3、找到响应值最大的pose，如果有多个最佳响应pose，那么对pose求个均值，得到最终pose
* 4、计算位姿的协方差
* @param pScan 当前帧
* @param rSearchCenter 当前帧位姿（搜索空间的中心）
* @param rSearchSpaceOffset 搜索范围，矩形半径
* @param rSearchSpaceResolution搜索步长
* @param searchAngleOffset 搜索角度范围
* @param searchAngleResolution 角度步长
* @param doPenalize 惩罚搜索距离较远的位姿，降低响应值
* @param rMean 输出匹配结果位姿
* @param rCovariance 输出匹配结果方差
* @param doingFineMatch 是否执行二次优化
* @return 返回响应值
*/
kt_double ScanMatcher::CorrelateScan(LocalizedRangeScan* pScan, const Pose2& rSearchCenter,
const Vector2<kt_double>& rSearchSpaceOffset,
const Vector2<kt_double>& rSearchSpaceResolution,
kt_double searchAngleOffset, kt_double searchAngleResolution,
kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch)
{
assert(searchAngleResolution != 0.0);
// 创建索引表，计算当前帧激光点，在各个旋转角度变换下，新的激光点位置相对于当前帧位置（机器人的位置）的坐标偏移量
// 不管当前帧在什么位置，只要旋转角度一定，那么坐标偏移量也是确定的。
// 目的是后面在搜索空间中对当前帧位姿施加不同的x平移、y平移之后再施加一个旋转的操作，不用重新计算每个搜索位置处旋转某个角度之后对应的点坐标，只需要加上对应旋转角度下算好的坐标偏移量即可。
m_pGridLookup->ComputeOffsets(pScan, rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution);
// only initialize probability grid if computing positional covariance (during coarse match)
if (!doingFineMatch)
{
m_pSearchSpaceProbs->Clear();
Vector2<kt_double> offset(rSearchCenter.GetPosition() - rSearchSpaceOffset);
m_pSearchSpaceProbs->GetCoordinateConverter()->SetOffset(offset);
}
std::vector<kt_double> xPoses;
// x轴搜索步数
kt_int32u nX = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetX() *
2.0 / rSearchSpaceResolution.GetX()) + 1);
// x轴搜索起始偏移
kt_double startX = -rSearchSpaceOffset.GetX();
// x轴搜索偏移集合
for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
{
xPoses.push_back(startX + xIndex * rSearchSpaceResolution.GetX());
}
assert(math::DoubleEqual(xPoses.back(), -startX));
std::vector<kt_double> yPoses;
// y轴搜索步数
kt_int32u nY = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetY() *
2.0 / rSearchSpaceResolution.GetY()) + 1);
// y轴搜索起始偏移
kt_double startY = -rSearchSpaceOffset.GetY();
// y轴搜索偏移集合
for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
{
yPoses.push_back(startY + yIndex * rSearchSpaceResolution.GetY());
}
assert(math::DoubleEqual(yPoses.back(), -startY));
// calculate pose response array size
// 搜索角度数量
kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1);
// 搜索空间数
kt_int32u poseResponseSize = static_cast<kt_int32u>(xPoses.size() * yPoses.size() * nAngles);
// allocate array
// 搜索空间
std::pair<kt_double, Pose2>* pPoseResponse = new std::pair<kt_double, Pose2>;
// 搜索起点
Vector2<kt_int32s> startGridPoint = m_pCorrelationGrid->WorldToGrid(Vector2<kt_double>(rSearchCenter.GetX()
+ startX, rSearchCenter.GetY() + startY));
// 按照y方向、x方向、角度，遍历搜索空间
kt_int32u poseResponseCounter = 0;
forEachAs(std::vector<kt_double>, &yPoses, yIter)
{
// y偏移量
kt_double y = *yIter;
// 当前帧偏移y的位置
kt_double newPositionY = rSearchCenter.GetY() + y;
// 搜索距离平方
kt_double squareY = math::Square(y);
forEachAs(std::vector<kt_double>, &xPoses, xIter)
{
// x偏移量
kt_double x = *xIter;
// 当前帧偏移x的位置
kt_double newPositionX = rSearchCenter.GetX() + x;
// 搜索距离平方
kt_double squareX = math::Square(x);
// 当前帧偏移(x,y)之后所在的网格点坐标，网格索引
Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(Vector2<kt_double>(newPositionX, newPositionY));
kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);
assert(gridIndex >= 0);
kt_double angle = 0.0;
// 起始角度，heading是0°朝向，searchAngleOffset是一半搜索角度偏移量
kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
{
// 角度
angle = startAngle + angleIndex * searchAngleResolution;
// 计算搜索pose的响应值
// 当前帧激光点集合在某个pose变换下得到新的位置，统计在局部map栅格中这些新位置处占用的数量，占用越多响应值越高，表示匹配越好
kt_double response = GetResponse(angleIndex, gridIndex);
// 惩罚一下搜索偏移量，越远响应值折扣越多（当前帧搜索距离、角度偏移）
if (doPenalize && (math::DoubleEqual(response, 0.0) == false))
{
// simple model (approximate Gaussian) to take odometry into account
// 距离平方
kt_double squaredDistance = squareX + squareY;
// 距离惩罚项
kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN *
squaredDistance / m_pMapper->m_pDistanceVariancePenalty->GetValue());
distancePenalty = math::Maximum(distancePenalty, m_pMapper->m_pMinimumDistancePenalty->GetValue());
// 角度惩罚项
kt_double squaredAngleDistance = math::Square(angle - rSearchCenter.GetHeading());
kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN *
squaredAngleDistance / m_pMapper->m_pAngleVariancePenalty->GetValue());
anglePenalty = math::Maximum(anglePenalty, m_pMapper->m_pMinimumAnglePenalty->GetValue());
// 响应打个折扣，距离、角度偏移越大，响应越小
// 意思是理论上纠正的位姿与当前的位姿不会变化很大，如果特别大就不太靠谱
response *= (distancePenalty * anglePenalty);
}
// 存响应值、新pose
pPoseResponse = std::pair<kt_double, Pose2>(response, Pose2(newPositionX, newPositionY,
math::NormalizeAngle(angle)));
poseResponseCounter++;
}
assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
}
}
assert(poseResponseSize == poseResponseCounter);
// find value of best response (in )
// 找到响应值最大的pose
kt_double bestResponse = -1;
for (kt_int32u i = 0; i < poseResponseSize; i++)
{
bestResponse = math::Maximum(bestResponse, pPoseResponse.first);
{
const Pose2& rPose = pPoseResponse.second;
Vector2<kt_int32s> grid = m_pSearchSpaceProbs->WorldToGrid(rPose.GetPosition());
// Changed (kt_double*) to the reinterpret_cast - Luc
kt_double* ptr = reinterpret_cast<kt_double*>(m_pSearchSpaceProbs->GetDataPointer(grid));
if (ptr == NULL)
{
throw std::runtime_error("Mapper FATAL ERROR - Index out of range in probability search!");
}
*ptr = math::Maximum(pPoseResponse.first, *ptr);
}
}
// 有多个最佳响应pose，那么对pose求个均值，得到最终pose
Vector2<kt_double> averagePosition;
kt_double thetaX = 0.0;
kt_double thetaY = 0.0;
kt_int32s averagePoseCount = 0;
for (kt_int32u i = 0; i < poseResponseSize; i++)
{
if (math::DoubleEqual(pPoseResponse.first, bestResponse))
{
averagePosition += pPoseResponse.second.GetPosition();
kt_double heading = pPoseResponse.second.GetHeading();
thetaX += cos(heading);
thetaY += sin(heading);
averagePoseCount++;
}
}
Pose2 averagePose;
if (averagePoseCount > 0)
{
averagePosition /= averagePoseCount;
thetaX /= averagePoseCount;
thetaY /= averagePoseCount;
averagePose = Pose2(averagePosition, atan2(thetaY, thetaX));
}
else
{
throw std::runtime_error("Mapper FATAL ERROR - Unable to find best position");
}
delete [] pPoseResponse;
if (!doingFineMatch)
{
// 计算新的位姿的协方差，位置
ComputePositionalCovariance(averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset,
rSearchSpaceResolution, searchAngleResolution, rCovariance);
}
else
{
// 计算新的位姿的协方差，角度
ComputeAngularCovariance(averagePose, bestResponse, rSearchCenter,
searchAngleOffset, searchAngleResolution, rCovariance);
}
// 最终pose
rMean = averagePose;
if (bestResponse > 1.0)
{
bestResponse = 1.0;
}
assert(math::InRange(bestResponse, 0.0, 1.0));
assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
// 最佳响应
return bestResponse;
}生成查找表
查找表存在的意义就是相比于暴力匹配，不要每次都重新计算每个激光数据信息，相同角度不同位置的激光数据信息只需要被索引一次。
http://pic2.zhimg.com/v2-ca9e050693d6caa3c6fff42e0c1b4f61_r.jpg
Karto slam源码阅读(三) 基于相关方法的scan-to-map匹配
计算响应（ScanMatcher::GetResponse）
/**
* 计算搜索pose的响应值
* 当前帧激光点集合在某个pose变换下得到新的位置，统计在局部map栅格中这些新位置处占用的数量，占用越多响应值越高，表示匹配越好
* @param angleIndex 旋转角度索引
* @param gridPositionIndex 新的激光帧网格位置索引
* @return 响应值
*/
kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const
{
kt_double response = 0.0;
// 局部map的栅格数据，存的是100占用，默认值0非占用，加上gridPositionIndex之后，pByte指向当前新的激光帧网格位置处
kt_int8u* pByte = m_pCorrelationGrid->GetDataPointer() + gridPositionIndex;
// 索引表内容为旋转角-旋转后激光点相对帧位置的坐标偏移量（转网格索引）之间的对应关系，取出当前旋转角下对应的偏移数据
const LookupArray* pOffsets = m_pGridLookup->GetLookupArray(angleIndex);
assert(pOffsets != NULL);
// 激光点数量
kt_int32u nPoints = pOffsets->GetSize();
if (nPoints == 0)
{
return response;
}
// calculate response
kt_int32s* pAngleIndexPointer = pOffsets->GetArrayPointer();
// 遍历激光点
for (kt_int32u i = 0; i < nPoints; i++)
{
// ignore points that fall off the grid
kt_int32s pointGridIndex = gridPositionIndex + pAngleIndexPointer;
if (!math::IsUpTo(pointGridIndex, m_pCorrelationGrid->GetDataSize()) || pAngleIndexPointer == INVALID_SCAN)
{
continue;
}
// pByte指向局部map栅格中新（经过平移）的激光帧位置，再加上pAngleIndexPointer是当前旋转之后新的激光点相对于激光帧位置的偏移量，
// 就等于新的激光点的位置，取它的栅格值。
// 也就是当前帧pose施加一个平移、旋转，激光点落在的栅格对应值相加，即为当前帧pose的响应值
// 100占用，0非占用。注：虽然定义了0未知、100占用、255free，但这三个是用于更新地图的，这里计算响应只用到100、0
response += pByte];
}
// 归一化，响应值越大越好，表示scan-to-map匹配越好
// 仅通过占用的数量就能判定匹配的好不好，这个在激光里面是合理的，因为激光打到物体表面上，占用点就是薄薄的一层，甚至1个像素，
// free点就特别多了，能匹配到占用点，说明很接近了。
// 如果点云是平面，那么匹配可以很好的纠正旋转，平移就不好说了；如果是墙角这种形状，旋转和平移都能纠正。
// 类似于VO里面的直接法，两帧图像直接用像素灰度值配准，当然它有很强的灰度不变假设。
// 上面仅是个人理解。
response /= (nPoints * GridStates_Occupied);
assert(fabs(response) <= 1.0);
return response;
}
页:
[1]