The previous part compared and analyzed the effect of map building when back-end optimization and loop detection are enabled through experiments, and theoretically analyzed the effects of back-end optimization and loop detection.
This article will explain how backend optimization and loopback detection are implemented.
It's been a long time since I finished writing the code comments, but I've been too busy recently to write an article...
Since the official account is not suitable for large sections of code, this article only briefly describes the implementation process of Karto's back-end optimization and loopback detection through text.
For more detailed code analysis, you can read this article of mine:
Open_karto Chapter 4 of Karto Quest - Loopback Detection and Backend Optimization:
https://blog.csdn.net/tiancailx/article/details/117109400
No picture above Get ready...
code interpretation
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.
1 Adding nodes to the graph structure
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.
2 Adding edges (constraints) to the graph structure
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 .
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.
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.
3 Loop closure detection and pose graph optimization
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.
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.
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.
4 Breadth-first search algorithm
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 code below is the specific implementation of the breadth-first search algorithm.
The idea of realization is,
Put the current scan into the toVisit queue, and then start looping until toVisit is empty.
In the loop, the following operations will be performed in sequence:
Take out the first data pNext of the toVisit queue, and add it to the set of seenVertices, which represents the collection of nodes that have been used.
Calculate the distance between pNext and the current scan, if the distance is less than the threshold, add it to validVertices, this vector, as a result.
Then use the GetAdjacentVertices() function to get all the next adjacent nodes of pNext in the graph structure, and add these nodes not in seenVertices to the toVisit queue and the seenVertices collection.
Circulate in turn, thus completing the breadth-first search. In fact, the specific implementation idea is very simple, but I have never done this kind of question, so I just can’t do it.
/**
* Traverse the graph starting with the given vertex; applies the visitor to visited nodes
* 广度优先搜索算法,查找给定范围内的所有的节点
*/
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;
}
5 summary
-
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.
6 Next
This article briefly explains the implementation process of Karto's back-end optimization and loopback detection through text. At the same time, it focuses on the breadth-first search algorithm used in it.
The next article will rewrite Karto's back-end optimizer through G2O or Ceres, which has achieved the effect of learning.
The article will be updated synchronously on the official account: SLAM from scratch , everyone is welcome to pay attention, you can add my WeChat in the official account, enter the laser SLAM communication group , and exchange SLAM technology together.
If you have any suggestions for the articles I wrote, or want to see how the functions are implemented, please reply directly in the official account, I can receive them, and will seriously consider your suggestions.
