[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方法进行优化

(2)http://wiki.ros.org/nav2d 


 

 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
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
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.

posted @   太一吾鱼水  阅读(10997)  评论(7编辑  收藏  举报
努力加载评论中...
点击右上角即可分享
微信分享提示