1 Mapper::Process()
The code related to backend optimization and loopback detection in the main function is as follows:
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;
}
After scanning and matching, the optimal pose of the current scan is obtained. After that, vertices and edges are added to the graph structure, and then loop detection is performed.
2 MapperGraph::AddVertex()
After that, the first step of the backend is performed, and the current scan is added to the graph structure as a node.
There are two graph structures here,
one is the Graph class in karto, which is used to do breadth-first search on this graph when looking for loopbacks.
Graph::AddVertex() here refers to calling the AddVertex() function of the base class Graph of MapperGraph to save the nodes.
The second is the graph structure in the sparse pose graph spa, which is used for global pose optimization.
/**
* 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进行优化，将节点添加进 <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 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));
}
}
There are three ways to add edges to the pose graph:
The first one: LinkScans()
Join the previous frame of radar data with the current radar data and add edge constraints. Call SpaSolver::AddConstraint() once.
The second LinkChainToScan()
In karto, multiple consecutive scans within a certain range are called a chain , Chinese is not easy to translate...just call it that.
Running scans are scans within a continuous range of 20m before the current scan.
Use running scans as a chain, add an edge to the current scan and this chain, find the scan closest to the current scan among all running scans, and call LinkScans() to add an edge to the two scans.
The third LinkNearChains()
First, in the Graph structure, search for all scans whose distance from the current scan is within a certain threshold range. These scans are called near scans by karto .
Then, according to the index of the near scan, each near scan is expanded into a chain, and the near scan is expanded into a chain in two directions of index increase and index decrease. At the same time, this chain cannot include the current scan. This chain is called near chain .
3.1 Add an edge between the current scan and the previous frame 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 Add an edge to the current scan and 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 Add edges to the current scan and near chains: MapperGraph::LinkNearChains()
3.3.1 LinkNearChains()
The LinkNearChains() function first calls FindNearChains() to get the near chain.
Then add an edge to each valid near chain and the current scan, and each near chain will call LinkChainToScan() once.
This is actually a loopback detection, but the search range will be smaller.
// 将满足要求的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()
The FindNearChains() function calls FindNearLinkedScans() to obtain near scans, and then expands all near scans into near chains to obtain many near chains, and then calculates the matching score by scanning and matching the current scan with this chain, if A matching score higher than the threshold indicates a valid 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 Breadth-first search algorithm Traverse()
The LinkNearChains() function when adding edges and the FindPossibleLoopClosure() function when loop detection will call the FindNearLinkedScans() function.
It's just that the former is the result of using FindNearLinkedScans(), and the latter is to exclude the result of FindNearLinkedScans().
The FindNearLinkedScans() function uses the breadth-first search algorithm to find all nodes within a certain distance from the current node in the graph structure.
Students who frequently read Leetcode may be familiar with this algorithm, but it is rarely encountered in slam, so I will focus on this one here.
So, students, brushing Leetcode is not useless, the algorithm there can really be used in actual projects. It is used in 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 Loop detection and global optimization
4.1 TryCloseLoop()
The TryCloseLoop() function first calls FindPossibleLoopClosure() to obtain a chain that may be a loopback, and then uses the current scan to perform rough scan matching with this chain. If the response value is greater than the threshold and the covariance is less than the threshold, fine matching is performed. If the fine matching score ( response value) is greater than the threshold, a loopback is found
If the score is greater than the threshold, it is considered to have found a loop, call the LinkChainToScan() function to add an edge, and then call the CorrectPoses() function to perform global optimization and recalculate all poses.
Then call the FindPossibleLoopClosure() function to continue loop detection and matching according to the last index, and scan and match again if there is a loop. Until the index is the index of the last 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() first calls FindNearLinkedScans() to obtain the near scans of the current node, and these near scans cannot be used as loopbacks.
I don't understand this part, maybe because the near scan has been used in the above program. But the search distance of this part is different from the above one... I don't understand.
After that, traverse all the previously saved scans one by one, and calculate the distance from the current scan.
If the distance is less than m_pLoopSearchMaximumDistance, it means that there may be a loopback area nearby, and add data to the chain when it is within the range of this area.
When the distance is greater than m_pLoopSearchMaximumDistance, it means that the area where the loopback may exist has been exited. When the area is out of the area, stop adding scan to the chain and judge whether the chain is a valid chain.
A valid chain must meet several conditions:
- Only when several consecutive candidate solutions are within this range can it be a valid chain.
- If pCandidateScan is in nearLinkedScans, delete this chain.
- The number of scans in the chain is greater than the threshold m_pLoopMatchMinimumChainSize
If a valid chain is found, the program exits first and returns the index of the currently processed scan.
// 遍历之前保存的每个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 Perform Pose Graph Optimization
4.4.1 MapperGraph::CorrectPoses()
CorrectPoses() is where the pose graph optimization solution is performed. Put this function inside the loopback detection, that is, only when the loopback detection passes (when the 2-frame scan matching score is greater than the threshold), the graph optimization solution is performed once.
// 执行后端优化,校正所有的位姿
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));
}
}
The result of graph optimization solution:
The corrections store all the vertex ids and vertex ids of the graph structure.
The result of graph optimization is to update the coordinate values of all vertices (that is, the effective scan)
Summarize
-
Karto's back-end optimization is combined with loopback detection. First, add vertices to SPA, then add edge structures to SPA in three ways, and then try to find the loopback.
-
Find the loop: firstly, if the coordinate distance is less than the threshold, and the number of scans in the formed chain is greater than the threshold, it can be regarded as a candidate loopback chain. Then use the current scan to perform rough scan matching with this chain. If the response value is greater than the threshold and the covariance is less than the threshold, fine matching is performed. If the fine matching score (response value) is greater than the threshold, a loop is found.
-
If a loop is found, the loop constraint is added to the SPA as an edge structure, and then the graph optimization solution is performed to update the coordinates of all scans.
-
Through scan matching and back-end optimization, it can be seen that the entire open_karto does not maintain maps, and the maps for each scan match are newly generated through a series of scans, which is very resource-saving.
The final grid map is generated through all optimized scans, and is called in slam_karto, without maintaining the entire grid map, which saves CPU and memory very much.
REFERENCES
Added open_karto address with detailed Chinese explanation: https://github.com/kadn/open_karto
Karto_slam framework and code analysis https://blog.csdn.net/qq_24893115/article/details/52965410
Open_karto code study notes for Karto SLAM (2) https://blog.csdn.net/wphkadn/article/details/90247146
SLAM_Karto learning (4) In-depth understanding of the ScanMatch process https://blog.csdn.net/Fourier_Legend/article/details/81558886?utm_source=blogxgwz0#process%E5%87%BD%E6%95%B0uml