#include <Mapper.h>

Inheritance diagram for karto::MapperGraph:

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)

ScanMatcher *	GetLoopScanMatcher () const

	MapperGraph ()

	MapperGraph (Mapper *pMapper, kt_double rangeThreshold)

kt_bool	TryCloseLoop (LocalizedRangeScan *pScan, const Name &rSensorName)

void	UpdateLoopScanMatcher (kt_double rangeThreshold)

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 VertexMap &	GetVertices () 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< LocalizedRangeScanVector >	FindNearChains (LocalizedRangeScan *pScan)

LocalizedRangeScanVector	FindPossibleLoopClosure (LocalizedRangeScan *pScan, const Name &rSensorName, kt_int32u &rStartNum)

LocalizedRangeScan *	GetClosestScanToPose (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
ScanMatcher *	m_pLoopScanMatcher

Mapper *	m_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 1430 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 1441 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
rIsNewEdge	set to true if the edge is new

Returns: edge between source and target scan vertices

Definition at line 1631 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
rCovariance	uncertainty of match

Definition at line 1473 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 1455 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 1945 of file Mapper.cpp.

◆ CorrectPoses()

void karto::MapperGraph::CorrectPoses ( )

Optimizes scan poses

Definition at line 2044 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 1904 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 1853 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 1865 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 1724 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 1835 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 1844 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 1989 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 1610 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 823 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 1707 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 1684 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 1662 of file Mapper.cpp.

◆ serialize()

template<class Archive >

void karto::MapperGraph::serialize	(	Archive &	ar,
		const unsigned int	version
	)

inlineprivate

Definition at line 926 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 1544 of file Mapper.cpp.

◆ UpdateLoopScanMatcher()

void karto::MapperGraph::UpdateLoopScanMatcher ( kt_double rangeThreshold )

Create new scan matcher for graph

Parameters

rangeThreshold

Definition at line 2066 of file Mapper.cpp.

Friends And Related Function Documentation

◆ boost::serialization::access

friend class boost::serialization::access

friend

Serialization: class MapperGraph

Definition at line 924 of file Mapper.h.

Member Data Documentation

◆ m_pLoopScanMatcher

ScanMatcher* karto::MapperGraph::m_pLoopScanMatcher

private

Scan matcher for loop closures

Definition at line 914 of file Mapper.h.

◆ m_pMapper

Mapper* karto::MapperGraph::m_pMapper

private

Mapper of this graph

Definition at line 909 of file Mapper.h.

◆ m_pTraversal

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

private

Traversal algorithm to find near linked scans

Definition at line 919 of file Mapper.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Friends

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ MapperGraph() [1/2]

◆ MapperGraph() [2/2]

◆ ~MapperGraph()

Member Function Documentation

◆ AddEdge()

◆ AddEdges()

◆ AddVertex()

◆ ComputeWeightedMean()

◆ CorrectPoses()

◆ FindNearByScan()

◆ FindNearByScans()

◆ FindNearByVertices()

◆ FindNearChains()

◆ FindNearLinkedScans()

◆ FindNearLinkedVertices()

◆ FindPossibleLoopClosure()

◆ GetClosestScanToPose()

◆ GetLoopScanMatcher()

◆ GetVertex()

◆ LinkChainToScan()

◆ LinkNearChains()

◆ LinkScans()

◆ serialize()

◆ TryCloseLoop()

◆ UpdateLoopScanMatcher()

Friends And Related Function Documentation

◆ boost::serialization::access

Member Data Documentation

◆ m_pLoopScanMatcher

◆ m_pMapper

◆ m_pTraversal