#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)

void	AddVertex (LocalizedRangeScan *pScan)

LocalizedRangeScanVector	FindNearLinkedScans (LocalizedRangeScan *pScan, kt_double maxDistance)

ScanMatcher *	GetLoopScanMatcher () const

	MapperGraph (Mapper *pMapper, kt_double rangeThreshold)

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 VertexMap &	GetVertices () const

	Graph ()

virtual	~Graph ()

Private Member Functions
Pose2	ComputeWeightedMean (const Pose2Vector &rMeans, const std::vector< Matrix3 > &rCovariances) const

void	CorrectPoses ()

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)

Private Attributes
ScanMatcher *	m_pLoopScanMatcher

Mapper *	m_pMapper

GraphTraversal< LocalizedRangeScan > *	m_pTraversal

Additional Inherited Members
Public Types inherited from karto::Graph< LocalizedRangeScan >
typedef std::map< Name, std::vector< Vertex< LocalizedRangeScan > * > >	VertexMap

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 548 of file Mapper.h.

Constructor & Destructor Documentation

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

Graph for graph SLAM

Parameters

pMapper
rangeThreshold

Definition at line 1125 of file Mapper.cpp.

karto::MapperGraph::~MapperGraph ( )

virtual

Destructor

Definition at line 1136 of file Mapper.cpp.

Member Function Documentation

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 1313 of file Mapper.cpp.

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 1160 of file Mapper.cpp.

void karto::MapperGraph::AddVertex ( LocalizedRangeScan * pScan )

Adds a vertex representing the given scan to the graph

Parameters

pScan

Definition at line 1145 of file Mapper.cpp.

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 1506 of file Mapper.cpp.

void karto::MapperGraph::CorrectPoses ( )

private

Optimizes scan poses

Definition at line 1600 of file Mapper.cpp.

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 1398 of file Mapper.cpp.

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 1497 of file Mapper.cpp.

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 1550 of file Mapper.cpp.

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 1292 of file Mapper.cpp.

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

inline

Gets the graph's scan matcher

Returns: scan matcher

Definition at line 607 of file Mapper.h.

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 618 of file Mapper.h.

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 1381 of file Mapper.cpp.

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 1358 of file Mapper.cpp.

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 1341 of file Mapper.cpp.

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 1226 of file Mapper.cpp.

Member Data Documentation

ScanMatcher* karto::MapperGraph::m_pLoopScanMatcher

private

Scan matcher for loop closures

Definition at line 703 of file Mapper.h.

Mapper* karto::MapperGraph::m_pMapper

private

Mapper of this graph

Definition at line 698 of file Mapper.h.

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

private

Traversal algorithm to find near linked scans

Definition at line 708 of file Mapper.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation