karto探秘之open_karto 第四章 --- 回环检测与后端优化
1 Mapper::Process()
主函数中与后端优化和回环检测相关的代码如下所示:
kt_bool Mapper::Process(LocalizedRangeScan* pScan)
{
// 省略...
if (m_pUseScanMatching->GetValue())
{
// add to graph
m_pGraph->AddVertex(pScan);
//边是具有约束关系的两个顶点，在 AddEdges中的操作有： 寻找可以连接的两个帧，计算scanMatch，同时保存scanMatch的结果到Graph之中，然后添加到SPA里面
m_pGraph->AddEdges(pScan, covariance);
m_pMapperSensorManager->AddRunningScan(pScan); // 这里面有RunningScan的维护，即删除不合适的头
//进行回环检测的步骤
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); //每一次的 LastScan都是一个正常的pScan，而不是空的
return true;
}
return false;
}
进行完扫描匹配之后,得到了当前scan的最优位姿. 之后向图结构中添加顶点和边, 再之后是进行回环检测.
2 MapperGraph::AddVertex()
之后,进行后端的第一步,将当前scan作为节点添加到图结构中.
这里的图结构有2个,
一个是 karto里的Graph类, 用于查找回环时,在这个图上做广度优先搜索.
这里的Graph::AddVertex() ，指的是调用MapperGraph的基类Graph 的AddVertex()函数，进行节点的保存．
第二个就是稀疏位姿图spa中的图结构了, 用于做全局位姿优化.
/**
* Adds a vertex representing the given scan to the graph
* @param pScan
*/
void MapperGraph::AddVertex(LocalizedRangeScan* pScan)
{
assert(pScan);
if (pScan != NULL)
{
// 将当前scan转换成 vertex
Vertex<LocalizedRangeScan>* pVertex = new Vertex<LocalizedRangeScan>(pScan);
// 将vertex加入Graph图结构中
Graph<LocalizedRangeScan>::AddVertex(pScan->GetSensorName(), pVertex);
if (m_pMapper->m_pScanOptimizer != NULL)
{
//使用SPA进行优化，将节点添加进 的求解类型中
m_pMapper->m_pScanOptimizer->AddNode(pVertex);
}
}
}
// spa_solver.cpp
void SpaSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex)
{
karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
m_Spa.addNode(vector, pVertex->GetObject()->GetUniqueId());
}
3 MapperGraph::AddEdges()
// 向图中添加边结构
void MapperGraph::AddEdges(LocalizedRangeScan *pScan, const Matrix3 &rCovariance)
{
MapperSensorManager *pSensorManager = m_pMapper->m_pMapperSensorManager;
const Name &rSensorName = pScan->GetSensorName();
// link to previous scan
kt_int32s previousScanNum = pScan->GetStateId() - 1;
if (pSensorManager->GetLastScan(rSensorName) != NULL)
{
assert(previousScanNum >= 0);
// 添加边的第一种方式，总共有三种方式
// 第一种 将前一帧雷达数据与当前雷达数据连接，添加边约束，添加1条边
LinkScans(pSensorManager->GetScan(rSensorName, previousScanNum), pScan, pScan->GetSensorPose(), rCovariance);
}
Pose2Vector means;
std::vector<Matrix3> covariances;
// first scan (link to first scan of other robots)
// 如果是第一帧雷达数据
if (pSensorManager->GetLastScan(rSensorName) == NULL)
{
assert(pSensorManager->GetScans(rSensorName).size() == 1);
std::vector<Name> deviceNames = pSensorManager->GetSensorNames();
// 看看是不是还有其他的设备在传输 scan，对于单一的雷达，进不去循环
forEach(std::vector<Name>, &deviceNames)
{
const Name &rCandidateSensorName = *iter;
// skip if candidate device is the same or other device has no scans
if ((rCandidateSensorName == rSensorName) || (pSensorManager->GetScans(rCandidateSensorName).empty()))
{
continue;
}
Pose2 bestPose;
Matrix3 covariance;
kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan,
pSensorManager->GetScans(rCandidateSensorName),
bestPose, covariance);
LinkScans(pSensorManager->GetScan(rCandidateSensorName, 0), pScan, bestPose, covariance);
// only add to means and covariances if response was high "enough"
if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue())
{
means.push_back(bestPose);
covariances.push_back(covariance);
}
}
}
else
{
// link to running scans
// 第二种 将当前scan 与 running scans 添加边，running scans 就是最近的70个scan，所有的running scans只添加1条边
Pose2 scanPose = pScan->GetSensorPose();
means.push_back(scanPose);
covariances.push_back(rCovariance);
LinkChainToScan(pSensorManager->GetRunningScans(rSensorName), pScan, scanPose, rCovariance);
}
// link to other near chains (chains that include new scan are invalid)
// 第三种 将当前scan 与 NearChains scans 添加边
LinkNearChains(pScan, means, covariances);
if (!means.empty())
{
pScan->SetSensorPose(ComputeWeightedMean(means, covariances));
}
}
向位姿图中添加边一共存在三种方式:
第一种: LinkScans()
将前一帧雷达数据与当前雷达数据连接，添加边约束. 调用一次SpaSolver::AddConstraint().
第二种 LinkChainToScan()
karto中将 一定范围内的 多个连续的 scan 称为一条chain, 中文不好翻译…就直接这么叫吧.
running scans 是在当前scan之前的连续的20m范围内的scan.
将running scans 作为一条chain, 将当前scan与这个chain添加一条边, 在所有的running scans中找到距离当前scan最近的一个scan, 再调用 LinkScans() 将这两个scan添加一条边.
第三种 LinkNearChains()
首先Graph图结构中搜索与当前scan的距离在一定阈值范围内的所有的scan, 这些scan被karto称为near scan .
然后根据near scan的index, 为每一个near scan 扩充成为一条chain, 以index增加 和 index减小 两个方向将 near scan 扩充成为一条chain. 同时, 这条chain 不可以包含当前的scan, 这种chain被成为 near chain.
3.1 将当前scan与前一帧scan添加边
3.1.1 MapperGraph::LinkScans()
// 将前一帧雷达数据与当前雷达数据连接，添加边约束，添加1条边
void MapperGraph::LinkScans(LocalizedRangeScan *pFromScan, LocalizedRangeScan *pToScan,
const Pose2 &rMean, const Matrix3 &rCovariance)
{
kt_bool isNewEdge = true;
// 将source与target连接起来，或者说将pFromScan与pToScan连接起来
Edge<LocalizedRangeScan> *pEdge = AddEdge(pFromScan, pToScan, isNewEdge);
// only attach link information if the edge is new
if (isNewEdge == true)
{
pEdge->SetLabel(new LinkInfo(pFromScan->GetSensorPose(), rMean, rCovariance));
if (m_pMapper->m_pScanOptimizer != NULL)
{
m_pMapper->m_pScanOptimizer->AddConstraint(pEdge);
}
}
}
3.1.2 SpaSolver::AddConstraint()
void SpaSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge)
{
karto::LocalizedRangeScan *pSource = pEdge->GetSource()->GetObject();
karto::LocalizedRangeScan *pTarget = pEdge->GetTarget()->GetObject();
karto::LinkInfo *pLinkInfo = (karto::LinkInfo *)(pEdge->GetLabel());
// 两个位姿间的坐标变换
karto::Pose2 diff = pLinkInfo->GetPoseDifference();
Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());
// 信息矩阵, 协方差矩阵的逆
karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
Eigen::Matrix<double, 3, 3> m;
m(0, 0) = precisionMatrix(0, 0);
m(0, 1) = m(1, 0) = precisionMatrix(0, 1);
m(0, 2) = m(2, 0) = precisionMatrix(0, 2);
m(1, 1) = precisionMatrix(1, 1);
m(1, 2) = m(2, 1) = precisionMatrix(1, 2);
m(2, 2) = precisionMatrix(2, 2);
m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
}
3.2 将当前scan与 running scans 添加边：MapperGraph::LinkChainToScan()
// 将当前scan 与 scanchain 添加边，在所有的数据中找到和当前scan距离最近的一个scan,与当前scan添加一条边
void MapperGraph::LinkChainToScan(const LocalizedRangeScanVector &rChain, LocalizedRangeScan *pScan,
const Pose2 &rMean, const Matrix3 &rCovariance)
{
Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
LocalizedRangeScan *pClosestScan = GetClosestScanToPose(rChain, pose);
assert(pClosestScan != NULL);
Pose2 closestScanPose = pClosestScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
kt_double squaredDistance = pose.GetPosition().SquaredDistance(closestScanPose.GetPosition());
if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
{
LinkScans(pClosestScan, pScan, rMean, rCovariance);
}
}
3.3 将当前scan与 near chains 添加边：MapperGraph::LinkNearChains()
3.3.1 LinkNearChains()
LinkNearChains() 函数首先调用 FindNearChains() 获取 near chain .
之后再对每一个有效的near chain 与 当前scan 添加边, 每一个near chain 会调用一次 LinkChainToScan().
这其实也算是个回环检测, 只不过这个搜索的范围会小一些.
// 将满足要求的near chain在图结构中添加一条边
void MapperGraph::LinkNearChains(LocalizedRangeScan *pScan, Pose2Vector &rMeans, std::vector<Matrix3> &rCovariances)
{
// 找到满足要求的 near chain
const std::vector<LocalizedRangeScanVector> nearChains = FindNearChains(pScan);
const_forEach(std::vector<LocalizedRangeScanVector>, &nearChains)
{
// chain中scan的个数要满足要求
if (iter->size() < m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
{
continue;
}
Pose2 mean;
Matrix3 covariance;
// match scan against "near" chain
// 将当前scan 对 所有的chain 进行匹配,匹配得分大于阈值就将这条chain与scan添加一个边
kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false);
if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE)
{
rMeans.push_back(mean);
rCovariances.push_back(covariance);
// 得分满足要求就在图结构中 将这个chain添加一条边
LinkChainToScan(*iter, pScan, mean, covariance);
}
}
}
3.3.2 FindNearChains()
FindNearChains() 函数调用FindNearLinkedScans() 来获取 near scan, 再对所有的near scan 扩充成为 near chain , 就会得到很多个 near chain , 之后通过将当前scan与这个chain进行扫描匹配, 计算下匹配得分, 如果匹配得分高于阈值就说明是个有效的 near chain.
// 找到不包含当前scan的一系列near chain
std::vector<LocalizedRangeScanVector> MapperGraph::FindNearChains(LocalizedRangeScan *pScan)
{
std::vector<LocalizedRangeScanVector> nearChains;
Pose2 scanPose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
// to keep track of which scans have been added to a chain
LocalizedRangeScanVector processed;
// 在pscan周围查找可以链接的scan
// m_pMapper->m_pLinkScanMaximumDistance 为1.5m
const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan,
m_pMapper->m_pLinkScanMaximumDistance->GetValue());
// 利用每一个 LinkedScans 来扩充成一个chain， chain要满足不包含pscan的条件
const_forEach(LocalizedRangeScanVector, &nearLinkedScans)
{
LocalizedRangeScan *pNearScan = *iter;
if (pNearScan == pScan)
{
continue;
}
// scan has already been processed, skip
// scan已经被添加过了就跳过
if (find(processed.begin(), processed.end(), pNearScan) != processed.end())
{
continue;
}
processed.push_back(pNearScan);
// build up chain
kt_bool isValidChain = true;
std::list<LocalizedRangeScan *> chain;
// add scans before current scan being processed
// 从当前scan的index-1开始遍历,直到index=0
for (kt_int32s candidateScanNum = pNearScan->GetStateId() - 1; candidateScanNum >= 0; candidateScanNum--)
{
LocalizedRangeScan *pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(),
candidateScanNum);
// chain is invalid--contains scan being added
// 包含当前scan的chain不能用
if (pCandidateScan == pScan)
{
isValidChain = false;
}
Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
{
chain.push_front(pCandidateScan);
processed.push_back(pCandidateScan);
}
else
{
break;
}
}
// 将当前scan加入到chain中
chain.push_back(pNearScan);
// add scans after current scan being processed
// 从当前scan的index+1开始遍历,直到index=end
kt_int32u end = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(pNearScan->GetSensorName()).size());
for (kt_int32u candidateScanNum = pNearScan->GetStateId() + 1; candidateScanNum < end; candidateScanNum++)
{
LocalizedRangeScan *pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(),
candidateScanNum);
// 包含当前scan的chain不能用
if (pCandidateScan == pScan)
{
isValidChain = false;
}
Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
{
chain.push_back(pCandidateScan);
processed.push_back(pCandidateScan);
}
else
{
break;
}
}
if (isValidChain)
{
// change list to vector
LocalizedRangeScanVector tempChain;
std::copy(chain.begin(), chain.end(), std::inserter(tempChain, tempChain.begin()));
// add chain to collection
nearChains.push_back(tempChain);
}
}
return nearChains;
}
3.3.3 FindNearLinkedScans()
// 根据传入不同的搜索距离,在一定范围内来寻找 NearLinkedScans
LocalizedRangeScanVector MapperGraph::FindNearLinkedScans(LocalizedRangeScan *pScan, kt_double maxDistance)
{
// 在pScan周围寻找 NearLinkedScans
NearScanVisitor *pVisitor = new NearScanVisitor(pScan, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue());
LocalizedRangeScanVector nearLinkedScans = m_pTraversal->Traverse(GetVertex(pScan), pVisitor);
delete pVisitor;
return nearLinkedScans;
}
3.3.4 广度优先搜索算法 Traverse()
添加边时的LinkNearChains()函数 与 回环检测时的 FindPossibleLoopClosure()函数 都会调用 FindNearLinkedScans() 函数.
只不过前者是使用 FindNearLinkedScans() 的结果, 后者是将 FindNearLinkedScans() 的结果排除掉.
FindNearLinkedScans() 函数 是使用广度优先搜索算法, 在图结构中找到与当前节点距离在一定范围内的所有节点.
经常刷 leetcode 的同学可能会对这个算法很熟悉, 但是在slam中挺少遇到的, 所以这里将这一块拿出来着重讲一下.
所以, 同学们, 刷 leetcode 不是毫无作用的, 那里的算法是真正可以用到实际项目中的. slam 里就用到了.
/**
* Traverse the graph starting with the given vertex; applies the visitor to visited nodes
* 广度优先搜索算法,查找给定范围内的所有的节点
* @param pStartVertex
* @param pVisitor
* @return visited vertices
*/
virtual std::vector<T *> Traverse(Vertex<T> *pStartVertex, Visitor<T> *pVisitor)
{
std::queue<Vertex<T> *> toVisit;
std::set<Vertex<T> *> seenVertices;
std::vector<Vertex<T> *> validVertices;
toVisit.push(pStartVertex);
seenVertices.insert(pStartVertex);
do
{
Vertex<T> *pNext = toVisit.front();
toVisit.pop();
// 距离小于阈值就加入到validVertices中
if (pVisitor->Visit(pNext))
{
// vertex is valid, explore neighbors
validVertices.push_back(pNext);
// 获取与这个节点相连的所有节点
std::vector<Vertex<T> *> adjacentVertices = pNext->GetAdjacentVertices();
forEach(typename std::vector<Vertex<T> *>, &adjacentVertices)
{
Vertex<T> *pAdjacent = *iter;
// adjacent vertex has not yet been seen, add to queue for processing
if (seenVertices.find(pAdjacent) == seenVertices.end())
{
// 如果没有使用过,就加入到toVisit中,同时加入seenVertices以防止重复使用
toVisit.push(pAdjacent);
seenVertices.insert(pAdjacent);
}
}
}
} while (toVisit.empty() == false);
// 将结果保存成vector
std::vector<T *> objects;
forEach(typename std::vector<Vertex<T> *>, &validVertices)
{
objects.push_back((*iter)->GetObject());
}
return objects;
}
3.3.6 GetAdjacentVertices()
/**
* Gets a vector of the vertices adjacent to this vertex
* @return adjacent vertices
*/
std::vector<Vertex<T> *> GetAdjacentVertices() const
{
std::vector<Vertex<T> *> vertices;
const_forEach(typename std::vector<Edge<T> *>, &m_Edges)
{
Edge<T> *pEdge = *iter;
// check both source and target because we have a undirected graph
if (pEdge->GetSource() != this)
{
vertices.push_back(pEdge->GetSource());
}
if (pEdge->GetTarget() != this)
{
vertices.push_back(pEdge->GetTarget());
}
}
return vertices;
}
4 回环检测与全局优化
4.1 TryCloseLoop()
TryCloseLoop() 函数首先调用 FindPossibleLoopClosure() 获取可能是回环的 chain , 然后使用当前scan与这个chain进行粗扫描匹配，如果响应值大于阈值，协方差小于阈值，再进行精匹配，如果精匹配的得分（响应值）大于阈值，则找到了回环
如果得分大于阈值, 就认为找到了一个回环, 调用LinkChainToScan() 函数添加一条边, 并紧接着调用 CorrectPoses() 函数执行全局优化,重新计算所有位姿.
之后再调用FindPossibleLoopClosure() 函数, 按照上次的index继续进行回环检测与匹配, 如果有回环了再进行扫描匹配. 直到index为最后一个scan的索引为止.
// 进行回环检测,找到回环后再进行全局优化
kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan *pScan, const Name &rSensorName)
{
kt_bool loopClosed = false;
kt_int32u scanIndex = 0;
// 通过FindPossibleLoopClosure遍历所有之前的scan,计算其与当前pscan的距离
// 找到 连续的几帧和当前帧的距离小于阈值，同时不属于 Near Linked Scans的 几个scan,也是一条chain
LocalizedRangeScanVector candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
while (!candidateChain.empty())
{
Pose2 bestPose;
Matrix3 covariance;
// 粗匹配
kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain,bestPose, covariance, false, false);
if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
(covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
(covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()))
{
LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector());
tmpScan.SetUniqueId(pScan->GetUniqueId());
tmpScan.SetTime(pScan->GetTime());
tmpScan.SetStateId(pScan->GetStateId());
tmpScan.SetCorrectedPose(pScan->GetCorrectedPose());
tmpScan.SetSensorPose(bestPose); // This also updates OdometricPose.
// 精匹配 这里的这句和上面 不同之处在于 MatchScan的最后一个参数, 这里使用了缺省值 true，因此进行细匹配
kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, candidateChain,bestPose, covariance, false);
if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue())
{
// m_pMapper->FireLoopClosureCheck("REJECTED!");
}
else
{
// 精匹配得分大于阈值,认为找到回环了
pScan->SetSensorPose(bestPose);
// 将chain添加边约束，1个candidateChain添加1个边约束，等会要用来优化
LinkChainToScan(candidateChain, pScan, bestPose, covariance);
// 找到回环了立刻执行一下全局优化,重新计算所有位姿
CorrectPoses();
loopClosed = true;
}
}
// 计算下一个candidateChain, scanIndex会继续增加
candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
}
return loopClosed;
}
4.3 FindPossibleLoopClosure()
FindPossibleLoopClosure() 首先调用 FindNearLinkedScans() 获取当前节点的 near scan, 这些near scan 不可以作为回环.
这块我没搞懂, 可能是因为 near scan 已经在上边的程序中使用过了. 但是这块的搜索距离又和上边的不一样… 不理解.
之后, 对所有之前保存的scan做依次遍历, 计算与当前scan的距离.
如果距离小于m_pLoopSearchMaximumDistance, 那说明这附近可能是个存在回环的区域, 在这个区域范围内时向chain中添加数据.
当距离已经大于 m_pLoopSearchMaximumDistance , 说明已经出了回环可能存在的区域, 当出了这个区域时停止向chain添加scan,并判断这个chain是否是个有效的chain
有效的chain 要满足几个条件:
- 要满足连续的几个候选解都在这个范围内才能是一个有效的chain.
- 如果 pCandidateScan 在 nearLinkedScans 中, 就将这个chain删掉.
- chain中的scan个数大于阈值m_pLoopMatchMinimumChainSize
如果找到了一个有效的chain, 程序就先退出, 并将当前处理到的scan的index返回.
// 遍历之前保存的每个scan,计算与当前pscan的距离,如果能够满足连续几个scan与pscan的距离都在阈值范围内,则认为找到了一个可能的回环
LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(LocalizedRangeScan *pScan,
const Name &rSensorName,
kt_int32u &rStartNum)
{
LocalizedRangeScanVector chain; // return value
// 得到 当前帧的扫描点世界坐标的平均值
Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
// possible loop closure chain should not include close scans that have a
// path of links to the scan of interest
// m_pMapper->m_pLoopSearchMaximumDistance可以设置为15m
// 找到可以作为near linked 的scan,这些scan不可作回环
const LocalizedRangeScanVector nearLinkedScans =
FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue());
kt_int32u nScans = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size());
for (; rStartNum < nScans; rStartNum++)
{
// 遍历每个scan, 每个scan都是可能是回环的候选项
LocalizedRangeScan *pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum);
// 获取候选项的位姿
Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
// 计算当前scan与候选项的距离的平方
kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition());
// 如果距离小于m_pLoopSearchMaximumDistance, 那说明这附近可能是个存在回环的区域
// 在这个区域范围内时 向chain中添加数据, 要满足连续的几个候选解都在这个范围内才能是一个有效的chain
if (squaredDistance < math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE)
{
// a linked scan cannot be in the chain
// 如果 pCandidateScan 在 nearLinkedScans 中,就将这个chain删掉
if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end())
{
chain.clear();
}
// 在这个区域范围内时 向chain中添加数据
else
{
chain.push_back(pCandidateScan);
}
}
// 当距离已经大于 m_pLoopSearchMaximumDistance , 说明已经出了回环可能存在的区域
// 当出了这个区域时停止向chain添加scan,并判断这个chain是否是个有效的chain
else
{
// return chain if it is long "enough"
// 如果chain中的scan个数大于阈值,说明是个有效的回环chain,提前返回
if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
{
return chain;
}
// 个数小于阈值,是个无效的回环chain,删掉
else
{
chain.clear();
}
}
}
return chain;
}
4.4 执行位姿图优化
4.4.1 MapperGraph::CorrectPoses()
CorrectPoses() 是进行位姿图优化求解的地方。将这个函数放在了 回环检测的里边，也就是说，只有在回环检测通过（2帧scan匹配得分大于阈值时）的情况下，才进行一次图优化求解。
// 执行后端优化,校正所有的位姿
void MapperGraph::CorrectPoses()
{
// optimize scans!
ScanSolver *pSolver = m_pMapper->m_pScanOptimizer;
if (pSolver != NULL)
{
// 执行优化操作
pSolver->Compute();
const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections())
{
// 保存优化后的所有位姿
m_pMapper->m_pMapperSensorManager->GetScan(iter->first)->SetSensorPose(iter->second);
}
pSolver->Clear();
}
}
4.4.2 SpaSolver::Compute()
// 进行全局优化
void SpaSolver::Compute()
{
corrections.clear();
typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d>> NodeVector;
ROS_INFO("Calling doSPA for loop closure");
// 进行优化求解
m_Spa.doSPA(40);
ROS_INFO("Finished doSPA for loop closure");
// 将优化后的位姿转换成karto的格式
NodeVector nodes = m_Spa.getNodes();
forEach(NodeVector, &nodes)
{
karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
corrections.push_back(std::make_pair(iter->nodeId, pose));
}
}
图优化求解的结果：
corrections 存储的是图结构 所有的 顶点id 与 顶点
图优化的结果是 更新了所有顶点（也就是有效scan）的坐标值
总结
-
karto的后端优化是和回环检测结合在一起的，首先向SPA中添加顶点，再通过3种方式向SPA中添加边结构，然后试着找回环。
-
找回环：首先通过坐标距离小于阈值, 并且形成的chain中scan的个数大于阈值，才算是一个候选的回环chain。然后使用当前scan与这个chain进行粗扫描匹配，如果响应值大于阈值，协方差小于阈值，再进行精匹配，如果精匹配的得分（响应值）大于阈值，则找到了回环。
-
找到了回环则将回环约束作为边结构添加到SPA中，然后进行图优化求解，更新所有scan的坐标。
-
通过 扫描匹配 与 后端优化环节可知，整个open_karto没有维护地图, 每次扫描匹配的地图都是通过一系列的scan新生成的, 是非常省资源的。
最后的栅格地图是通过所有优化后的scan进行生成的，在slam_karto进行进行调用，不用维护整个栅格地图，非常的省cpu与内存。
REFERENCES
加入了详细中文解释的open_karto地址: https://github.com/kadn/open_karto
Karto_slam框架与代码解析 https://blog.csdn.net/qq_24893115/article/details/52965410
Karto SLAM之open_karto代码学习笔记（二） https://blog.csdn.net/wphkadn/article/details/90247146
SLAM_Karto 学习（四） 深入理解 ScanMatch 过程 https://blog.csdn.net/Fourier_Legend/article/details/81558886?utm_source=blogxgwz0#process%E5%87%BD%E6%95%B0uml