[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()方法进行处理

//后台处理线程执行的过程
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类是进行图后端优化计算的基类。

 class ScanSolver : public Referenced
   {
   public:
     /**
      * Vector of id-pose pairs
      */
     typedef List<Pair<kt_int32s, Pose2> > IdPoseVector;

     /**
      * Default constructor
      */
     ScanSolver()
     {
     }

   protected:
     //@cond EXCLUDE
     /**
      * Destructor
      */
     virtual ~ScanSolver()
     {
     }
     //@endcond

   public:
     /**
      * Solve!
      */
     virtual void Compute() = ;

     /**
      * Gets corrected poses after optimization
      * @return optimized poses
      */
     virtual const IdPoseVector& GetCorrections() const = ;

     /**
      * Adds a node to the solver
      */
     virtual void AddNode(Vertex<LocalizedObjectPtr>* /*pVertex*/)
     {
     }

     /**
      * Removes a node from the solver
      */
     virtual void RemoveNode(kt_int32s /*id*/)
     {
     }

     /**
      * Adds a constraint to the solver
      */
     virtual void AddConstraint(Edge<LocalizedObjectPtr>* /*pEdge*/)
     {
     }

     /**
      * Removes a constraint from the solver
      */
     virtual void RemoveConstraint(kt_int32s /*sourceId*/, kt_int32s /*targetId*/)
     {
     }

     /**
      * Resets the solver
      */
     virtual void Clear() {};
   }; // ScanSolver

ScanSolver

MapperGraph类维护了一个图结构，用于存储位姿节点和边。

  /**
    * Graph for graph SLAM algorithm
    */
   class KARTO_EXPORT MapperGraph : public Graph<LocalizedObjectPtr>
   {
   public:
     /**
      * Graph for graph SLAM
      * @param pOpenMapper mapper
      * @param rangeThreshold range threshold
      */
     MapperGraph(OpenMapper* pOpenMapper, kt_double rangeThreshold);

     /**
      * Destructor
      */
     virtual ~MapperGraph();

   public:
     /**
      * Adds a vertex representing the given object to the graph
      * @param pObject object
      */
     void AddVertex(LocalizedObject* pObject);

     /**
      * Creates an edge between the source object and the target object if it
      * does not already exist; otherwise return the existing edge
      * @param pSourceObject source object
      * @param pTargetObject target object
      * @param rIsNewEdge set to true if the edge is new
      * @return edge between source and target vertices
      */
     Edge<LocalizedObjectPtr>* AddEdge(LocalizedObject* pSourceObject, LocalizedObject* pTargetObject, kt_bool& rIsNewEdge);

     /**
      * Links object to last scan
      * @param pObject object
      */
     void AddEdges(LocalizedObject* pObject);

     /**
      * Links scan to last scan and nearby chains of scans
      * @param pScan scan
      * @param rCovariance match uncertainty
      */
     void AddEdges(LocalizedLaserScan* pScan, const Matrix3& rCovariance);

     /**
      * Tries to close a loop using the given scan with the scans from the given sensor
      * @param pScan scan
      * @param rSensorName name of sensor
      * @return true if a loop was closed
      */
     kt_bool TryCloseLoop(LocalizedLaserScan* pScan, const Identifier& rSensorName);

     /**
      * Finds "nearby" (no further than given distance away) scans through graph links
      * @param pScan scan
      * @param maxDistance maximum distance
      * @return "nearby" scans that have a path of links to given scan
      */
     LocalizedLaserScanList FindNearLinkedScans(LocalizedLaserScan* pScan, kt_double maxDistance);

     /**
      * Finds scans that overlap the given scan (based on bounding boxes)
      * @param pScan scan
      * @return overlapping scans
      */
      LocalizedLaserScanList FindOverlappingScans(LocalizedLaserScan* pScan);

     /**
      * Gets the graph's scan matcher
      * @return scan matcher
      */
     inline ScanMatcher* GetLoopScanMatcher() const
     {
       return m_pLoopScanMatcher;
     }

     /**
      * Links the chain of scans to the given scan by finding the closest scan in the chain to the given scan
      * @param rChain chain
      * @param pScan scan
      * @param rMean mean
      * @param rCovariance match uncertainty
      */
     void LinkChainToScan(const LocalizedLaserScanList& rChain, LocalizedLaserScan* pScan, const Pose2& rMean, const Matrix3& rCovariance);

   private:
     /**
      * Gets the vertex associated with the given scan
      * @param pScan scan
      * @return vertex of scan
      */
     inline Vertex<LocalizedObjectPtr>* GetVertex(LocalizedObject* pObject)
     {
       return m_Vertices[pObject->GetUniqueId()];
     }

     /**
      * Finds the closest scan in the vector to the given pose
      * @param rScans scan
      * @param rPose pose
      */
     LocalizedLaserScan* GetClosestScanToPose(const LocalizedLaserScanList& rScans, const Pose2& rPose) const;

     /**
      * Adds an edge between the two objects and labels the edge with the given mean and covariance
      * @param pFromObject from object
      * @param pToObject to object
      * @param rMean mean
      * @param rCovariance match uncertainty
      */
     void LinkObjects(LocalizedObject* pFromObject, LocalizedObject* pToObject, const Pose2& rMean, const Matrix3& rCovariance);

     /**
      * Finds nearby chains of scans and link them to scan if response is high enough
      * @param pScan scan
      * @param rMeans means
      * @param rCovariances match uncertainties
      */
     void LinkNearChains(LocalizedLaserScan* pScan, Pose2List& rMeans, List<Matrix3>& rCovariances);

     /**
      * Finds chains of scans that are close to given scan
      * @param pScan scan
      * @return chains of scans
      */
     List<LocalizedLaserScanList> FindNearChains(LocalizedLaserScan* pScan);

     /**
      * Compute mean of poses weighted by covariances
      * @param rMeans list of poses
      * @param rCovariances uncertainties
      * @return weighted mean
      */
     Pose2 ComputeWeightedMean(const Pose2List& rMeans, const List<Matrix3>& rCovariances) const;

     /**
      * Tries to find a chain of scan from the given sensor starting at the
      * given scan index that could possibly close a loop with the given scan
      * @param pScan scan
      * @param rSensorName name of sensor
      * @param rStartScanIndex start index
      * @return chain that can possibly close a loop with given scan
      */
     LocalizedLaserScanList FindPossibleLoopClosure(LocalizedLaserScan* pScan, const Identifier& rSensorName, kt_int32u& rStartScanIndex);

     /**
      * Optimizes scan poses
      */
     void CorrectPoses();

   private:
     /**
      * Mapper of this graph
      */
     OpenMapper* m_pOpenMapper;

     /**
      * Scan matcher for loop closures
      */
     ScanMatcher* m_pLoopScanMatcher;    

     /**
      * Traversal algorithm to find near linked scans
      */
     GraphTraversal<LocalizedObjectPtr>* m_pTraversal;
   }; // MapperGraph

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.