Public Member Functions | Private Member Functions | Private Attributes | Friends | List of all members
karto::MapperGraph Class Reference

#include <Mapper.h>

Inheritance diagram for karto::MapperGraph:
Inheritance graph
[legend]

Public Member Functions

Edge< LocalizedRangeScan > * AddEdge (LocalizedRangeScan *pSourceScan, LocalizedRangeScan *pTargetScan, kt_bool &rIsNewEdge)
 
void AddEdges (LocalizedRangeScan *pScan, const Matrix3 &rCovariance)
 
Vertex< LocalizedRangeScan > * AddVertex (LocalizedRangeScan *pScan)
 
void CorrectPoses ()
 
Vertex< LocalizedRangeScan > * FindNearByScan (Name name, const Pose2 refPose)
 
LocalizedRangeScanVector FindNearByScans (Name name, const Pose2 refPose, kt_double maxDistance)
 
std::vector< Vertex< LocalizedRangeScan > * > FindNearByVertices (Name name, const Pose2 refPose, kt_double maxDistance)
 
LocalizedRangeScanVector FindNearLinkedScans (LocalizedRangeScan *pScan, kt_double maxDistance)
 
std::vector< Vertex< LocalizedRangeScan > * > FindNearLinkedVertices (LocalizedRangeScan *pScan, kt_double maxDistance)
 
ScanMatcherGetLoopScanMatcher () const
 
 MapperGraph (Mapper *pMapper, kt_double rangeThreshold)
 
 MapperGraph ()
 
kt_bool TryCloseLoop (LocalizedRangeScan *pScan, const Name &rSensorName)
 
virtual ~MapperGraph ()
 
- Public Member Functions inherited from karto::Graph< LocalizedRangeScan >
void AddEdge (Edge< LocalizedRangeScan > *pEdge)
 
void AddVertex (const Name &rName, Vertex< LocalizedRangeScan > *pVertex)
 
void Clear ()
 
const std::vector< Edge< LocalizedRangeScan > *> & GetEdges () const
 
const VertexMapGetVertices () const
 
 Graph ()
 
void RemoveEdge (const int &idx)
 
void RemoveVertex (const Name &rName, const int &idx)
 
virtual ~Graph ()
 

Private Member Functions

Pose2 ComputeWeightedMean (const Pose2Vector &rMeans, const std::vector< Matrix3 > &rCovariances) const
 
std::vector< LocalizedRangeScanVectorFindNearChains (LocalizedRangeScan *pScan)
 
LocalizedRangeScanVector FindPossibleLoopClosure (LocalizedRangeScan *pScan, const Name &rSensorName, kt_int32u &rStartNum)
 
LocalizedRangeScanGetClosestScanToPose (const LocalizedRangeScanVector &rScans, const Pose2 &rPose) const
 
Vertex< LocalizedRangeScan > * GetVertex (LocalizedRangeScan *pScan)
 
void LinkChainToScan (const LocalizedRangeScanVector &rChain, LocalizedRangeScan *pScan, const Pose2 &rMean, const Matrix3 &rCovariance)
 
void LinkNearChains (LocalizedRangeScan *pScan, Pose2Vector &rMeans, std::vector< Matrix3 > &rCovariances)
 
void LinkScans (LocalizedRangeScan *pFromScan, LocalizedRangeScan *pToScan, const Pose2 &rMean, const Matrix3 &rCovariance)
 
template<class Archive >
void serialize (Archive &ar, const unsigned int version)
 

Private Attributes

ScanMatcherm_pLoopScanMatcher
 
Mapperm_pMapper
 
GraphTraversal< LocalizedRangeScan > * m_pTraversal
 

Friends

class boost::serialization::access
 

Additional Inherited Members

- Public Types inherited from karto::Graph< LocalizedRangeScan >
typedef std::map< Name, std::map< int, Vertex< LocalizedRangeScan > *> > VertexMap
 
- Protected Member Functions inherited from karto::Graph< LocalizedRangeScan >
void serialize (Archive &ar, const unsigned int version)
 
- Protected Attributes inherited from karto::Graph< LocalizedRangeScan >
std::vector< Edge< LocalizedRangeScan > *> m_Edges
 
VertexMap m_Vertices
 

Detailed Description

Graph for graph SLAM algorithm

Definition at line 713 of file Mapper.h.

Constructor & Destructor Documentation

◆ MapperGraph() [1/2]

karto::MapperGraph::MapperGraph ( Mapper pMapper,
kt_double  rangeThreshold 
)

Graph for graph SLAM

Parameters
pMapper
rangeThreshold

Definition at line 1364 of file Mapper.cpp.

◆ MapperGraph() [2/2]

karto::MapperGraph::MapperGraph ( )
inline

Definition at line 722 of file Mapper.h.

◆ ~MapperGraph()

karto::MapperGraph::~MapperGraph ( )
virtual

Destructor

Definition at line 1375 of file Mapper.cpp.

Member Function Documentation

◆ AddEdge()

Edge< LocalizedRangeScan > * karto::MapperGraph::AddEdge ( LocalizedRangeScan pSourceScan,
LocalizedRangeScan pTargetScan,
kt_bool rIsNewEdge 
)

Creates an edge between the source scan vertex and the target scan vertex if it does not already exist; otherwise return the existing edge

Parameters
pSourceScan
pTargetScan
rIsNewEdgeset to true if the edge is new
Returns
edge between source and target scan vertices

Definition at line 1563 of file Mapper.cpp.

◆ AddEdges()

void karto::MapperGraph::AddEdges ( LocalizedRangeScan pScan,
const Matrix3 rCovariance 
)

Link scan to last scan and nearby chains of scans

Parameters
pScan
rCovarianceuncertainty of match

Definition at line 1405 of file Mapper.cpp.

◆ AddVertex()

Vertex< LocalizedRangeScan > * karto::MapperGraph::AddVertex ( LocalizedRangeScan pScan)

Adds a vertex representing the given scan to the graph

Parameters
pScan

Definition at line 1389 of file Mapper.cpp.

◆ ComputeWeightedMean()

Pose2 karto::MapperGraph::ComputeWeightedMean ( const Pose2Vector rMeans,
const std::vector< Matrix3 > &  rCovariances 
) const
private

Compute mean of poses weighted by covariances

Parameters
rMeans
rCovariances
Returns
weighted mean

Definition at line 1877 of file Mapper.cpp.

◆ CorrectPoses()

void karto::MapperGraph::CorrectPoses ( )

Optimizes scan poses

Definition at line 1976 of file Mapper.cpp.

◆ FindNearByScan()

Vertex< LocalizedRangeScan > * karto::MapperGraph::FindNearByScan ( Name  name,
const Pose2  refPose 
)

Find closest scan to pose

Parameters
pScan

Definition at line 1836 of file Mapper.cpp.

◆ FindNearByScans()

LocalizedRangeScanVector karto::MapperGraph::FindNearByScans ( Name  name,
const Pose2  refPose,
kt_double  maxDistance 
)

Find "nearby" (no further than given distance away) scans through graph links

Parameters
pScan
maxDistance

Definition at line 1785 of file Mapper.cpp.

◆ FindNearByVertices()

std::vector< Vertex< LocalizedRangeScan > * > karto::MapperGraph::FindNearByVertices ( Name  name,
const Pose2  refPose,
kt_double  maxDistance 
)

Find "nearby" (no further than given distance away) vertices through KD-tree

Parameters
pScan
maxDistance

Definition at line 1797 of file Mapper.cpp.

◆ FindNearChains()

std::vector< LocalizedRangeScanVector > karto::MapperGraph::FindNearChains ( LocalizedRangeScan pScan)
private

Find chains of scans that are close to given scan

Parameters
pScan
Returns
chains of scans

Definition at line 1656 of file Mapper.cpp.

◆ FindNearLinkedScans()

LocalizedRangeScanVector karto::MapperGraph::FindNearLinkedScans ( LocalizedRangeScan pScan,
kt_double  maxDistance 
)

Find "nearby" (no further than given distance away) scans through graph links

Parameters
pScan
maxDistance

Definition at line 1767 of file Mapper.cpp.

◆ FindNearLinkedVertices()

std::vector< Vertex< LocalizedRangeScan > * > karto::MapperGraph::FindNearLinkedVertices ( LocalizedRangeScan pScan,
kt_double  maxDistance 
)

Find "nearby" (no further than given distance away) vertices through graph links

Parameters
pScan
maxDistance

Definition at line 1776 of file Mapper.cpp.

◆ FindPossibleLoopClosure()

LocalizedRangeScanVector karto::MapperGraph::FindPossibleLoopClosure ( LocalizedRangeScan pScan,
const Name rSensorName,
kt_int32u rStartNum 
)
private

Tries to find a chain of scan from the given device starting at the given scan index that could possibly close a loop with the given scan

Parameters
pScan
rSensorName
rStartNum
Returns
chain that can possibly close a loop with given scan

Definition at line 1921 of file Mapper.cpp.

◆ GetClosestScanToPose()

LocalizedRangeScan * karto::MapperGraph::GetClosestScanToPose ( const LocalizedRangeScanVector rScans,
const Pose2 rPose 
) const
private

Finds the closest scan in the vector to the given pose

Parameters
rScans
rPose

Definition at line 1542 of file Mapper.cpp.

◆ GetLoopScanMatcher()

ScanMatcher* karto::MapperGraph::GetLoopScanMatcher ( ) const
inline

Gets the graph's scan matcher

Returns
scan matcher

Definition at line 806 of file Mapper.h.

◆ GetVertex()

Vertex<LocalizedRangeScan>* karto::MapperGraph::GetVertex ( LocalizedRangeScan pScan)
inlineprivate

Gets the vertex associated with the given scan

Parameters
pScan
Returns
vertex of scan

Definition at line 817 of file Mapper.h.

◆ LinkChainToScan()

void karto::MapperGraph::LinkChainToScan ( const LocalizedRangeScanVector rChain,
LocalizedRangeScan pScan,
const Pose2 rMean,
const Matrix3 rCovariance 
)
private

Link the chain of scans to the given scan by finding the closest scan in the chain to the given scan

Parameters
rChain
pScan
rMean
rCovariance

Definition at line 1639 of file Mapper.cpp.

◆ LinkNearChains()

void karto::MapperGraph::LinkNearChains ( LocalizedRangeScan pScan,
Pose2Vector rMeans,
std::vector< Matrix3 > &  rCovariances 
)
private

Find nearby chains of scans and link them to scan if response is high enough

Parameters
pScan
rMeans
rCovariances

Definition at line 1616 of file Mapper.cpp.

◆ LinkScans()

void karto::MapperGraph::LinkScans ( LocalizedRangeScan pFromScan,
LocalizedRangeScan pToScan,
const Pose2 rMean,
const Matrix3 rCovariance 
)
private

Adds an edge between the two scans and labels the edge with the given mean and covariance

Parameters
pFromScan
pToScan
rMean
rCovariance

Definition at line 1594 of file Mapper.cpp.

◆ serialize()

template<class Archive >
void karto::MapperGraph::serialize ( Archive &  ar,
const unsigned int  version 
)
inlineprivate

Definition at line 920 of file Mapper.h.

◆ TryCloseLoop()

kt_bool karto::MapperGraph::TryCloseLoop ( LocalizedRangeScan pScan,
const Name rSensorName 
)

Tries to close a loop using the given scan with the scans from the given device

Parameters
pScan
rSensorName

Definition at line 1476 of file Mapper.cpp.

Friends And Related Function Documentation

◆ boost::serialization::access

friend class boost::serialization::access
friend

Serialization: class MapperGraph

Definition at line 918 of file Mapper.h.

Member Data Documentation

◆ m_pLoopScanMatcher

ScanMatcher* karto::MapperGraph::m_pLoopScanMatcher
private

Scan matcher for loop closures

Definition at line 908 of file Mapper.h.

◆ m_pMapper

Mapper* karto::MapperGraph::m_pMapper
private

Mapper of this graph

Definition at line 903 of file Mapper.h.

◆ m_pTraversal

GraphTraversal<LocalizedRangeScan>* karto::MapperGraph::m_pTraversal
private

Traversal algorithm to find near linked scans

Definition at line 913 of file Mapper.h.


The documentation for this class was generated from the following files:


slam_toolbox
Author(s): Steve Macenski
autogenerated on Sat Oct 3 2020 03:51:02