Public Member Functions | Static Public Member Functions | Protected Member Functions | Private Member Functions | Private Attributes | Friends | List of all members
karto::ScanMatcher Class Reference

#include <Mapper.h>

Public Member Functions

void ComputeAngularCovariance (const Pose2 &rBestPose, kt_double bestResponse, const Pose2 &rSearchCenter, kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3 &rCovariance)
 
void ComputePositionalCovariance (const Pose2 &rBestPose, kt_double bestResponse, const Pose2 &rSearchCenter, const Vector2< kt_double > &rSearchSpaceOffset, const Vector2< kt_double > &rSearchSpaceResolution, kt_double searchAngleResolution, Matrix3 &rCovariance)
 
kt_double CorrelateScan (LocalizedRangeScan *pScan, const Pose2 &rSearchCenter, const Vector2< kt_double > &rSearchSpaceOffset, const Vector2< kt_double > &rSearchSpaceResolution, kt_double searchAngleOffset, kt_double searchAngleResolution, kt_bool doPenalize, Pose2 &rMean, Matrix3 &rCovariance, kt_bool doingFineMatch)
 
CorrelationGridGetCorrelationGrid () const
 
template<class T = LocalizedRangeScanVector>
kt_double MatchScan (LocalizedRangeScan *pScan, const T &rBaseScans, Pose2 &rMean, Matrix3 &rCovariance, kt_bool doPenalize=true, kt_bool doRefineMatch=true)
 
void operator() (const kt_double &y) const
 
 ScanMatcher ()
 
virtual ~ScanMatcher ()
 

Static Public Member Functions

static ScanMatcherCreate (Mapper *pMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold)
 

Protected Member Functions

 ScanMatcher (Mapper *pMapper)
 

Private Member Functions

void AddScan (LocalizedRangeScan *pScan, const Vector2< kt_double > &rViewPoint, kt_bool doSmear=true)
 
void AddScans (const LocalizedRangeScanMap &rScans, Vector2< kt_double > viewPoint)
 
void AddScans (const LocalizedRangeScanVector &rScans, Vector2< kt_double > viewPoint)
 
PointVectorDouble FindValidPoints (LocalizedRangeScan *pScan, const Vector2< kt_double > &rViewPoint) const
 
kt_double GetResponse (kt_int32u angleIndex, kt_int32s gridPositionIndex) const
 
template<class Archive >
void serialize (Archive &ar, const unsigned int version)
 

Private Attributes

kt_bool m_doPenalize
 
kt_int32u m_nAngles
 
CorrelationGridm_pCorrelationGrid
 
GridIndexLookup< kt_int8u > * m_pGridLookup
 
Mapperm_pMapper
 
std::pair< kt_double, Pose2 > * m_pPoseResponse
 
Grid< kt_double > * m_pSearchSpaceProbs
 
Pose2 m_rSearchCenter
 
kt_double m_searchAngleOffset
 
kt_double m_searchAngleResolution
 
std::vector< kt_doublem_xPoses
 
std::vector< kt_doublem_yPoses
 

Friends

class boost::serialization::access
 

Detailed Description

Scan matcher

Definition at line 1314 of file Mapper.h.

Constructor & Destructor Documentation

◆ ScanMatcher() [1/2]

karto::ScanMatcher::ScanMatcher ( )
inline

Definition at line 1317 of file Mapper.h.

◆ ~ScanMatcher()

karto::ScanMatcher::~ScanMatcher ( )
virtual

Destructor

Definition at line 468 of file Mapper.cpp.

◆ ScanMatcher() [2/2]

karto::ScanMatcher::ScanMatcher ( Mapper pMapper)
inlineprotected

Default constructor

Definition at line 1464 of file Mapper.h.

Member Function Documentation

◆ AddScan()

void karto::ScanMatcher::AddScan ( LocalizedRangeScan pScan,
const Vector2< kt_double > &  rViewPoint,
kt_bool  doSmear = true 
)
private

Marks cells where scans' points hit as being occupied. Can smear points as they are added.

Parameters
pScanscan whose points will mark cells in grid as being occupied
viewPointdo not add points that belong to scans "opposite" the view point
doSmearwhether the points will be smeared

Definition at line 1100 of file Mapper.cpp.

◆ AddScans() [1/2]

void karto::ScanMatcher::AddScans ( const LocalizedRangeScanMap rScans,
Vector2< kt_double viewPoint 
)
private

Marks cells where scans' points hit as being occupied

Parameters
rScansscans whose points will mark cells in grid as being occupied
viewPointdo not add points that belong to scans "opposite" the view point

Definition at line 1078 of file Mapper.cpp.

◆ AddScans() [2/2]

void karto::ScanMatcher::AddScans ( const LocalizedRangeScanVector rScans,
Vector2< kt_double viewPoint 
)
private

Marks cells where scans' points hit as being occupied

Parameters
rScansscans whose points will mark cells in grid as being occupied
viewPointdo not add points that belong to scans "opposite" the view point

Definition at line 1057 of file Mapper.cpp.

◆ ComputeAngularCovariance()

void karto::ScanMatcher::ComputeAngularCovariance ( const Pose2 rBestPose,
kt_double  bestResponse,
const Pose2 rSearchCenter,
kt_double  searchAngleOffset,
kt_double  searchAngleResolution,
Matrix3 rCovariance 
)

Computes the angular covariance of the best pose

Parameters
rBestPose
bestResponse
rSearchCenter
searchAngleOffset
searchAngleResolution
rCovarianceComputes the angular covariance of the best pose
rBestPose
bestResponse
rSearchCenter
rSearchAngleOffset
searchAngleResolution
rCovariance

Definition at line 999 of file Mapper.cpp.

◆ ComputePositionalCovariance()

void karto::ScanMatcher::ComputePositionalCovariance ( const Pose2 rBestPose,
kt_double  bestResponse,
const Pose2 rSearchCenter,
const Vector2< kt_double > &  rSearchSpaceOffset,
const Vector2< kt_double > &  rSearchSpaceResolution,
kt_double  searchAngleResolution,
Matrix3 rCovariance 
)

Computes the positional covariance of the best pose

Parameters
rBestPose
bestResponse
rSearchCenter
rSearchSpaceOffset
rSearchSpaceResolution
searchAngleResolution
rCovariance

Definition at line 893 of file Mapper.cpp.

◆ CorrelateScan()

kt_double karto::ScanMatcher::CorrelateScan ( LocalizedRangeScan pScan,
const Pose2 rSearchCenter,
const Vector2< kt_double > &  rSearchSpaceOffset,
const Vector2< kt_double > &  rSearchSpaceResolution,
kt_double  searchAngleOffset,
kt_double  searchAngleResolution,
kt_bool  doPenalize,
Pose2 rMean,
Matrix3 rCovariance,
kt_bool  doingFineMatch 
)

Finds the best pose for the scan centering the search in the correlation grid at the given pose and search in the space by the vector and angular offsets in increments of the given resolutions

Parameters
pScanscan to match against correlation grid
rSearchCenterthe center of the search space
rSearchSpaceOffsetsearches poses in the area offset by this vector around search center
rSearchSpaceResolutionhow fine a granularity to search in the search space
searchAngleOffsetsearches poses in the angles offset by this angle around search center
searchAngleResolutionhow fine a granularity to search in the angular search space
doPenalizewhether to penalize matches further from the search center
rMeanoutput parameter of mean (best pose) of match
rCovarianceoutput parameter of covariance of match
doingFineMatchwhether to do a finer search after coarse search
Returns
strength of response

Finds the best pose for the scan centering the search in the correlation grid at the given pose and search in the space by the vector and angular offsets in increments of the given resolutions

Parameters
rScanscan to match against correlation grid
rSearchCenterthe center of the search space
rSearchSpaceOffsetsearches poses in the area offset by this vector around search center
rSearchSpaceResolutionhow fine a granularity to search in the search space
searchAngleOffsetsearches poses in the angles offset by this angle around search center
searchAngleResolutionhow fine a granularity to search in the angular search space
doPenalizewhether to penalize matches further from the search center
rMeanoutput parameter of mean (best pose) of match
rCovarianceoutput parameter of covariance of match
doingFineMatchwhether to do a finer search after coarse search
Returns
strength of response

Definition at line 719 of file Mapper.cpp.

◆ Create()

ScanMatcher * karto::ScanMatcher::Create ( Mapper pMapper,
kt_double  searchSize,
kt_double  resolution,
kt_double  smearDeviation,
kt_double  rangeThreshold 
)
static

Create a scan matcher with the given parameters

Definition at line 485 of file Mapper.cpp.

◆ FindValidPoints()

PointVectorDouble karto::ScanMatcher::FindValidPoints ( LocalizedRangeScan pScan,
const Vector2< kt_double > &  rViewPoint 
) const
private

Compute which points in a scan are on the same side as the given viewpoint

Parameters
pScan
rViewPoint
Returns
points on the same side

Definition at line 1140 of file Mapper.cpp.

◆ GetCorrelationGrid()

CorrelationGrid* karto::ScanMatcher::GetCorrelationGrid ( ) const
inline

Gets the correlation grid data (for debugging)

Returns
correlation grid

Definition at line 1422 of file Mapper.h.

◆ GetResponse()

kt_double karto::ScanMatcher::GetResponse ( kt_int32u  angleIndex,
kt_int32s  gridPositionIndex 
) const
private

Get response at given position for given rotation (only look up valid points)

Parameters
angleIndex
gridPositionIndex
Returns
response

Definition at line 1203 of file Mapper.cpp.

◆ MatchScan()

template<class T >
kt_double karto::ScanMatcher::MatchScan ( LocalizedRangeScan pScan,
const T &  rBaseScans,
Pose2 rMean,
Matrix3 rCovariance,
kt_bool  doPenalize = true,
kt_bool  doRefineMatch = true 
)

Match given scan against set of scans

Parameters
pScanscan being scan-matched
rBaseScansset of scans whose points will mark cells in grid as being occupied
rMeanoutput parameter of mean (best pose) of match
rCovarianceoutput parameter of covariance of match
doPenalizewhether to penalize matches further from the search center
doRefineMatchwhether to do finer-grained matching if coarse match is good (default is true)
Returns
strength of response

Definition at line 544 of file Mapper.cpp.

◆ operator()()

void karto::ScanMatcher::operator() ( const kt_double y) const

Parallelize scan matching

Definition at line 649 of file Mapper.cpp.

◆ serialize()

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

Definition at line 1493 of file Mapper.h.

Friends And Related Function Documentation

◆ boost::serialization::access

friend class boost::serialization::access
friend

Serialization: class ScanMatcher

Definition at line 1491 of file Mapper.h.

Member Data Documentation

◆ m_doPenalize

kt_bool karto::ScanMatcher::m_doPenalize
private

Definition at line 1486 of file Mapper.h.

◆ m_nAngles

kt_int32u karto::ScanMatcher::m_nAngles
private

Definition at line 1484 of file Mapper.h.

◆ m_pCorrelationGrid

CorrelationGrid* karto::ScanMatcher::m_pCorrelationGrid
private

Definition at line 1476 of file Mapper.h.

◆ m_pGridLookup

GridIndexLookup<kt_int8u>* karto::ScanMatcher::m_pGridLookup
private

Definition at line 1478 of file Mapper.h.

◆ m_pMapper

Mapper* karto::ScanMatcher::m_pMapper
private

Definition at line 1475 of file Mapper.h.

◆ m_pPoseResponse

std::pair<kt_double, Pose2>* karto::ScanMatcher::m_pPoseResponse
private

Definition at line 1479 of file Mapper.h.

◆ m_pSearchSpaceProbs

Grid<kt_double>* karto::ScanMatcher::m_pSearchSpaceProbs
private

Definition at line 1477 of file Mapper.h.

◆ m_rSearchCenter

Pose2 karto::ScanMatcher::m_rSearchCenter
private

Definition at line 1482 of file Mapper.h.

◆ m_searchAngleOffset

kt_double karto::ScanMatcher::m_searchAngleOffset
private

Definition at line 1483 of file Mapper.h.

◆ m_searchAngleResolution

kt_double karto::ScanMatcher::m_searchAngleResolution
private

Definition at line 1485 of file Mapper.h.

◆ m_xPoses

std::vector<kt_double> karto::ScanMatcher::m_xPoses
private

Definition at line 1480 of file Mapper.h.

◆ m_yPoses

std::vector<kt_double> karto::ScanMatcher::m_yPoses
private

Definition at line 1481 of file Mapper.h.


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


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:56