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转换成 vertexVertex<LocalizedRangeScan>* pVertex = new Vertex<LocalizedRangeScan>(pScan);// 将vertex加入Graph图结构中 Graph<LocalizedRangeScan>::AddVertex(pScan->GetSensorName(), pVertex);if (m_pMapper->m_pScanOptimizer != NULL){//使用SPA进行优化，将节点添加进 <sparse_bundle_adjustment/spa2d.h> 的求解类型中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 scankt_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 scansif ((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 newif (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 chainconst 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 chainLocalizedRangeScanVector processed;// 在pscan周围查找可以链接的scan// m_pMapper->m_pLinkScanMaximumDistance 为1.5mconst 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 chainkt_bool isValidChain = true;std::list<LocalizedRangeScan *> chain;// add scans before current scan being processed// 从当前scan的index-1开始遍历,直到index=0for (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=endkt_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 vectorLocalizedRangeScanVector tempChain;std::copy(chain.begin(), chain.end(), std::inserter(tempChain, tempChain.begin()));// add chain to collectionnearChains.push_back(tempChain);}}return nearChains;
}

3.3.3 FindNearLinkedScans()


// 根据传入不同的搜索距离,在一定范围内来寻找 NearLinkedScans
LocalizedRangeScanVector MapperGraph::FindNearLinkedScans(LocalizedRangeScan *pScan, kt_double maxDistance)
{// 在pScan周围寻找 NearLinkedScansNearScanVisitor *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 neighborsvalidVertices.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 processingif (seenVertices.find(pAdjacent) == seenVertices.end()){// 如果没有使用过,就加入到toVisit中,同时加入seenVertices以防止重复使用toVisit.push(pAdjacent);seenVertices.insert(pAdjacent);}}}} while (toVisit.empty() == false);// 将结果保存成vectorstd::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 graphif (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,也是一条chainLocalizedRangeScanVector 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中添加数据, 要满足连续的几个候选解都在这个范围内才能是一个有效的chainif (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是否是个有效的chainelse{// 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

karto探秘之open_karto 第四章 --- 回环检测与后端优化相关推荐

Karto的后端优化与回环检测功能对比测试与分析
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达上篇文章讲解了Karto的前端是如何工作的. 这篇文章将slam_karto中的后端优化部分的代码添加 ...
视觉slam十四讲 pdf_视觉SLAM十四讲|第12讲回环检测
1. 什么是回环检测前面有说过累积误差的问题,前一时刻的误差会积累到后面,导致画不成圈圈,如图12-1所示,而画圈圈(全局一致性)很重要,所以需要有一个步骤来纠正当前的计算偏差. 回环检测通过判断相 ...
【SLAM十四讲】ch11 回环检测词袋法实验得出相似分数后计算PR曲线 VPR实验编辑中
[SLAM十四讲]ch11 回环检测词袋法实验得出相似分数后计算PR曲线 [SLAM十四讲]ch11 回环检测词袋法实验得出相似分数后计算PR曲线 DBow3库安装 ch11编译 ch11 词 ...
视觉SLAM十四讲学习笔记——ch11回环检测
文章目录 11.1 理论部分 11.2 实践部分 11.2.1 创建字典 11.2.2 利用已经创建的vocabulary.yml.gz字典,得到图片与图片之间相似性的评分 11.2.3 利用规模更大 ...
详解 | SLAM回环检测问题
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文经知乎作者fishmarch授权转载,二次转载请联系作者原文 ...
综述 | SLAM回环检测方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨任旭倩来源丨计算机视觉life 在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一 ...
视觉SLAM回环检测、词袋模型和视觉位置识别--论文记录和实验简析
一.传统方法部分(词袋模型,bag of words,BoW) 1. 预先在环境中采集足够多的图像或者所有位置的图像(成千上万张图片)之后构建视觉词汇表参考论文:2012年的TRO顶刊 Gálvez ...
SLAM笔记（七）回环检测中的词袋BOW
1.词频 (摘自阮一峰博客,参见附录参考) 如果某个词很重要,它应该在这篇文章中多次出现.于是,我们进行"词频"(Term Frequency,缩写为TF)统计.考虑到文章有长短之 ...
一文详解回环检测与重定位
标题:VINS-Mono代码解读-回环检测与重定位 pose graph loop closing 作者:Manii 来源:https://blog.csdn.net/qq_41839222/cate ...
最全综述 | SLAM中回环检测方法收藏
在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一帧位姿解算当前帧位姿,因此其中的误差便这样一帧一帧的传递下去,也就是我们所说的累积误差.一个消除误差有效的办法是进行回环检测.回环检测判断 ...

karto探秘之open_karto 第四章 --- 回环检测与后端优化