目錄
1 kt_bool Mapper::Process(LocalizedRangeScan* pScan)
2 void MapperGraph::AddVertex(LocalizedRangeScan* pScan)
3 void MapperGraph::AddEdges()
3.1 將當前scan與前一幀scan添加邊：MapperGraph::LinkScans()
3.2 將當前scan與 running scans 添加邊：MapperGraph::LinkChainToScan()
3.3 將當前scan與 near chains scans 添加邊：MapperGraph::LinkNearChains()
4 MapperGraph::ComputeWeightedMean（）
5 kt_bool MapperGraph::TryCloseLoop()
5.1 LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure()
5.2 進行SPA優化：void MapperGraph::CorrectPoses()
由於閱讀代碼的時間很短，很多地方還不懂，代碼裏的中文註釋大部分來自於
加入了詳細中文解釋的open_karto地址: https://github.com/kadn/open_karto
1 kt_bool Mapper::Process(LocalizedRangeScan* pScan)
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;
}
2 void MapperGraph::AddVertex(LocalizedRangeScan* pScan)
MapperGraph類自己是沒有儲存 頂點結構 的，這個方法直接將pScan轉換爲頂點結構 再加入到SPA中：
m_pMapper->m_pScanOptimizer->AddNode(pVertex);
/**
* 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<LocalizedRangeScan>::AddVertex(pScan->GetSensorName(), pVertex);
if (m_pMapper->m_pScanOptimizer != NULL)
{
//使用SPA進行優化，將節點添加進 <sparse_bundle_adjustment/spa2d.h> 的求解類型中
m_pMapper->m_pScanOptimizer->AddNode(pVertex);
}
}
}
// slam_karto-melodic-devel\src\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 void MapperGraph::AddEdges()
同頂點結構一樣，先通過各種方式找到 邊結構，然後直接加入到SPA的 AddConstraint() 中。
/**
* Link scan to last scan and nearby chains of scans
* @param pScan
* @param rCovariance uncertainty of match
*/
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. 將前一幀雷達數據與當前雷達數據連接，添加邊約束，添加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;
// 對於來自不同設備的laserscan， 可以計算他們之間的關係
kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan,
pSensorManager->GetScans(rCandidateSensorName),
bestPose, covariance);
// 將當前雷達的scan與其他雷達的上一幀scan連成邊，添加1條邊
LinkScans(pScan, pSensorManager->GetScan(rCandidateSensorName, 0), 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
// 2. 將當前scan 與 running scans 添加邊，running scans 就是最近的70個scan，添加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)
// 3. 將當前scan 與 NearChains scans 添加邊
LinkNearChains(pScan, means, covariances);
if (!means.empty())
{
pScan->SetSensorPose(ComputeWeightedMean(means, covariances));
}
}
3.1 將當前scan與前一幀scan添加邊：MapperGraph::LinkScans()
/**
* Adds an edge between the two scans and labels the edge with the given mean and covariance
* @param pFromScan
* @param pToScan
* @param rMean
* @param rCovariance
*/
void MapperGraph::LinkScans(LocalizedRangeScan* pFromScan, LocalizedRangeScan* pToScan,
const Pose2& rMean, const Matrix3& rCovariance)
{
kt_bool isNewEdge = true;
//將source與target連接起來，或者說將from與to連接起來
Edge<LocalizedRangeScan>* pEdge = AddEdge(pFromScan, pToScan, isNewEdge);
// only attach link information if the edge is new
if (isNewEdge == true)
{
// 添加邊的描述信息，2個頂點的座標，誤差的均值，協方差矩陣
pEdge->SetLabel(new LinkInfo(pFromScan->GetSensorPose(), rMean, rCovariance));
if (m_pMapper->m_pScanOptimizer != NULL)
{
// 調用SPA的 m_Spa.addConstraint（），將約束添加進優化問題
m_pMapper->m_pScanOptimizer->AddConstraint(pEdge);
}
}
}
/**
* Creates an edge between the source scan vertex and the target scan vertex if it
* does not already exist; otherwise return the existing edge
* @param pSourceScan
* @param pTargetScan
* @param rIsNewEdge set to true if the edge is new
* @return edge between source and target scan vertices
*/
Edge<LocalizedRangeScan>* MapperGraph::AddEdge(LocalizedRangeScan* pSourceScan,
LocalizedRangeScan* pTargetScan, kt_bool& rIsNewEdge)
{
// check that vertex exists
assert(pSourceScan->GetStateId() < (kt_int32s)m_Vertices[pSourceScan->GetSensorName()].size());
assert(pTargetScan->GetStateId() < (kt_int32s)m_Vertices[pTargetScan->GetSensorName()].size());
Vertex<LocalizedRangeScan>* v1 = m_Vertices[pSourceScan->GetSensorName()][pSourceScan->GetStateId()];
Vertex<LocalizedRangeScan>* v2 = m_Vertices[pTargetScan->GetSensorName()][pTargetScan->GetStateId()];
// see if edge already exists
const_forEach(std::vector<Edge<LocalizedRangeScan>*>, &(v1->GetEdges()))
{
Edge<LocalizedRangeScan>* pEdge = *iter;
if (pEdge->GetTarget() == v2)
{
rIsNewEdge = false;
return pEdge;
}
}
// 生成一個新的邊結構，同時給這條邊的 2個頂點 添加 邊信息
Edge<LocalizedRangeScan>* pEdge = new Edge<LocalizedRangeScan>(v1, v2);
// 圖結構添加邊
Graph<LocalizedRangeScan>::AddEdge(pEdge);
rIsNewEdge = true;
return pEdge; // 返回 邊數據
}
// slam_karto-melodic-devel\src\spa_solver.cpp
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);
// 利用SPA添加邊，輸入的是邊，姿態差和不確定性（由協方差矩陣表示）。
m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
}
3.2 將當前scan與 running scans 添加邊：MapperGraph::LinkChainToScan()
// chains 的概念
// 如果 pscan是234
// 那麼其中一個 chain 可能是 7-8-9-10-11， 另一個是56-57-58-59-60-61-62，還有 一個是 135-136-137
// 但 如果chain中包含了 234，那麼就不對，這不應該是FindNearChains(pScan)的結果
/**
* Link the chain of scans to the given scan by finding the closest scan in the chain to the given scan
* 在輸入的chain中 找到 離當前scan(也就是pScan) 距離最近的 scan
* @param rChain
* @param pScan
* @param rMean
* @param rCovariance
*/
void MapperGraph::LinkChainToScan(const LocalizedRangeScanVector& rChain, LocalizedRangeScan* pScan,
const Pose2& rMean, const Matrix3& rCovariance)
{
Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
// 找到 running scan 這個chain 中 距離 pScan 最近的那個scan
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());
// 這裏又進行了一次距離的判斷， 判斷2幀scan間的距離是否小於10米，小於則進行邊的構建
if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
{
LinkScans(pClosestScan, pScan, rMean, rCovariance);
}
}
// 返回 rScans 中 和rPose 最近的 scan
LocalizedRangeScan* MapperGraph::GetClosestScanToPose(const LocalizedRangeScanVector& rScans,
const Pose2& rPose) const
{
LocalizedRangeScan* pClosestScan = NULL;
kt_double bestSquaredDistance = DBL_MAX;
const_forEach(LocalizedRangeScanVector, &rScans)
{
Pose2 scanPose = (*iter)->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
kt_double squaredDistance = rPose.GetPosition().SquaredDistance(scanPose.GetPosition());
if (squaredDistance < bestSquaredDistance)
{
bestSquaredDistance = squaredDistance;
pClosestScan = *iter;
}
}
return pClosestScan;
}
3.3 將當前scan與 near chains scans 添加邊：MapperGraph::LinkNearChains()
NearChain: 以當前節點開始廣度優先的方式從graph中遍歷相鄰的一定距離範圍內所有節點，依據當前id從sensorManager中分別遞增與遞減尋找一定範圍內的chain， 生成nearLinkScans.
前兩個步驟都好理解，主要是第三個，以當前節點爲搜索中心，以一定範圍（比如說4m）進行廣度搜索，得到了所有關聯的節點，然後利用節點生成數據鏈（數據鏈需要包含約5個ID連續的關鍵幀）（不包括當前結點，否則放棄該數據鏈），用當前節點進行匹配，若響應值達到一定閾值，產生一條邊，這條邊一端是當前節點，還有一端是數據鏈中質心距離當前節點質心最近的節點
————————————————
版權聲明：本文爲CSDN博主「辜鴻銘」的原創文章，遵循 CC 4.0 BY-SA 版權協議，轉載請附上原文出處鏈接及本聲明。
原文鏈接：https://blog.csdn.net/qq_24893115/article/details/52965410
/**
* Find nearby chains of scans and link them to scan if response is high enough
* @param pScan
* @param rMeans
* @param rCovariances
*/
void MapperGraph::LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector<Matrix3>& rCovariances)
{
//先使用廣度優先搜索的方法訪問，後面還有一些判斷條件，保證搜索到的相鄰幀組成的鏈和自己之間（按時間順序訪問）存在着不相鄰的幀
const std::vector<LocalizedRangeScanVector> nearChains = FindNearChains(pScan);
const_forEach(std::vector<LocalizedRangeScanVector>, &nearChains) //這是一個vector<vector<LocalizedRangeScan>>
{
//這裏的m_pLoopMatchMinimumChainSize 是閉環檢測的參數
// 因爲後面的閉環檢測代碼有說不讓linkedscan參與閉環，那是因爲 這些linkedscan已經在這裏面加入過優化的約束了
if (iter->size() < m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
{
continue;
}
Pose2 mean;
Matrix3 covariance;
// match scan against "near" chain
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);
LinkChainToScan(*iter, pScan, mean, covariance);
}
}
}
/**
* Find chains of scans that are close to given scan
* 找到那些離pscan近，但和pscan之間又存在相距較遠的幀（找的過程是按時間順序來的）
* 最終結果是 chains， 一個chain是時間連續的一些幀， 同時這個幀之中又不包含 pscan， 函數返回多個chain
* @param pScan
* @return chains of scans
*/
std::vector<LocalizedRangeScanVector> MapperGraph::FindNearChains(LocalizedRangeScan* pScan)
{
std::vector<LocalizedRangeScanVector> nearChains;
// 這裏得到了 Barycenter，這個當前幀讀到的數據的世界座標系的平均值
Pose2 scanPose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
// to keep track of which scans have been added to a chain
LocalizedRangeScanVector processed;
// m_pMapper->m_pLinkScanMaximumDistance->GetValue()值是10，是說要在10m內找 LinkedScans
// 通過廣度優先算法實現了查找相鄰pscan
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 已經被添加過了，這裏process就是被添加的意思
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 把臨近幀 pNearScan 前面的（按時間前後）得到的距離近的幀也連接
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
if (pCandidateScan == pScan)
{
isValidChain = false;
}
Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
// pScan 與 pCandidateScan 間 在世界座標系下的距離偏差，距離小於10纔算是一個chain
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;
}
} // for
// 把pNearScan也放到chain中，循環是從pNearScan的前一個數據開始的
chain.push_back(pNearScan);
// add scans after current scan being processed 把臨近幀 pNearScan 後面的（按時間前後）得到的距離近的幀也連接
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);
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;
}
} // for
if (isValidChain) //如果是有效的
{
// change list to vector
LocalizedRangeScanVector tempChain;
std::copy(chain.begin(), chain.end(), std::inserter(tempChain, tempChain.begin())); //chain一定是時間連續的
// add chain to collection
nearChains.push_back(tempChain);
}
} // const_forEach(LocalizedRangeScanVector, &nearLinkedScans)
return nearChains;
}
/**
* Find "nearby" (no further than given distance away) scans through graph links
* @param pScan
* @param maxDistance
*/
LocalizedRangeScanVector MapperGraph::FindNearLinkedScans(LocalizedRangeScan* pScan, kt_double maxDistance)
{
// 在pScan周圍10米範圍內來尋找 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.4 廣度優先搜索
//很簡單的class，第一個功能是初始化一個參照的當前幀，以及比較距離的閾值； 第二個功能是根據當前幀和閾值實現 與 指定 pVertex的數據的barycenter的平方距離計算，判斷是否相鄰
class NearScanVisitor : public Visitor<LocalizedRangeScan>
{
public:
NearScanVisitor(LocalizedRangeScan* pScan, kt_double maxDistance, kt_bool useScanBarycenter)
: m_MaxDistanceSquared(math::Square(maxDistance))
, m_UseScanBarycenter(useScanBarycenter)
{
//m_CenterPose就是 Barycenter
m_CenterPose = pScan->GetReferencePose(m_UseScanBarycenter);
}
// 判斷2個scan距離是否小於10米，小於返回true
virtual kt_bool Visit(Vertex<LocalizedRangeScan>* pVertex)
{
LocalizedRangeScan* pScan = pVertex->GetObject();
Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter);
// m_CenterPose存儲了當前幀的 Barycenter， 而pose則是 pVertex中的幀的Barycenter，於是下面squareDistance就是在比較這兩個數值的平方距離
kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition());
return (squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE);
}
protected:
Pose2 m_CenterPose;
kt_double m_MaxDistanceSquared;
kt_bool m_UseScanBarycenter;
}; // NearScanVisitor
/**
* Graph traversal algorithm
*/
template<typename T>
class GraphTraversal
{
/**
* Traverse the graph starting at the given vertex and applying the visitor to all visited nodes
* @param pStartVertex
* @param pVisitor
*/
virtual std::vector<T*> Traverse(Vertex<T>* pStartVertex, Visitor<T>* pVisitor) = 0;
protected:
/**
* Graph being traversed
*/
Graph<T>* m_pGraph;
}; // GraphTraversal<T>
// 廣度優先（BFS）算法
template<typename T>
class BreadthFirstTraversal : public GraphTraversal<T>
{
public:
/**
* Constructs a breadth-first traverser for the given graph
*/
BreadthFirstTraversal(Graph<T>* pGraph)
: GraphTraversal<T>(pGraph)
{
}
/**
* Destructor
*/
virtual ~BreadthFirstTraversal()
{
}
public:
/**
* 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();
if (pVisitor->Visit(pNext)) // 訪問第一個節點，如果與pScan的距離在10米之內則true
{
// 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()) // 如果沒找到，也就是沒有訪問過
{
// 通過這個push操作使得queue不斷增加數據，直到所有連接關係被訪問過，最終使用validVertices來作爲相鄰結果
toVisit.push(pAdjacent);
seenVertices.insert(pAdjacent);
}
}
}
} while (toVisit.empty() == false);
std::vector<T*> objects;
forEach(typename std::vector<Vertex<T>*>, &validVertices)
{
objects.push_back((*iter)->GetObject());
}
return objects; // 最終使用validVertices的實際scan（而不是vertex<...scan...>）來作爲相鄰結果
}
}; // class BreadthFirstTraversal
/**
* Gets a vector of the vertices adjacent to this vertex
* @return adjacent vertices
*/
// 通過訪問本class的 m_Edges來知道結果，同時m_Edges一個邊只會最多有兩個節點vertex
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 MapperGraph::ComputeWeightedMean（）
通過協方差作爲權重，求各個位姿在不同權重下的平均位姿，在賦值給SetSensorPose()
/**
* Compute mean of poses weighted by covariances
* @param rMeans
* @param rCovariances
* @return weighted mean
*/
Pose2 MapperGraph::ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector<Matrix3>& rCovariances) const
{
assert(rMeans.size() == rCovariances.size());
// compute sum of inverses and create inverse list
std::vector<Matrix3> inverses;
inverses.reserve(rCovariances.size());
Matrix3 sumOfInverses;
const_forEach(std::vector<Matrix3>, &rCovariances)
{
Matrix3 inverse = iter->Inverse();
inverses.push_back(inverse);
sumOfInverses += inverse;
}
Matrix3 inverseOfSumOfInverses = sumOfInverses.Inverse();
// compute weighted mean
Pose2 accumulatedPose;
kt_double thetaX = 0.0;
kt_double thetaY = 0.0;
Pose2Vector::const_iterator meansIter = rMeans.begin();
const_forEach(std::vector<Matrix3>, &inverses)
{
Pose2 pose = *meansIter;
kt_double angle = pose.GetHeading();
thetaX += cos(angle);
thetaY += sin(angle);
Matrix3 weight = inverseOfSumOfInverses * (*iter);
accumulatedPose += weight * pose;
++meansIter;
}
thetaX /= rMeans.size();
thetaY /= rMeans.size();
accumulatedPose.SetHeading(atan2(thetaY, thetaX));
return accumulatedPose;
}
5 kt_bool MapperGraph::TryCloseLoop()
首先找到座標距離較近的 chain ，然後進行掃描匹配，如果匹配得分高於閾值則迴環約束當做邊添加到圖結構中，通過SPA進行圖優化求解，找到最優解。
/**
* Tries to close a loop using the given scan with the scans from the given device
* @param pScan
* @param rSensorName
*/
kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName)
{
kt_bool loopClosed = false;
kt_int32u scanIndex = 0;
// 通過這個FindPossibleLoopClosure先找到臨近的10幀，要保證它們和當前幀barycenter距離小於4m，
// 同時是連續的10幀，同時10幀之中不包含當前幀的相鄰幀（相鄰幀的獲取在AddEdges函數中）
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
{
m_pMapper->FireBeginLoopClosure("Closing loop...");
pScan->SetSensorPose(bestPose);
// 將chain添加邊約束，1個chain添加1個邊約束，等會要用來優化
LinkChainToScan(candidateChain, pScan, bestPose, covariance);
CorrectPoses();
m_pMapper->FireEndLoopClosure("Loop closed!");
loopClosed = true;
}
}
// 這裏再使用 FindPossibleLoopClosure而不是 直接刪除已經用過的，
// 是因爲前面位姿調整了，所以可能會有一些原來不是chain的現在可以成爲chain了
candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
}
return loopClosed;
}
5.1 LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure()
/**
* Tries to find a chain of scan from the given device starting at the
* given scan index that could possibly close a loop with the given scan
* @param pScan
* @param rSensorName
* @param rStartNum
* @return chain that can possibly close a loop with given scan
*/
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
// 知道合適的 scan們，沒有組成鏈，在當前幀周圍4米內範圍尋找可能的迴環
const LocalizedRangeScanVector nearLinkedScans =
FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue());
// 當前這個雷達的所有scan的個數
kt_int32u nScans = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size());
for (; rStartNum < nScans; rStartNum++) // rStartNum = 0
{
// 獲取 第rStartNum個 scan
LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum);
Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
// 得到pScan與 所有幀中第rStartNum幀 之間的 平方距離，小於4米才進行考慮
kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition());
if (squaredDistance < math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE)
{
// a linked scan cannot be in the chain
// 如果 pCandidateScan 在 nearLinkedScans 中則跳過，因爲這個 pCandidateScan 是 別的迴環chain 的起始位置
if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end())
{
chain.clear();
}
else
{
chain.push_back(pCandidateScan);
}
}
else
{
// return chain if it is long "enough"
// 下一幀的距離大於4米了，看看是不是滿足10個scan了，如果不夠就繼續循環，夠了就退出了
if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
{
return chain;
}
else
{
chain.clear();
}
}
}
return chain;
}
5.2 進行SPA優化：void MapperGraph::CorrectPoses()
CorrectPoses() 是進行優化求解的地方。將這個函數放在了 迴環檢測的裏邊，也就是說，只有在迴環檢測通過（2幀scan匹配得分大於閾值時）的情況下，先將這個迴環約束添加到圖結構中，再進行圖優化求解。
使用SPA進行優化，SPA的具體實現：文件位置：slam_karto/src/spa_solver.cpp
圖優化求解的結果：
corrections 存儲的是圖結構 所有的 頂點id 與 頂點
圖優化的結果是 更新了所有頂點（也就是有效scan）的座標值
/**
* Optimizes scan poses
*/
void MapperGraph::CorrectPoses()
{
// optimize scans!
ScanSolver* pSolver = m_pMapper->m_pScanOptimizer;
if (pSolver != NULL)
{
pSolver->Compute();
// typedef std::vector<std::pair<kt_int32s, Pose2> > IdPoseVector;
// const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections())
for ( ScanSolver::IdPoseVector::const_iterator iter = (&pSolver->GetCorrections())->begin();
iter != (&pSolver->GetCorrections())->end();
++iter )
{
// corrections 存儲的是圖結構 所有的 頂點id 與 頂點
// 所以下邊這句話更新了所有頂點（也就是有效scan）的座標值
m_pMapper->m_pMapperSensorManager->GetScan(iter->first)->SetSensorPose(iter->second);
}
pSolver->Clear();
}
}
// slam_karto-melodic-devel\src\spa_solver.cpp
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");
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));
}
}
// class ScanSolver
typedef std::vector<std::pair<kt_int32s, Pose2> > IdPoseVector;
// class SpaSolver
karto::ScanSolver::IdPoseVector corrections;
總結
1 karto的後端優化是和迴環檢測結合在一起的，首先向SPA中添加頂點，再通過3種方式向SPA中添加邊結構，然後試着找回環。
2 找回環：首先通過座標距離小於4m，並且形成的chain中scan的個數大於10個，纔算是一個候選的迴環chain。然後使用當前scan與這個chain進行粗掃描匹配，如果響應值大於閾值，協方差小於閾值，再進行精匹配，如果精匹配的得分（響應值）大於閾值，則找到了迴環。
3 找到了迴環則將回環約束作爲邊結構添加到SPA中，然後進行圖優化求解，更新所有scan的座標。
4 通過 掃描匹配 與 後端優化環節可知，整個open_karto維護的地圖 只是用於掃描匹配，範圍爲 前後左右12米的柵格大小，是非常省資源的。他的柵格地圖是通過所有的scan進行生成的，在slam_karto進行進行調用，不用維護整個柵格地圖，非常的省cpu與內存。
REFERENCES
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