目錄
1 kt_bool Mapper::Process(LocalizedRangeScan* pScan)
3 kt_double ScanMatcher::MatchScan()
4 kt_double ScanMatcher::CorrelateScan()
4.2 kt_double ScanMatcher::GetResponse()
4.3 void ScanMatcher::ComputePositionalCovariance()
4.4 void ScanMatcher::ComputeAngularCovariance()
6 MapperSensorManager::AddScan()
7 void MapperSensorManager::AddRunningScan()
由於閱讀代碼的時間很短，很多地方還不懂，代碼裏的中文註釋大部分來自於
加入了詳細中文解釋的open_karto地址: https://github.com/kadn/open_karto
slam_karto的addScan()函數調用了Process()函數。slam_karto的代碼請看上一篇博文。
open_karto的一些數據類型介紹請看 karto探祕之open_karto 第一章 --- 數據結構。
1 kt_bool Mapper::Process(LocalizedRangeScan* pScan)
kt_bool Mapper::Process(LocalizedRangeScan* pScan)
{
// ...省略
Matrix3 covariance;
covariance.SetToIdentity();
// 如果當前幀是第一幀，則不執行MatchScan操作，否則執行。MatchScan是用來將當前幀和前面的幀（多個）進行比較，根據odometry給出的初始位姿，在初始位姿附近找到更合適的位姿作爲機器人移動位姿，同時返回該位姿下的response（衡量標準）以及covariance（可信度），具體內容在後面介紹
// correct scan (if not first scan)
if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
{
Pose2 bestPose;
m_pSequentialScanMatcher->MatchScan(pScan,
m_pMapperSensorManager->
GetRunningScans(pScan->GetSensorName()),
bestPose, // 求出的最優位姿
covariance);
pScan->SetSensorPose(bestPose);
}
// add scan to buffer and assign id 將當前幀加入數據庫中
m_pMapperSensorManager->AddScan(pScan);
if (m_pUseScanMatching->GetValue())
{
// add to graph
m_pGraph->AddVertex(pScan); // 將當前幀設置爲圖優化的頂點
m_pGraph->AddEdges(pScan, covariance); // 將當前幀執行 添加邊的操作
// 將當前幀添加到runningScan中，並進行維護. runningScan即是 掃描匹配 中被比較的對象，也即是“前面的幀”
m_pMapperSensorManager->AddRunningScan(pScan);
if (m_pDoLoopClosing->GetValue())
{
std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
const_forEach(std::vector<Name>, &deviceNames)
{
m_pGraph->TryCloseLoop(pScan, *iter); // 進行閉環檢測
}
}
}
m_pMapperSensorManager->SetLastScan(pScan);
return true;
}
return false;
}
3 kt_double ScanMatcher::MatchScan()
1次粗匹配(搜索步長爲兩倍的分辨率), 如果沒匹配到的話改變角度再進行一次粗匹配, 之後進行1次精細匹配(步長爲分辨率)
/**
* Match given scan against set of scans 在相關網格中找到最佳掃描姿勢
* @param pScan scan being scan-matched 當前雷達數據
* @param rBaseScans set of scans whose points will mark cells in grid as being occupied ，也就是 m_RunningScans
* @param rMean output parameter of mean (best pose) of match 輸出的位姿的平均值
* @param rCovariance output parameter of covariance of match 輸出位姿的協方差
* @param doPenalize default = true
whether to penalize matches further from the search center
* @param doRefineMatch default= true
whether to do finer-grained matching if coarse match is good (default is true)
* @return strength of response
*/
kt_double ScanMatcher::MatchScan(LocalizedRangeScan* pScan,
const LocalizedRangeScanVector& rBaseScans,
Pose2& rMean,
Matrix3& rCovariance,
kt_bool doPenalize,
kt_bool doRefineMatch)
{
///////////////////////////////////////
// set scan pose to be center of grid
// pScan還沒有更新父類LaserRangeScan中原始數據，掃描深度數據都在父類中，通過 pScan->GetRangeReadings()[iterNum]這種形式來訪問。
// 1. get scan position
Pose2 scanPose = pScan->GetSensorPose();
// scan has no readings; cannot do scan matching
// best guess of pose is based off of adjusted odometer reading
if (pScan->GetNumberOfRangeReadings() == 0)
{
rMean = scanPose;
// maximum covariance
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
// 這個是矩形柵格地圖，2431 * 2431的柵格（雷達範圍12, 之前設置過的 SetRangeThreshold) * 2 + 0.3m(滑窗範圍) 除以 分辨率 + 1
Rectangle2<kt_int32s> roi = m_pCorrelationGrid->GetROI();
// 3. compute offset (in meters - lower left corner)
Vector2<kt_double> offset; //offset是柵格左下角在世界座標系中的位置
// m_pCorrelationGrid->GetResolution() 柵格地圖一個格子在實際中的長度，單位：默認是 m
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);
///////////////////////////////////////
// set up correlation grid
// 這裏的scanPose.GetPosition()是 Lidar的位置，不是Lidar point的位置
// 將rBaseScans的點所對應的柵格置爲“occupied”，這就相當於存儲好了原始的數據，之後只要拿現在的和原始的柵格相比較就行了
// 獲得了Roi裏柵格狀態 //這個會對掃描點進行處理
AddScans(rBaseScans, scanPose.GetPosition());
// compute how far to search in each direction
// m_pSearchSpaceProbs 是一個 31 * 31 大小的柵格地圖
Vector2<kt_double> searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight());
// coarseSearchOffset 是一個座標，31 * 31 網格的中心 距離左下角的距離，單位m
Vector2<kt_double> coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(),
0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution());
// a coarse search only checks half the cells in each dimension
// 因爲每兩個格子掃描一下，所以認爲是粗略的
Vector2<kt_double> coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(),
2 * m_pCorrelationGrid->GetResolution());
// actual scan-matching //找到了 最好的位姿使得response最大
// 粗匹配 使用的位姿是 scanPose
// offset是coarseSearchOffset，搜索空間是 coarseSearchOffset * 2 除以分辨率，再加1，xy搜索分辨率爲2個柵格大小
// 角度搜索範圍 20度，裏邊會乘以2所以是40度，角度搜索分辨率爲2
kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
m_pMapper->m_pCoarseSearchAngleOffset->GetValue(),
m_pMapper->m_pCoarseAngleResolution->GetValue(),
doPenalize, rMean, rCovariance, false);
// 如果最初沒有找到合適的匹配項，是否增加 角度搜索空間，默認爲false
if (m_pMapper->m_pUseResponseExpansion->GetValue() == true)
{
if (math::DoubleEqual(bestResponse, 0.0))
{
// try and increase search angle offset with 20 degrees and do another match
kt_double newSearchAngleOffset = m_pMapper->m_pCoarseSearchAngleOffset->GetValue();
for (kt_int32u i = 0; i < 3; i++)
{
newSearchAngleOffset += math::DegreesToRadians(20);
bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(),
doPenalize, rMean, rCovariance, false);
if (math::DoubleEqual(bestResponse, 0.0) == false)
{
break;
}
}
}
}
if (doRefineMatch)
{
// fineSearchOffset 爲什麼是 coarseSearchResolution*0.5？ 等於一個柵格的分辨率？
Vector2<kt_double> fineSearchOffset(coarseSearchResolution * 0.5);
Vector2<kt_double> fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution());
// 精確匹配時 使用的位姿是 粗匹配 輸出的rMean
// 匹配範圍爲 3個的 柵格大小 fineSearchOffset * 2 除以分辨率 再 +1，xy搜索分辨率也爲1個柵格大小
// 角度搜索範圍 是粗匹配的一半，角度搜索分辨率爲0.2
bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution,
0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(),
m_pMapper->m_pFineSearchAngleOffset->GetValue(),
doPenalize, rMean, rCovariance, true); //這裏的true會讓程序計算角度方差
}
return bestResponse;
}
3.1 AddScans()
首先將 m_pCorrelationGrid 中柵格全置爲 0 ，
然後遍歷 running scna , 將每一幀雷達數據進行 AddScan()
確定是否是有效的點，首先判斷距離大於0.01，再判斷是否是同方向的，應該是按照右手定則判斷的，直接按公式求cos得正負，
然後將這些點對應的柵格地圖中的點 不是佔用狀態的 設置成 佔用狀態，同時，更新當前座標點周圍的佔用值, SmearPoint(gridPoint)
/**
* Marks cells where scans' points hit as being occupied
* @param rScans scans whose points will mark cells in grid as being occupied
* @param viewPoint do not add points that belong to scans "opposite" the view point
*/
void ScanMatcher::AddScans(const LocalizedRangeScanVector& rScans, Vector2<kt_double> viewPoint)
{
m_pCorrelationGrid->Clear(); //把柵格地圖的所有值(0,100,255)置爲0
// add all scans to grid
// rScans 可能是 running scans 或者 相鄰的scans 或者 閉環產生的scans
const_forEach(LocalizedRangeScanVector, &rScans)
{
// viewPoint 是當前幀的 在世界座標系中的位置，但是不包含laser朝向信息，
// 而 *iter都是存儲的之前的幀的掃描點在世界座標系中的位置
AddScan(*iter, viewPoint);
}
}
/**
* Marks cells where scans' points hit as being occupied. Can smear points as they are added.
* @param pScan scan whose points will mark cells in grid as being occupied
* @param viewPoint do not add points that belong to scans "opposite" the view point
* @param doSmear whether the points will be smeared
*/
void ScanMatcher::AddScan(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint, kt_bool doSmear)
{
PointVectorDouble validPoints = FindValidPoints(pScan, rViewPoint); //找到之前的scan在 rviewpoint這個位置上看來合適的點，具體條件見函數內部，記爲有效的點
// put in all valid points
const_forEach(PointVectorDouble, &validPoints)
{
Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(*iter); //把 point的位置變爲 在柵格地圖上的座標，柵格地圖的原點是左下角
// IsUpTo means <
if (!math::IsUpTo(gridPoint.GetX(), m_pCorrelationGrid->GetROI().GetWidth()) ||
!math::IsUpTo(gridPoint.GetY(), m_pCorrelationGrid->GetROI().GetHeight()))
{
// point not in grid
continue;
}
int gridIndex = m_pCorrelationGrid->GridIndex(gridPoint); //二維變爲一維索引
// set grid cell as occupied
if (m_pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied) //查詢grid cell狀態
{
// value already set
continue;
}
m_pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied; //在這裏將之前的數據幀的所有掃描點都在grid中更新狀態，用於之後的匹配
// smear grid
if (doSmear == true)
{
m_pCorrelationGrid->SmearPoint(gridPoint);
}
}
}
3.1.1 FindValidPoints（）
/**
* Compute which points in a scan are on the same side as the given viewpoint
* @param pScan
* @param rViewPoint
* @return points on the same side
*/
PointVectorDouble ScanMatcher::FindValidPoints(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint) const
{
//得到以前已經經過了轉換的點， 但是還需要判斷方向：順時針還是逆時針
const PointVectorDouble& rPointReadings = pScan->GetPointReadings();
// points must be at least 10 cm away when making comparisons of inside/outside of viewpoint
const kt_double minSquareDistance = math::Square(0.1); // in m^2
// this iterator lags from the main iterator adding points only when the points are on
// the same side as the viewpoint
PointVectorDouble::const_iterator trailingPointIter = rPointReadings.begin();
PointVectorDouble validPoints;
Vector2<kt_double> firstPoint;
kt_bool firstTime = true;
// 訪問一個幀的掃描點數據，這些數據的含義是掃描點在世界座標系中的位置
const_forEach(PointVectorDouble, &rPointReadings)
{
Vector2<kt_double> currentPoint = *iter;
if (firstTime && !std::isnan(currentPoint.GetX()) && !std::isnan(currentPoint.GetY()))
{
firstPoint = currentPoint;
firstTime = false;
}
Vector2<kt_double> delta = firstPoint - currentPoint;
if (delta.SquaredLength() > minSquareDistance)
{
// This compute the Determinant (viewPoint FirstPoint, viewPoint currentPoint)
// Which computes the direction of rotation, if the rotation is counterclock
// wise then we are looking at data we should keep. If it's negative rotation
// we should not included in in the matching
// have enough distance, check viewpoint
double a = rViewPoint.GetY() - firstPoint.GetY();
double b = firstPoint.GetX() - rViewPoint.GetX();
double c = firstPoint.GetY() * rViewPoint.GetX() - firstPoint.GetX() * rViewPoint.GetY();
double ss = currentPoint.GetX() * a + currentPoint.GetY() * b + c;
// reset beginning point
firstPoint = currentPoint;
if (ss < 0.0) // wrong side, skip and keep going
{
trailingPointIter = iter;
}
else
{
for (; trailingPointIter != iter; ++trailingPointIter)
{
// 將符合條件的（1.距離大於一定值 2.是正確的時針方向）掃描點加入validPoints之中
validPoints.push_back(*trailingPointIter);
}
}
}
}
return validPoints;
}
3.2 SmearPoint（）
大致上感覺是：通過一個 6 * 6 的核函數，將當前座標點附近 6*6 的點 做卷積，如果附近點的佔用值小於核函數中的佔用值，則將 核函數中的佔用值 賦值給 附近點的佔用值。
/**
* Smear cell if the cell at the given point is marked as "occupied"
* @param rGridPoint
*/
inline void SmearPoint(const Vector2<kt_int32s>& rGridPoint)
{
assert(m_pKernel != NULL);
int gridIndex = GridIndex(rGridPoint);
if (GetDataPointer()[gridIndex] != GridStates_Occupied)
{
return;
}
kt_int32s halfKernel = m_KernelSize / 2;
// apply kernel
for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
{
kt_int8u* pGridAdr = GetDataPointer(Vector2<kt_int32s>(rGridPoint.GetX(), rGridPoint.GetY() + j));
kt_int32s kernelConstant = (halfKernel) + m_KernelSize * (j + halfKernel);
// if a point is on the edge of the grid, there is no problem
// with running over the edge of allowable memory, because
// the grid has margins to compensate for the kernel size
for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
{
kt_int32s kernelArrayIndex = i + kernelConstant;
kt_int8u kernelValue = m_pKernel[kernelArrayIndex];
if (kernelValue > pGridAdr[i])
{
// kernel value is greater, so set it to kernel value
pGridAdr[i] = kernelValue;
}
}
}
}
/**
* Sets up the kernel for grid smearing.
*/
virtual void CalculateKernel()
核函數的佔用值爲；
kt_double distanceFromMean = hypot(i * resolution, j * resolution);
kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2));
kt_int32u kernelValue = static_cast<kt_int32u>(math::Round(z * GridStates_Occupied));
int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel);
m_pKernel[kernelArrayIndex] = static_cast<kt_int8u>(kernelValue);
4 kt_double ScanMatcher::CorrelateScan()
掃描匹配的實際處理函數
/**
* Finds the best pose for the scan centering the search in the correlation grid
* at the given pose and search in the space by the vector and angular offsets
* in increments of the given resolutions
* @param rScan scan to match against correlation grid
* @param rSearchCenter the center of the search space
* @param rSearchSpaceOffset searches poses in the area offset by this vector around search center
* @param rSearchSpaceResolution how fine a granularity to search in the search space
* @param searchAngleOffset searches poses in the angles offset by this angle around search center （20 degrees）
* @param searchAngleResolution how fine a granularity to search in the angular search space （2 degrees）
* @param doPenalize whether to penalize matches further from the search center
是否對距離搜索中心較遠的匹配進行處罰
* @param rMean output parameter of mean (best pose) of match
* @param rCovariance output parameter of covariance of match
* @param doingFineMatch whether to do a finer search after coarse search
* @return strength of response
*/
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)
{
//////////////////////////////////////////////////////////////////////////////
// 1. 製作角度查找表
assert(searchAngleResolution != 0.0);
// setup lookup arrays
// 這個函數把lookuparray的角度寫好了。每一個lookuparry存儲了每一個激光數據
// (總共361/修改之後爲180個)在角度滑窗之後的結果，還沒有xy的滑窗
m_pGridLookup->ComputeOffsets(pScan, rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution);
// only initialize probability grid if computing positional covariance (during coarse match)
if (!doingFineMatch)
{
m_pSearchSpaceProbs->Clear();
// position search grid - finds lower left corner of search grid
Vector2<kt_double> offset(rSearchCenter.GetPosition() - rSearchSpaceOffset);
m_pSearchSpaceProbs->GetCoordinateConverter()->SetOffset(offset);
}
//////////////////////////////////////////////////////////////////////////////
// 2. 求得搜索空間的個數，分別爲x y angle 的個數
// calculate position arrays
//以下計算出需要的各種x，y，angle用於三重循環 ，在這裏x有16個值，y有16個值，angle有21個值
std::vector<kt_double> xPoses;
kt_int32u nX = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetX() *
2.0 / rSearchSpaceResolution.GetX()) + 1);
kt_double startX = -rSearchSpaceOffset.GetX();
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;
kt_int32u nY = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetY() *
2.0 / rSearchSpaceResolution.GetY()) + 1);
kt_double startY = -rSearchSpaceOffset.GetY();
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 //Pose2包含了 x,y,theta. 滑窗的結果存儲在pPoseResponse中
std::pair<kt_double, Pose2>* pPoseResponse = new std::pair<kt_double, Pose2>[poseResponseSize];
// rSearchCenter 是搜索空間的中心
Vector2<kt_int32s> startGridPoint = m_pCorrelationGrid->WorldToGrid(Vector2<kt_double>(rSearchCenter.GetX() + startX,
rSearchCenter.GetY() + startY));
//////////////////////////////////////////////////////////////////////////////
// 3. 遍歷搜索空間，求得所有搜索項的響應值
// 在 xy範圍上進行遍歷，因爲 使用了角度的查找表，所以其實 遍歷了 x y theta， 最終找到最大的response
// 如果使用了 懲罰項，那麼公式變成了 response = k * response. 當 dx dy dtheta越大的時候， k越小
kt_int32u poseResponseCounter = 0;
forEachAs(std::vector<kt_double>, &yPoses, yIter)
{
kt_double y = *yIter;
kt_double newPositionY = rSearchCenter.GetY() + y;
kt_double squareY = math::Square(y);
forEachAs(std::vector<kt_double>, &xPoses, xIter)
{
kt_double x = *xIter;
kt_double newPositionX = rSearchCenter.GetX() + x;
kt_double squareX = math::Square(x);
//由 x,y得出一個 gridIndex
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;
kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
{
angle = startAngle + angleIndex * searchAngleResolution;
//由 angleIndex 和 gridIndex得出response
//將旋轉平移之後的這一幀數據和上一幀的數據進行匹配得分
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;
// DISTANCE_PENALTY_GAIN ： 0.2 ， DISTANCE_PENALTY_GAIN ：0.3
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());
// m_pAngleVariancePenalty 20角度變成弧度的平方
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);
}
// store response and 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);
//////////////////////////////////////////////////////////////////////////////
// 4. 在 所有搜索項的響應值 中尋找最大的響應值
// find value of best response (in [0; 1])
kt_double bestResponse = -1;
for (kt_int32u i = 0; i < poseResponseSize; i++)
{
bestResponse = math::Maximum(bestResponse, pPoseResponse[i].first);
// will compute positional covariance, save best relative probability for each cell
if (!doingFineMatch)
{
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);
}
}
//////////////////////////////////////////////////////////////////////////////
// 5. 最大的響應值(同爲最大)對應的位姿可能有很多種，求得 x y angle 的平均值
// average all poses with same highest response
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++)
{
// DoubleEqual: 在 正負KT_TOLERANCE = 1e-06 內範圍內的都是相等
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++; //對 x,y進行平均
}
}
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 pose response array
delete [] pPoseResponse;
#ifdef KARTO_DEBUG
std::cout << "bestPose: " << averagePose << std::endl;
std::cout << "bestResponse: " << bestResponse << std::endl;
#endif
//////////////////////////////////////////////////////////////////////////////
// 6. 粗匹配時計算位置協方差，精匹配時計算角度協方差
if (!doingFineMatch)
{ //計算位置協方差
ComputePositionalCovariance(averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset,
rSearchSpaceResolution, searchAngleResolution, rCovariance);
}
else
{
//角度上的協方差
ComputeAngularCovariance(averagePose, bestResponse, rSearchCenter,
searchAngleOffset, searchAngleResolution, rCovariance);
}
rMean = averagePose; // 最優位姿
#ifdef KARTO_DEBUG
std::cout << "bestPose: " << averagePose << std::endl;
#endif
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;
}
4.1 生成角度查找表
/**
* Compute lookup table of the points of the given scan for the given angular space
* @param pScan the scan
* @param angleCenter
* @param angleOffset computes lookup arrays for the angles within this offset around angleStart
* @param angleResolution how fine a granularity to compute lookup arrays in the angular space
*/
void ComputeOffsets(LocalizedRangeScan* pScan,
kt_double angleCenter,
kt_double angleOffset,
kt_double angleResolution)
{
assert(angleOffset != 0.0);
assert(angleResolution != 0.0);
// 角度搜索範圍：40度： 2 * 20
kt_int32u nAngles = static_cast<kt_int32u>(math::Round(angleOffset * 2.0 / angleResolution) + 1);
SetSize(nAngles);
// convert points into local coordinates of scan pose
// rPointReadings是激光數據的深度信息轉換爲在世界座標系中的二維座標
const PointVectorDouble& rPointReadings = pScan->GetPointReadings();
// compute transform to scan pose
Transform transform(pScan->GetSensorPose());
Pose2Vector localPoints; //localPoints的所有數據的角度是 0.0
const_forEach(PointVectorDouble, &rPointReadings)
{
// do inverse transform to get points in local coordinates 進行逆變換以獲取局部座標中的點
Pose2 vec = transform.InverseTransformPose(Pose2(*iter, 0.0));
localPoints.push_back(vec);
}
//////////////////////////////////////////////////////
// create lookup array for different angles
kt_double angle = 0.0;
kt_double startAngle = angleCenter - angleOffset;
for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
{
angle = startAngle + angleIndex * angleResolution;
ComputeOffsets(angleIndex, angle, localPoints, pScan);
}
}
/**
* Compute lookup value of points for given angle
* @param angleIndex
* @param angle
* @param rLocalPoints
*/
void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2Vector& rLocalPoints, LocalizedRangeScan* pScan)
{
// m_ppLookupArray 是個2維指針，存儲不同角度下的雷達數據
m_ppLookupArray[angleIndex]->SetSize(static_cast<kt_int32u>(rLocalPoints.size()));
m_Angles.at(angleIndex) = angle; // m_Angles 一維數組，只保存當前scan的角度
// set up point array by computing relative offsets to points readings
// when rotated by given angle
const Vector2<kt_double>& rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset();
kt_double cosine = cos(angle);
kt_double sine = sin(angle);
kt_int32u readingIndex = 0;
// 第i個角度的所有掃描數據的指針，每一個掃描數據又是一個uint數組，
// 這個uint數組的大小是LookupArray的m_Capacity， m_Capacity 的大小是rLocalPoints.size()
kt_int32s* pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer();
kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
const_forEach(Pose2Vector, &rLocalPoints)
{
const Vector2<kt_double>& rPosition = iter->GetPosition();
if (std::isnan(pScan->GetRangeReadings()[readingIndex]) ||
std::isinf(pScan->GetRangeReadings()[readingIndex]))
{
// const kt_int32s INVALID_SCAN = std::numeric_limits<kt_int32s>::max();
pAngleIndexPointer[readingIndex] = INVALID_SCAN;
readingIndex++;
continue;
}
// counterclockwise rotation and that rotation is about the origin (0, 0). 是關於原點（0，0）的 逆時針 旋轉。
// 將rPosition相對於當前幀的sensor這個點(比如在tutorial1中第一次來這裏時是1,1)逆時針旋轉angle度，得到offset
Vector2<kt_double> offset;
offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY());
offset.SetY(sine * rPosition.GetX() + cosine * rPosition.GetY());
// have to compensate for the grid offset when getting the grid index
// rPosition 旋轉後的座標，加上 地圖中心 與原點間的偏差？地圖就是 pCorrelationGrid
Vector2<kt_int32s> gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset);
// use base GridIndex to ignore ROI
// lookupIndex 就是 雷達點經過旋轉之後在地圖中的索引值
kt_int32s lookupIndex = m_pGrid->Grid<T>::GridIndex(gridPoint, false);
// pAngleIndexPointer = m_ppLookupArray[angleIndex]
// m_ppLookupArray[angleIndex][readingIndex] = = lookupIndex
// 所以一個角度值 angleIndex 對應的 查找表裏 存儲的是 一幀雷達數據 經過旋轉後在地圖中的索引值，
// 也就是 只算出了scan旋轉後的座標值，並沒有進行實際的雷達數據旋轉
pAngleIndexPointer[readingIndex] = lookupIndex;
readingIndex++;
}
assert(readingIndex == rLocalPoints.size());
}
4.2 kt_double ScanMatcher::GetResponse()
response : 響應值，就是對應的雷達點在地圖中的佔用值 的和 的平均值。
假設一幀雷達只有2個點，第一個點是佔用狀態，就是100，第二個點是unknow狀態，就是0，然後用（100 + 0）/ ( 2 * 100) = 0.5
來自於 https://blog.csdn.net/qq_24893115/article/details/52965410
/**
* Get response at given position for given rotation (only look up valid points)
* @param angleIndex
* @param gridPositionIndex
* @return response
*/
kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const
{
kt_double response = 0.0;
// add up value for each point
// 實際獲得了Grid<T>的m_pData， T* m_pData; 即 uchar* m_pData; 大小是m_WidthStep * m_Height，因此是柵格格子個數大小
kt_int8u* pByte = m_pCorrelationGrid->GetDataPointer() + gridPositionIndex;
// m_pGridLoopup存儲 nAngles個LookupArray指針，每一個角度索引 對應着一個 pOffsets，
// pOffsets 存儲了 scan 所有的數據點 經過旋轉後 的座標 對應着的 柵格的索引
const LookupArray* pOffsets = m_pGridLookup->GetLookupArray(angleIndex);
assert(pOffsets != NULL);
// get number of points in offset list，一幀scan有多少個點
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;
}
// uses index offsets to efficiently find location of point in the grid
// pAngleIndexPointer[i] 是這幀scan的第i個點經過旋轉後對應着的柵格地圖的索引
// pByte[pAngleIndexPointer[i]] 就是這個索引對應着的佔用值：
// GridStates_Unknown = 0, GridStates_Occupied = 100, GridStates_Free = 255
response += pByte[pAngleIndexPointer[i]];
}
// normalize response
// 歸一化爲什麼要除以 GridStates_Occupied，明明GridStates_Free 255 纔是最高的？
// 因爲mapper.cpp中只有佔用 和 free 兩種狀態，沒有給格子添加 free 狀態。
// free 狀態是在 OccupancyGrid 中設置的，OccupancyGrid 是在slam_karto中進行調用的
response /= (nPoints * GridStates_Occupied);
assert(fabs(response) <= 1.0);
return response;
}
4.3 void ScanMatcher::ComputePositionalCovariance()
圖片來自於 https://blog.csdn.net/qq_24893115/article/details/52965410
/**
* Computes the positional covariance of the best pose
* @param rBestPose
* @param bestResponse
* @param rSearchCenter
* @param rSearchSpaceOffset
* @param rSearchSpaceResolution
* @param searchAngleResolution
* @param rCovariance
*/
void ScanMatcher::ComputePositionalCovariance(const Pose2& rBestPose, kt_double bestResponse,
const Pose2& rSearchCenter,
const Vector2<kt_double>& rSearchSpaceOffset,
const Vector2<kt_double>& rSearchSpaceResolution,
kt_double searchAngleResolution, Matrix3& rCovariance)
{
// reset covariance to identity matrix
rCovariance.SetToIdentity();
// if best response is vary small return max variance
if (bestResponse < KT_TOLERANCE)
{
rCovariance(0, 0) = MAX_VARIANCE; // XX
rCovariance(1, 1) = MAX_VARIANCE; // YY
rCovariance(2, 2) = 4 * math::Square(searchAngleResolution); // TH*TH
return;
}
kt_double accumulatedVarianceXX = 0;
kt_double accumulatedVarianceXY = 0;
kt_double accumulatedVarianceYY = 0;
kt_double norm = 0;
kt_double dx = rBestPose.GetX() - rSearchCenter.GetX();
kt_double dy = rBestPose.GetY() - rSearchCenter.GetY();
kt_double offsetX = rSearchSpaceOffset.GetX();
kt_double offsetY = rSearchSpaceOffset.GetY();
kt_int32u nX = static_cast<kt_int32u>(math::Round(offsetX * 2.0 / rSearchSpaceResolution.GetX()) + 1);
kt_double startX = -offsetX;
assert(math::DoubleEqual(startX + (nX - 1) * rSearchSpaceResolution.GetX(), -startX));
kt_int32u nY = static_cast<kt_int32u>(math::Round(offsetY * 2.0 / rSearchSpaceResolution.GetY()) + 1);
kt_double startY = -offsetY;
assert(math::DoubleEqual(startY + (nY - 1) * rSearchSpaceResolution.GetY(), -startY));
for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
{
kt_double y = startY + yIndex * rSearchSpaceResolution.GetY();
for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
{
kt_double x = startX + xIndex * rSearchSpaceResolution.GetX();
Vector2<kt_int32s> gridPoint = m_pSearchSpaceProbs->WorldToGrid(Vector2<kt_double>(rSearchCenter.GetX() + x,
rSearchCenter.GetY() + y));
kt_double response = *(m_pSearchSpaceProbs->GetDataPointer(gridPoint));
// response is not a low response
if (response >= (bestResponse - 0.1))
{
norm += response;
accumulatedVarianceXX += (math::Square(x - dx) * response);
accumulatedVarianceXY += ((x - dx) * (y - dy) * response);
accumulatedVarianceYY += (math::Square(y - dy) * response);
}
}
}
if (norm > KT_TOLERANCE)
{
kt_double varianceXX = accumulatedVarianceXX / norm;
kt_double varianceXY = accumulatedVarianceXY / norm;
kt_double varianceYY = accumulatedVarianceYY / norm;
kt_double varianceTHTH = 4 * math::Square(searchAngleResolution);
// lower-bound variances so that they are not too small;
// ensures that links are not too tight
kt_double minVarianceXX = 0.1 * math::Square(rSearchSpaceResolution.GetX());
kt_double minVarianceYY = 0.1 * math::Square(rSearchSpaceResolution.GetY());
varianceXX = math::Maximum(varianceXX, minVarianceXX);
varianceYY = math::Maximum(varianceYY, minVarianceYY);
// increase variance for poorer responses
kt_double multiplier = 1.0 / bestResponse;
rCovariance(0, 0) = varianceXX * multiplier;
rCovariance(0, 1) = varianceXY * multiplier;
rCovariance(1, 0) = varianceXY * multiplier;
rCovariance(1, 1) = varianceYY * multiplier;
rCovariance(2, 2) = varianceTHTH; // this value will be set in ComputeAngularCovariance
}
// if values are 0, set to MAX_VARIANCE
// values might be 0 if points are too sparse and thus don't hit other points
if (math::DoubleEqual(rCovariance(0, 0), 0.0))
{
rCovariance(0, 0) = MAX_VARIANCE;
}
if (math::DoubleEqual(rCovariance(1, 1), 0.0))
{
rCovariance(1, 1) = MAX_VARIANCE;
}
}
4.3 void ScanMatcher::ComputeAngularCovariance()
/**
* Computes the angular covariance of the best pose
* @param rBestPose
* @param bestResponse
* @param rSearchCenter
* @param rSearchAngleOffset
* @param searchAngleResolution
* @param rCovariance
*/
void ScanMatcher::ComputeAngularCovariance(const Pose2& rBestPose,
kt_double bestResponse,
const Pose2& rSearchCenter,
kt_double searchAngleOffset,
kt_double searchAngleResolution,
Matrix3& rCovariance)
{
// NOTE: do not reset covariance matrix
// normalize angle difference
kt_double bestAngle = math::NormalizeAngleDifference(rBestPose.GetHeading(), rSearchCenter.GetHeading());
Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(rBestPose.GetPosition());
kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);
kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2 / searchAngleResolution) + 1);
kt_double angle = 0.0;
kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
kt_double norm = 0.0;
kt_double accumulatedVarianceThTh = 0.0;
for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
{
angle = startAngle + angleIndex * searchAngleResolution;
kt_double response = GetResponse(angleIndex, gridIndex);
// response is not a low response
if (response >= (bestResponse - 0.1))
{
norm += response;
accumulatedVarianceThTh += (math::Square(angle - bestAngle) * response);
}
}
assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
if (norm > KT_TOLERANCE)
{
if (accumulatedVarianceThTh < KT_TOLERANCE)
{
accumulatedVarianceThTh = math::Square(searchAngleResolution);
}
accumulatedVarianceThTh /= norm;
}
else
{
accumulatedVarianceThTh = 1000 * math::Square(searchAngleResolution);
}
rCovariance(2, 2) = accumulatedVarianceThTh;
}
5 void SetSensorPose()
更新 m_CorrectedPose ：車 相對 世界座標系 的位置及角度。以及當前雷達在世界座標系的位置
/**
* Computes the robot pose given the corrected scan pose
* @param rScanPose pose of the sensor
*/
void SetSensorPose(const Pose2& rScanPose)
{
Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose(); //這是sensor相對於車中心的座標
kt_double offsetLength = deviceOffsetPose2.GetPosition().Length();
kt_double offsetHeading = deviceOffsetPose2.GetHeading(); //這是sensor相對於車座標系的角度
kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX());
kt_double correctedHeading = math::NormalizeAngle(rScanPose.GetHeading());
// worldSensorOffset 是"scan相對於車的偏移"在世界座標系中的位置
//先假設angleoffset是0，那麼correctHeading減去offsetHeading是 offset相對於世界座標系的角度。
//假設 offsetLength是1， correctedHeading 是pi， offsetHeading是 pi/2， 那麼
//offsetLength * cos(correctedHeading + angleoffset - offsetHeading) = 0
//offsetLength * sin(correctedHeading + angleoffset - offsetHeading) = 1
// angleoffset 的含義與在最初設定offset時有關，且可以認爲這裏的angleoffset的符號寫反了，
// offset 的heading 的含義如果是相對於車座標系而言的，那麼就沒有必要使用 angleoffset， 如果是相對於 "以車原點爲圓心，laser到圓心爲x軸的座標系"，
// 那麼也應該用 correctedHeading - angleoffset - offsetHeading。
Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading),
offsetLength * sin(correctedHeading + angleoffset - offsetHeading),
offsetHeading);
// rScanPose是 scan在世界座標系的位置， worldSensorOffset是 "scan相對於車的偏移"在世界座標系中的位置，相減之後就是車相對世界座標系的位置
// 這裏的相減 會讓角度也相減，也就是 correctedHeading - offsetHeading， 這樣就得到了車的角度相對於世界座標系的角度
m_CorrectedPose = rScanPose - worldSensorOffset;
Update();
}
/**
* Compute point readings based on range readings
* Only range readings within [minimum range; range threshold] are returned
*/
virtual void Update()
{
LaserRangeFinder* pLaserRangeFinder = GetLaserRangeFinder();
if (pLaserRangeFinder != NULL)
{
m_PointReadings.clear();
m_UnfilteredPointReadings.clear();
kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold();
kt_double minimumAngle = pLaserRangeFinder->GetMinimumAngle();
kt_double angularResolution = pLaserRangeFinder->GetAngularResolution();
Pose2 scanPose = GetSensorPose();
// compute point readings
Vector2<kt_double> rangePointsSum;
kt_int32u beamNum = 0;
for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++)
{
kt_double rangeReading = GetRangeReadings()[i];
if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold))
{
kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
//將laser scan的讀入數據轉換成了在地圖上的座標
Vector2<kt_double> point;
point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
m_UnfilteredPointReadings.push_back(point);
continue;
}
kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
//將laser scan的讀入數據轉換成了在地圖上的座標
Vector2<kt_double> point;
point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
m_PointReadings.push_back(point);
m_UnfilteredPointReadings.push_back(point);
//將laser scan掃描到的點的世界座標加在一起，然後求取平均值，就是m_BarycenterPose的結果
rangePointsSum += point;
}
// compute barycenter
kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
if (nPoints != 0.0)
{
Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
m_BarycenterPose = Pose2(averagePosition, 0.0);
}
else
{
m_BarycenterPose = scanPose;
}
// calculate bounding box of scan
m_BoundingBox = BoundingBox2();
//m_BoundingBox本身是沒有範圍的，通過Add函數得出一個能把這個點框進去的範圍，總共有nPoints個點，那麼就形成了一個剛好將m_PointReadings都能包含進去的BoundingBox
m_BoundingBox.Add(scanPose.GetPosition());
forEach(PointVectorDouble, &m_PointReadings)
{
m_BoundingBox.Add(*iter); //Add函數會不斷地改變 m_BoundingBox的包含範圍
}
}
m_IsDirty = false;
}
6 MapperSensorManager::AddScan()
/**
* Adds scan to scan vector of device that recorded scan
* @param pScan
*/
void MapperSensorManager::AddScan(LocalizedRangeScan* pScan)
{
GetScanManager(pScan)->AddScan(pScan, m_NextScanId); //這個是在ScanManager 類中加入了 pScan
m_Scans.push_back(pScan); //這個是在當前的MapperSensorManager 中加入了 pScan，兩者有關係
m_NextScanId++; //MapperSensorManager has a "typedef std::map<Name, ScanManager*> ScanManagerMap"
}
class ScanManager
{
/**
* Adds scan to vector of processed scans tagging scan with given unique id
* @param pScan
*/
inline void AddScan(LocalizedRangeScan* pScan, kt_int32s uniqueId)
{
// assign state id to scan
pScan->SetStateId(static_cast<kt_int32u>(m_Scans.size()));
// assign unique id to scan
pScan->SetUniqueId(uniqueId);
// add it to scan buffer
m_Scans.push_back(pScan);
}
}
7 void MapperSensorManager::AddRunningScan()
/**
* Adds scan to running scans of device that recorded scan
* @param pScan
*/
inline void MapperSensorManager::AddRunningScan(LocalizedRangeScan* pScan)
{
GetScanManager(pScan)->AddRunningScan(pScan);
}
/**
* Adds scan to vector of running scans
* @param pScan
*/
void AddRunningScan(LocalizedRangeScan* pScan)
{
m_RunningScans.push_back(pScan);
// vector has at least one element (first line of this function), so this is valid
Pose2 frontScanPose = m_RunningScans.front()->GetSensorPose();
Pose2 backScanPose = m_RunningScans.back()->GetSensorPose();
// cap vector size and remove all scans from front of vector that are too far from end of vector
//進行 runningScan的維護，數量上的維護以及第一個與最後一個距離之間的維護
kt_double squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
while (m_RunningScans.size() > m_RunningBufferMaximumSize ||
squaredDistance > math::Square(m_RunningBufferMaximumDistance) - KT_TOLERANCE)
{
// remove front of running scans
m_RunningScans.erase(m_RunningScans.begin());
// recompute stats of running scans
frontScanPose = m_RunningScans.front()->GetSensorPose();
backScanPose = m_RunningScans.back()->GetSensorPose();
squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
}
}
總結：
process 的過程
本節介紹了掃描匹配的大致流程，其中涉及各個函數的初始化部分及參數部分可以參照 karto探祕之open_karto 第一章 和 karto探祕之open_karto 第二章 --- 參數解析。
1 通過上一幀雷達數據的座標，加上上一幀雷達到當前幀雷達的odom變化，得到當前幀的初始位姿
2 判斷 scan 滿足 時間，距離，角度 的條件，則是一個有效的點，將這個點進行 掃描匹配，求出最優的位姿。
3 將這幀雷達加入到 running scan 中
4 將將這幀雷達加入到 圖結構 中，添加頂點，與邊，進行迴環檢測與整體圖優化。
掃描匹配 的過程
1 首先獲取當前幀的初始位姿，通過里程計得到的猜測，
2 roi 就是 m_pCorrelationGrid 的區域，大小爲 2431 * 2431 的柵格，也就是前後左右12米的範圍，又加了0.3m的 核函數的邊界。這個區域是隨着雷達的移動不斷更新的，通過 running scan 進行更新，running scan 不包括當前的scan。karto的掃描匹配只維護這麼大的一個網格，所以消耗的內存和cpu很小
所以掃描匹配是當前scan與過去的一段時間內scan形成的柵格地圖間的匹配。
3 地圖的offset 是當前scan 與 m_pCorrelationGrid 地圖中心的 座標偏差
4 通過AddScans() 將m_pCorrelationGrid中 的 雷達點上佔據的柵格設置爲 佔用狀態，首先是將m_pCorrelationGrid的柵格值全設置爲0，然後將 pScan 進行篩選，距離與方向上的篩選。篩選之後將這些雷達點對應的柵格設置爲佔用狀態，同時使用smear將雷達點周圍的柵格使用核函數進行更新一下。
5 設置搜索範圍與搜索分辨率，粗匹配時計算位置協方差；使用粗匹配輸出的座標均值 再次進行精匹配，設置搜索範圍與分辨率，精匹配時計算角度協方差。
6 響應值的計算：就是當前scan對應在柵格地圖中的格子的佔用值的平均值，如果這幀scan對應所有格子都爲佔用，則相應值爲1，格子目前只有 GridStates_Unknown = 0，GridStates_Occupied = 100 2種狀態，所以歸一化時除以的是GridStates_Occupied 。free 狀態實在OccupiedGrid中設置的，OccupiedGrid類在slam_karto中進行調用。
7 角度查找表：將當前scan按照不同角度進行旋轉，每個旋轉角度對應一個角度查找表。一個角度查找表中存儲的是當前scan的每一個數據點 經過旋轉後對應着柵格地圖中格子的索引值，一個角度查找表存儲了當前scan經過旋轉後的索引值，這裏並不是真正的將scan進行旋轉了，只是記錄下旋轉後的對應的格子的索引，用於求取響應值。
REFERENCES
Karto SLAM之open_karto代碼學習筆記（一） https://blog.csdn.net/wphkadn/article/details/85144186
Karto SLAM之open_karto代碼學習筆記（二） https://blog.csdn.net/wphkadn/article/details/90247146
Karto_slam框架與代碼解析 https://blog.csdn.net/qq_24893115/article/details/52965410
加入了詳細中文解釋的open_karto地址 https://github.com/kadn/open_karto
SLAM_Karto 學習（四） 深入理解 ScanMatch 過程 https://blog.csdn.net/Fourier_Legend/article/details/81558886?utm_source=blogxgwz0#process%E5%87%BD%E6%95%B0uml