#include <OpenMapper.h>
Public Member Functions | |
kt_double | CorrelateScan (ScanMatcherGridSet *pScanMatcherGridSet, LocalizedLaserScan *pScan, const Pose2 &rSearchCenter, const Vector2d &rSearchSpaceOffset, const Vector2d &rSearchSpaceResolution, kt_double searchAngleOffset, kt_double searchAngleResolution, kt_bool doPenalize, Pose2 &rMean, Matrix3 &rCovariance, kt_bool doingFineMatch) |
CorrelationGrid * | GetCorrelationGrid () const |
Grid< kt_double > * | GetSearchGrid () const |
kt_double | MatchScan (LocalizedLaserScan *pScan, const LocalizedLaserScanList &rBaseScans, Pose2 &rMean, Matrix3 &rCovariance, kt_bool doPenalize=true, kt_bool doRefineMatch=true) |
virtual | ~ScanMatcher () |
Static Public Member Functions | |
static void | ComputeAngularCovariance (ScanMatcherGridSet *pScanMatcherGridSet, const Pose2 &rBestPose, kt_double bestResponse, const Pose2 &rSearchCenter, kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3 &rCovariance) |
static void | ComputePositionalCovariance (Grid< kt_double > *pSearchSpaceProbs, const Pose2 &rBestPose, kt_double bestResponse, const Pose2 &rSearchCenter, const Vector2d &rSearchSpaceOffset, const Vector2d &rSearchSpaceResolution, kt_double searchAngleResolution, Matrix3 &rCovariance) |
static ScanMatcher * | Create (OpenMapper *pOpenMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold) |
static kt_double | GetResponse (ScanMatcherGridSet *pScanMatcherGridSet, kt_int32u angleIndex, kt_int32s gridPositionIndex) |
Protected Member Functions | |
ScanMatcher (OpenMapper *pOpenMapper) | |
Static Private Member Functions | |
static void | AddScan (CorrelationGrid *pCorrelationGrid, LocalizedLaserScan *pScan, const Vector2d &rViewPoint, kt_bool doSmear=true) |
static void | AddScanNew (CorrelationGrid *pCorrelationGrid, const Vector2dList &rValidPoints, kt_bool doSmear=true) |
static void | AddScans (CorrelationGrid *pCorrelationGrid, const LocalizedLaserScanList &rScans, const Vector2d &rViewPoint) |
static void | AddScansNew (CorrelationGrid *pCorrelationGrid, const LocalizedLaserScanList &rScans, const Vector2d &rViewPoint) |
static Vector2dList | FindValidPoints (LocalizedLaserScan *pScan, const Vector2d &rViewPoint) |
Private Attributes | |
OpenMapper * | m_pOpenMapper |
SmartPointer< ScanMatcherGridSet > | m_pScanMatcherGridSet |
ScanMatcherGridSetBank * | m_pScanMatcherGridSetBank |
Scan matcher
Definition at line 1078 of file OpenMapper.h.
karto::ScanMatcher::~ScanMatcher | ( | ) | [virtual] |
Destructor
Definition at line 436 of file OpenMapper.cpp.
karto::ScanMatcher::ScanMatcher | ( | OpenMapper * | pOpenMapper | ) | [inline, protected] |
Default constructor
Definition at line 1216 of file OpenMapper.h.
void karto::ScanMatcher::AddScan | ( | CorrelationGrid * | pCorrelationGrid, |
LocalizedLaserScan * | pScan, | ||
const Vector2d & | rViewPoint, | ||
kt_bool | doSmear = true |
||
) | [static, private] |
Marks cells where scans' points hit as being occupied. Can smear points as they are added.
pCorrelationGrid | correlation grid used for scan matching |
pScan | scan whose points will mark cells in grid as being occupied |
viewPoint | do not add points that belong to scans "opposite" the view point |
doSmear | whether the points will be smeared |
Definition at line 1124 of file OpenMapper.cpp.
void karto::ScanMatcher::AddScanNew | ( | CorrelationGrid * | pCorrelationGrid, |
const Vector2dList & | rValidPoints, | ||
kt_bool | doSmear = true |
||
) | [static, private] |
Definition at line 1157 of file OpenMapper.cpp.
void karto::ScanMatcher::AddScans | ( | CorrelationGrid * | pCorrelationGrid, |
const LocalizedLaserScanList & | rScans, | ||
const Vector2d & | rViewPoint | ||
) | [static, private] |
Marks cells where scans' points hit as being occupied
pCorrelationGrid | correlation grid used for scan matching |
rScans | scans whose points will mark cells in grid as being occupied |
viewPoint | do not add points that belong to scans "opposite" the view point |
Definition at line 1084 of file OpenMapper.cpp.
void karto::ScanMatcher::AddScansNew | ( | CorrelationGrid * | pCorrelationGrid, |
const LocalizedLaserScanList & | rScans, | ||
const Vector2d & | rViewPoint | ||
) | [static, private] |
Definition at line 1095 of file OpenMapper.cpp.
void karto::ScanMatcher::ComputeAngularCovariance | ( | ScanMatcherGridSet * | pScanMatcherGridSet, |
const Pose2 & | rBestPose, | ||
kt_double | bestResponse, | ||
const Pose2 & | rSearchCenter, | ||
kt_double | searchAngleOffset, | ||
kt_double | searchAngleResolution, | ||
Matrix3 & | rCovariance | ||
) | [static] |
Computes the angular covariance of the best pose
pScanMatcherGridSet | set of grids used for scan matching |
rBestPose | best pose |
bestResponse | best response |
rSearchCenter | center of search space |
searchAngleOffset | amount to search in each angular direction |
searchAngleResolution | angular resolution of search |
rCovariance | output parameter covariance |
Definition at line 1033 of file OpenMapper.cpp.
void karto::ScanMatcher::ComputePositionalCovariance | ( | Grid< kt_double > * | pSearchSpaceProbs, |
const Pose2 & | rBestPose, | ||
kt_double | bestResponse, | ||
const Pose2 & | rSearchCenter, | ||
const Vector2d & | rSearchSpaceOffset, | ||
const Vector2d & | rSearchSpaceResolution, | ||
kt_double | searchAngleResolution, | ||
Matrix3 & | rCovariance | ||
) | [static] |
Computes the positional covariance of the best pose
pSearchSpaceProbs | probabilities of poses over search space |
rBestPose | best pose |
bestResponse | best response |
rSearchCenter | center of search space |
rSearchSpaceOffset | amount to search in each direction |
rSearchSpaceResolution | grid resolution of search |
searchAngleResolution | angular resolution of search |
rCovariance | output parameter covariance |
Definition at line 939 of file OpenMapper.cpp.
kt_double karto::ScanMatcher::CorrelateScan | ( | ScanMatcherGridSet * | pScanMatcherGridSet, |
LocalizedLaserScan * | pScan, | ||
const Pose2 & | rSearchCenter, | ||
const Vector2d & | rSearchSpaceOffset, | ||
const Vector2d & | 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
pScanMatcherGridSet | set of grids used for scan matching |
pScan | scan to match against correlation grid |
rSearchCenter | the center of the search space |
rSearchSpaceOffset | searches poses in the area offset by this vector around search center |
rSearchSpaceResolution | how fine a granularity to search in the search space |
searchAngleOffset | searches poses in the angles offset by this angle around search center |
searchAngleResolution | how fine a granularity to search in the angular search space |
doPenalize | whether to penalize matches further from the search center |
rMean | output parameter of mean (best pose) of match |
rCovariance | output parameter of covariance of match |
doingFineMatch | whether to do a finer search after coarse search |
Definition at line 718 of file OpenMapper.cpp.
ScanMatcher * karto::ScanMatcher::Create | ( | OpenMapper * | pOpenMapper, |
kt_double | searchSize, | ||
kt_double | resolution, | ||
kt_double | smearDeviation, | ||
kt_double | rangeThreshold | ||
) | [static] |
Creates a scan matcher with the given parameters
pOpenMapper | mapper |
searchSize | how much to search in each direction |
resolution | grid resolution |
smearDeviation | amount to smear when adding scans to grid |
rangeThreshold | cutoff for range readings |
Definition at line 441 of file OpenMapper.cpp.
Vector2dList karto::ScanMatcher::FindValidPoints | ( | LocalizedLaserScan * | pScan, |
const Vector2d & | rViewPoint | ||
) | [static, private] |
Computes which points in a scan are on the same side as the given viewpoint
pScan | scan |
rViewPoint | viewpoint |
Definition at line 1188 of file OpenMapper.cpp.
Gets the correlation grid data (for debugging). NOTE: This only works in single-threaded mode.
Exception | if not in single-threaded mode. |
Definition at line 1287 of file OpenMapper.cpp.
kt_double karto::ScanMatcher::GetResponse | ( | ScanMatcherGridSet * | pScanMatcherGridSet, |
kt_int32u | angleIndex, | ||
kt_int32s | gridPositionIndex | ||
) | [static] |
Gets response at given position for given rotation (only look up valid points)
pScanMatcherGridSet | set of grids used for scan matching |
angleIndex | index of angle |
gridPositionIndex | index of grid position |
Definition at line 1245 of file OpenMapper.cpp.
Grid< kt_double > * karto::ScanMatcher::GetSearchGrid | ( | ) | const |
Gets the search grid data (for debugging). NOTE: This only works in single-threaded mode.
Exception | if not in single-threaded mode. |
Definition at line 1299 of file OpenMapper.cpp.
kt_double karto::ScanMatcher::MatchScan | ( | LocalizedLaserScan * | pScan, |
const LocalizedLaserScanList & | rBaseScans, | ||
Pose2 & | rMean, | ||
Matrix3 & | rCovariance, | ||
kt_bool | doPenalize = true , |
||
kt_bool | doRefineMatch = true |
||
) |
Matches given scan against set of scans
pScan | scan being scan-matched |
rBaseScans | set of scans whose points will mark cells in grid as being occupied |
rMean | output parameter of mean (best pose) of match |
rCovariance | output parameter of covariance of match |
doPenalize | whether to penalize matches further from the search center |
doRefineMatch | whether to do finer-grained matching if coarse match is good |
Definition at line 496 of file OpenMapper.cpp.
OpenMapper* karto::ScanMatcher::m_pOpenMapper [private] |
Definition at line 1224 of file OpenMapper.h.
Definition at line 1226 of file OpenMapper.h.
Definition at line 1227 of file OpenMapper.h.