欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

ROS学习笔记------ROS深度解析----- day 6 2019/3/14 帅某(Karto SLAM算法学习)

程序员文章站 2024-03-25 09:26:58
...

Karto SLAM算法学习

博主地址:
https://www.cnblogs.com/yhlx125/p/5694370.html

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.githttps://github.com/ros-perception/slam_karto.git 采用SPA方法进行优化

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

1. 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() = 0;

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

    /**
     * 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()实现了优化计算的过程。

2.栅格地图

有三种状态,栅格被占用值为100。
  源码:

typedef enum
  {
    GridStates_Unknown = 0,
    GridStates_Occupied = 100,
    GridStates_Free = 255
  } GridStates;

3.扫描匹配与定位

相关分析方法,类似于一种求重叠面积的方法来算相关系数,或者叫响应函数值。可以参考文献[1],但是并不是Karto的文献,感觉很类似。

包括粗搜索和精搜索过程

Lookup Table

响应函数值越大,匹配效果越好。

4.位姿图优化计算

位姿图通过当前帧位姿与之前所有位姿的距离进行判断,还是一个非常简化的方式。

协方差作为边,作为约束。

可以采用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.

相关标签: karto slam