Karto_slam算法是一个Graph based SLAM算法。包括前端和后端。关于代码要分成两块内容来看。
一类是OpenKarto项目，是最初的开源代码，包括算法的核心内容： https://github.com/skasperski/OpenKarto.git 之后作者应该将该项目商业化了：https://www.kartorobotics.com/
作者是这样说的：
“When I worked at SRI, we developed a 2D SLAM mapping system that was among the best. Karto is an industrial-strength version of that system, and I’m glad to see that its core components are available open-source. We are working with Karto’s developers to make it the standard mapping technology for Willow Garage’s ROS robot software and PR2 robot.”
另一种是基于ROS开发的项目，在ROS上运行.
（1）包括两个项目：https://github.com/ros-perception/open_karto.git 和 https://github.com/ros-perception/slam_karto.git 采用SPA方法进行优化
OpenKarto源码中，后台线程调用OpenMapper::Process()方法进行处理
//后台处理线程执行的过程
kt_bool OpenMapper::Process(Object* pObject)
{
if (pObject == NULL)
{
return false;
}
kt_bool isObjectProcessed = Module::Process(pObject);
if (IsLaserRangeFinder(pObject))
{
LaserRangeFinder* pLaserRangeFinder = dynamic_cast<LaserRangeFinder*>(pObject);
if (m_Initialized == false)
{
// initialize mapper with range threshold from sensor
Initialize(pLaserRangeFinder->GetRangeThreshold());
}
// register sensor
m_pMapperSensorManager->RegisterSensor(pLaserRangeFinder->GetIdentifier());
return true;
}
LocalizedObject* pLocalizedObject = dynamic_cast<LocalizedObject*>(pObject);
if (pLocalizedObject != NULL)
{
LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
if (pScan != NULL)
{
karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();
// validate scan
if (pLaserRangeFinder == NULL)
{
return false;
}
// validate scan. Throws exception if scan is invalid.
pLaserRangeFinder->Validate(pScan);
if (m_Initialized == false)
{
// initialize mapper with range threshold from sensor
Initialize(pLaserRangeFinder->GetRangeThreshold());
}
}
// ensures sensor has been registered with mapper--does nothing if the sensor has already been registered
m_pMapperSensorManager->RegisterSensor(pLocalizedObject->GetSensorIdentifier());
// get last scan
LocalizedLaserScan* pLastScan = m_pMapperSensorManager->GetLastScan(pLocalizedObject->GetSensorIdentifier());
// update scans corrected pose based on last correction
if (pLastScan != NULL)
{
Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose());
//根据里程计定位
pLocalizedObject->SetCorrectedPose(lastTransform.TransformPose(pLocalizedObject->GetOdometricPose()));
}
// check custom data if object is not a scan or if scan has not moved enough (i.e.,
// scan is outside minimum boundary or if heading is larger then minimum heading)
if (pScan == NULL || (!HasMovedEnough(pScan, pLastScan) && !pScan->IsGpsReadingValid()))
{
if (pLocalizedObject->HasCustomItem() == true)
{
m_pMapperSensorManager->AddLocalizedObject(pLocalizedObject);
//添加到图中 add to graph
m_pGraph->AddVertex(pLocalizedObject);
m_pGraph->AddEdges(pLocalizedObject);
return true;
}
return false;
}
/////////////////////////////////////////////
// object is a scan
Matrix3 covariance;
covariance.SetToIdentity();
// correct scan (if not first scan)
if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
{
Pose2 bestPose;
//核心一：进行匹配
m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorIdentifier()), bestPose, covariance);
pScan->SetSensorPose(bestPose);
}
ScanMatched(pScan);
// add scan to buffer and assign id
m_pMapperSensorManager->AddLocalizedObject(pLocalizedObject);
if (m_pUseScanMatching->GetValue())
{
// add to graph
m_pGraph->AddVertex(pScan);
m_pGraph->AddEdges(pScan, covariance);
m_pMapperSensorManager->AddRunningScan(pScan);
List<Identifier> sensorNames = m_pMapperSensorManager->GetSensorNames();
karto_const_forEach(List<Identifier>, &sensorNames)
{
//核心二：尝试闭环
m_pGraph->TryCloseLoop(pScan, *iter);
}
}
m_pMapperSensorManager->SetLastScan(pScan);
ScanMatchingEnd(pScan);
isObjectProcessed = true;
} // if object is LocalizedObject
return isObjectProcessed;
}
调用了ScanMatcher::MatchScan()实现扫描匹配。
kt_double ScanMatcher::MatchScan(LocalizedLaserScan* pScan, const LocalizedLaserScanList& rBaseScans, Pose2& rMean, Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch)
ScanSolver类是进行图后端优化计算的基类。
1 class ScanSolver : public Referenced 2 { 3 public: 4 /** 5 * Vector of id-pose pairs 6 */
7 typedef List<Pair<kt_int32s, Pose2> > IdPoseVector; 8
9 /** 10 * Default constructor 11 */
12 ScanSolver() 13 { 14 } 15
16 protected: 17 //@cond EXCLUDE
18 /** 19 * Destructor 20 */
21 virtual ~ScanSolver() 22 { 23 } 24 //@endcond
25
26 public: 27 /** 28 * Solve! 29 */
30 virtual void Compute() = 0; 31
32 /** 33 * Gets corrected poses after optimization 34 * @return optimized poses 35 */
36 virtual const IdPoseVector& GetCorrections() const = 0; 37
38 /** 39 * Adds a node to the solver 40 */
41 virtual void AddNode(Vertex<LocalizedObjectPtr>* /*pVertex*/) 42 { 43 } 44
45 /** 46 * Removes a node from the solver 47 */
48 virtual void RemoveNode(kt_int32s /*id*/) 49 { 50 } 51
52 /** 53 * Adds a constraint to the solver 54 */
55 virtual void AddConstraint(Edge<LocalizedObjectPtr>* /*pEdge*/) 56 { 57 } 58
59 /** 60 * Removes a constraint from the solver 61 */
62 virtual void RemoveConstraint(kt_int32s /*sourceId*/, kt_int32s /*targetId*/) 63 { 64 } 65
66 /** 67 * Resets the solver 68 */
69 virtual void Clear() {}; 70 }; // ScanSolver
MapperGraph类维护了一个图结构，用于存储位姿节点和边。
1 /** 2 * Graph for graph SLAM algorithm 3 */
4 class KARTO_EXPORT MapperGraph : public Graph<LocalizedObjectPtr>
5 { 6 public: 7 /** 8 * Graph for graph SLAM 9 * @param pOpenMapper mapper 10 * @param rangeThreshold range threshold 11 */
12 MapperGraph(OpenMapper* pOpenMapper, kt_double rangeThreshold); 13
14 /** 15 * Destructor 16 */
17 virtual ~MapperGraph(); 18
19 public: 20 /** 21 * Adds a vertex representing the given object to the graph 22 * @param pObject object 23 */
24 void AddVertex(LocalizedObject* pObject); 25
26 /** 27 * Creates an edge between the source object and the target object if it 28 * does not already exist; otherwise return the existing edge 29 * @param pSourceObject source object 30 * @param pTargetObject target object 31 * @param rIsNewEdge set to true if the edge is new 32 * @return edge between source and target vertices 33 */
34 Edge<LocalizedObjectPtr>* AddEdge(LocalizedObject* pSourceObject, LocalizedObject* pTargetObject, kt_bool& rIsNewEdge); 35
36 /** 37 * Links object to last scan 38 * @param pObject object 39 */
40 void AddEdges(LocalizedObject* pObject); 41
42 /** 43 * Links scan to last scan and nearby chains of scans 44 * @param pScan scan 45 * @param rCovariance match uncertainty 46 */
47 void AddEdges(LocalizedLaserScan* pScan, const Matrix3& rCovariance); 48
49 /** 50 * Tries to close a loop using the given scan with the scans from the given sensor 51 * @param pScan scan 52 * @param rSensorName name of sensor 53 * @return true if a loop was closed 54 */
55 kt_bool TryCloseLoop(LocalizedLaserScan* pScan, const Identifier& rSensorName); 56
57 /** 58 * Finds "nearby" (no further than given distance away) scans through graph links 59 * @param pScan scan 60 * @param maxDistance maximum distance 61 * @return "nearby" scans that have a path of links to given scan 62 */
63 LocalizedLaserScanList FindNearLinkedScans(LocalizedLaserScan* pScan, kt_double maxDistance); 64
65 /** 66 * Finds scans that overlap the given scan (based on bounding boxes) 67 * @param pScan scan 68 * @return overlapping scans 69 */
70 LocalizedLaserScanList FindOverlappingScans(LocalizedLaserScan* pScan); 71
72 /** 73 * Gets the graph's scan matcher 74 * @return scan matcher 75 */
76 inline ScanMatcher* GetLoopScanMatcher() const
77 { 78 return m_pLoopScanMatcher; 79 } 80
81 /** 82 * Links the chain of scans to the given scan by finding the closest scan in the chain to the given scan 83 * @param rChain chain 84 * @param pScan scan 85 * @param rMean mean 86 * @param rCovariance match uncertainty 87 */
88 void LinkChainToScan(const LocalizedLaserScanList& rChain, LocalizedLaserScan* pScan, const Pose2& rMean, const Matrix3& rCovariance); 89
90 private: 91 /** 92 * Gets the vertex associated with the given scan 93 * @param pScan scan 94 * @return vertex of scan 95 */
96 inline Vertex<LocalizedObjectPtr>* GetVertex(LocalizedObject* pObject) 97 { 98 return m_Vertices[pObject->GetUniqueId()]; 99 } 100
101 /** 102 * Finds the closest scan in the vector to the given pose 103 * @param rScans scan 104 * @param rPose pose 105 */
106 LocalizedLaserScan* GetClosestScanToPose(const LocalizedLaserScanList& rScans, const Pose2& rPose) const; 107
108 /** 109 * Adds an edge between the two objects and labels the edge with the given mean and covariance 110 * @param pFromObject from object 111 * @param pToObject to object 112 * @param rMean mean 113 * @param rCovariance match uncertainty 114 */
115 void LinkObjects(LocalizedObject* pFromObject, LocalizedObject* pToObject, const Pose2& rMean, const Matrix3& rCovariance); 116
117 /** 118 * Finds nearby chains of scans and link them to scan if response is high enough 119 * @param pScan scan 120 * @param rMeans means 121 * @param rCovariances match uncertainties 122 */
123 void LinkNearChains(LocalizedLaserScan* pScan, Pose2List& rMeans, List<Matrix3>& rCovariances); 124
125 /** 126 * Finds chains of scans that are close to given scan 127 * @param pScan scan 128 * @return chains of scans 129 */
130 List<LocalizedLaserScanList> FindNearChains(LocalizedLaserScan* pScan); 131
132 /** 133 * Compute mean of poses weighted by covariances 134 * @param rMeans list of poses 135 * @param rCovariances uncertainties 136 * @return weighted mean 137 */
138 Pose2 ComputeWeightedMean(const Pose2List& rMeans, const List<Matrix3>& rCovariances) const; 139
140 /** 141 * Tries to find a chain of scan from the given sensor starting at the 142 * given scan index that could possibly close a loop with the given scan 143 * @param pScan scan 144 * @param rSensorName name of sensor 145 * @param rStartScanIndex start index 146 * @return chain that can possibly close a loop with given scan 147 */
148 LocalizedLaserScanList FindPossibleLoopClosure(LocalizedLaserScan* pScan, const Identifier& rSensorName, kt_int32u& rStartScanIndex); 149
150 /** 151 * Optimizes scan poses 152 */
153 void CorrectPoses(); 154
155 private: 156 /** 157 * Mapper of this graph 158 */
159 OpenMapper* m_pOpenMapper; 160
161 /** 162 * Scan matcher for loop closures 163 */
164 ScanMatcher* m_pLoopScanMatcher; 165
166 /** 167 * Traversal algorithm to find near linked scans 168 */
169 GraphTraversal<LocalizedObjectPtr>* m_pTraversal; 170 }; // MapperGraph
其中的CorrectPoses()实现了优化计算的过程。
1、栅格地图
有三种状态，栅格被占用值为100。
typedef enum
{
GridStates_Unknown = 0,
GridStates_Occupied = 100,
GridStates_Free = 255
} GridStates;
2、扫描匹配与定位
相关分析方法，类似于一种求重叠面积的方法来算相关系数，或者叫响应函数值。可以参考文献[1]，但是并不是Karto的文献，感觉很类似。
包括粗搜索和精搜索过程
Lookup Table
响应函数值越大，匹配效果越好。
3、位姿图优化计算
位姿图通过当前帧位姿与之前所有位姿的距离进行判断，还是一个非常简化的方式。
协方差作为边，作为约束。
可以采用SPA（Sparse Pose Adjustment）方法或者稀疏Cholesky decomposition
参考文献
[1]Konecny, J., et al. (2016). "Novel Point-to-Point Scan Matching Algorithm Based on Cross-Correlation." Mobile Information Systems 2016: 1-11.
[2]Harmon, R. S., et al. (2010). "Comparison of indoor robot localization techniques in the absence of GPS." 7664: 76641Z.
[3]Konolige, K., et al. (2010). "Efficient Sparse Pose Adjustment for 2D mapping." 22-29.