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.