[SLAM]Karto SLAM算法学习(草稿)
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()方法进行处理
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 | //后台处理线程执行的过程 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()实现扫描匹配。
1 | 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。
1 2 3 4 5 6 | 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.
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】凌霞软件回馈社区,博客园 & 1Panel & Halo 联合会员上线
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】博客园社区专享云产品让利特惠,阿里云新客6.5折上折
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步