Karto 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>[poseResponseSize];
- // 搜索起点
- 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[poseResponseCounter] = 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 [0; 1])
- // 找到响应值最大的pose
- kt_double bestResponse = -1;
- for (kt_int32u i = 0; i < poseResponseSize; i++)
- {
- bestResponse = math::Maximum(bestResponse, pPoseResponse[i].first);
- {
- const Pose2& rPose = pPoseResponse[i].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[i].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[i].first, bestResponse))
- {
- averagePosition += pPoseResponse[i].second.GetPosition();
- kt_double heading = pPoseResponse[i].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;
- }
复制代码 生成查找表
查找表存在的意义就是相比于暴力匹配,不要每次都重新计算每个激光数据信息,相同角度不同位置的激光数据信息只需要被索引一次。
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[i];
- if (!math::IsUpTo(pointGridIndex, m_pCorrelationGrid->GetDataSize()) || pAngleIndexPointer[i] == INVALID_SCAN)
- {
- continue;
- }
- // pByte指向局部map栅格中新(经过平移)的激光帧位置,再加上pAngleIndexPointer[i]是当前旋转之后新的激光点相对于激光帧位置的偏移量,
- // 就等于新的激光点的位置,取它的栅格值。
- // 也就是当前帧pose施加一个平移、旋转,激光点落在的栅格对应值相加,即为当前帧pose的响应值
- // 100占用,0非占用。注:虽然定义了0未知、100占用、255free,但这三个是用于更新地图的,这里计算响应只用到100、0
- response += pByte[pAngleIndexPointer[i]];
- }
- // 归一化[0,1],响应值越大越好,表示scan-to-map匹配越好
- // 仅通过占用的数量就能判定匹配的好不好,这个在激光里面是合理的,因为激光打到物体表面上,占用点就是薄薄的一层,甚至1个像素,
- // free点就特别多了,能匹配到占用点,说明很接近了。
- // 如果点云是平面,那么匹配可以很好的纠正旋转,平移就不好说了;如果是墙角这种形状,旋转和平移都能纠正。
- // 类似于VO里面的直接法,两帧图像直接用像素灰度值配准,当然它有很强的灰度不变假设。
- // 上面仅是个人理解。
- response /= (nPoints * GridStates_Occupied);
- assert(fabs(response) <= 1.0);
- return response;
- }
复制代码 |