版权声明：转载请注释出处，帅某不胜感激。严禁利用本博客进行商业或从事利益方面工作！！ https://blog.csdn.net/weixin_43262513/article/details/88555007
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.git 和 https://github.com/ros-perception/slam_karto.git 采用SPA方法进行优化
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.