#include <OpenMapper.h>
Graph for graph SLAM algorithm
Definition at line 563 of file OpenMapper.h.
karto::MapperGraph::MapperGraph | ( | OpenMapper * | pOpenMapper, |
kt_double | rangeThreshold | ||
) |
Graph for graph SLAM
pOpenMapper | mapper |
rangeThreshold | range threshold |
Definition at line 1429 of file OpenMapper.cpp.
karto::MapperGraph::~MapperGraph | ( | ) | [virtual] |
Destructor
Definition at line 1438 of file OpenMapper.cpp.
Edge< LocalizedObjectPtr > * karto::MapperGraph::AddEdge | ( | LocalizedObject * | pSourceObject, |
LocalizedObject * | pTargetObject, | ||
kt_bool & | rIsNewEdge | ||
) |
Creates an edge between the source object and the target object if it does not already exist; otherwise return the existing edge
pSourceObject | source object |
pTargetObject | target object |
rIsNewEdge | set to true if the edge is new |
Definition at line 1653 of file OpenMapper.cpp.
void karto::MapperGraph::AddEdges | ( | LocalizedObject * | pObject | ) |
void karto::MapperGraph::AddEdges | ( | LocalizedLaserScan * | pScan, |
const Matrix3 & | rCovariance | ||
) |
Links scan to last scan and nearby chains of scans
pScan | scan |
rCovariance | match uncertainty |
Definition at line 1490 of file OpenMapper.cpp.
void karto::MapperGraph::AddVertex | ( | LocalizedObject * | pObject | ) |
Adds a vertex representing the given object to the graph
pObject | object |
Definition at line 1447 of file OpenMapper.cpp.
Pose2 karto::MapperGraph::ComputeWeightedMean | ( | const Pose2List & | rMeans, |
const List< Matrix3 > & | rCovariances | ||
) | const [private] |
Compute mean of poses weighted by covariances
rMeans | list of poses |
rCovariances | uncertainties |
Definition at line 2033 of file OpenMapper.cpp.
void karto::MapperGraph::CorrectPoses | ( | ) | [private] |
Optimizes scan poses
Definition at line 2125 of file OpenMapper.cpp.
List< LocalizedLaserScanList > karto::MapperGraph::FindNearChains | ( | LocalizedLaserScan * | pScan | ) | [private] |
Finds chains of scans that are close to given scan
pScan | scan |
Definition at line 1861 of file OpenMapper.cpp.
LocalizedLaserScanList karto::MapperGraph::FindNearLinkedScans | ( | LocalizedLaserScan * | pScan, |
kt_double | maxDistance | ||
) |
Finds "nearby" (no further than given distance away) scans through graph links
pScan | scan |
maxDistance | maximum distance |
Definition at line 1987 of file OpenMapper.cpp.
LocalizedLaserScanList karto::MapperGraph::FindOverlappingScans | ( | karto::LocalizedLaserScan * | pScan | ) |
Finds scans that overlap the given scan (based on bounding boxes)
pScan | scan |
Definition at line 2007 of file OpenMapper.cpp.
LocalizedLaserScanList karto::MapperGraph::FindPossibleLoopClosure | ( | LocalizedLaserScan * | pScan, |
const Identifier & | rSensorName, | ||
kt_int32u & | rStartScanIndex | ||
) | [private] |
Tries to find a chain of scan from the given sensor starting at the given scan index that could possibly close a loop with the given scan
pScan | scan |
rSensorName | name of sensor |
rStartScanIndex | start index |
Definition at line 2077 of file OpenMapper.cpp.
LocalizedLaserScan * karto::MapperGraph::GetClosestScanToPose | ( | const LocalizedLaserScanList & | rScans, |
const Pose2 & | rPose | ||
) | const [private] |
Finds the closest scan in the vector to the given pose
rScans | scan |
rPose | pose |
Definition at line 1633 of file OpenMapper.cpp.
ScanMatcher* karto::MapperGraph::GetLoopScanMatcher | ( | ) | const [inline] |
Vertex<LocalizedObjectPtr>* karto::MapperGraph::GetVertex | ( | LocalizedObject * | pObject | ) | [inline, private] |
Gets the vertex associated with the given scan
pScan | scan |
Definition at line 655 of file OpenMapper.h.
void karto::MapperGraph::LinkChainToScan | ( | const LocalizedLaserScanList & | rChain, |
LocalizedLaserScan * | pScan, | ||
const Pose2 & | rMean, | ||
const Matrix3 & | rCovariance | ||
) |
Links the chain of scans to the given scan by finding the closest scan in the chain to the given scan
rChain | chain |
pScan | scan |
rMean | mean |
rCovariance | match uncertainty |
Definition at line 1842 of file OpenMapper.cpp.
void karto::MapperGraph::LinkNearChains | ( | LocalizedLaserScan * | pScan, |
Pose2List & | rMeans, | ||
List< Matrix3 > & | rCovariances | ||
) | [private] |
Finds nearby chains of scans and link them to scan if response is high enough
pScan | scan |
rMeans | means |
rCovariances | match uncertainties |
Definition at line 1761 of file OpenMapper.cpp.
void karto::MapperGraph::LinkObjects | ( | LocalizedObject * | pFromObject, |
LocalizedObject * | pToObject, | ||
const Pose2 & | rMean, | ||
const Matrix3 & | rCovariance | ||
) | [private] |
Adds an edge between the two objects and labels the edge with the given mean and covariance
pFromObject | from object |
pToObject | to object |
rMean | mean |
rCovariance | match uncertainty |
Definition at line 1680 of file OpenMapper.cpp.
kt_bool karto::MapperGraph::TryCloseLoop | ( | LocalizedLaserScan * | pScan, |
const Identifier & | rSensorName | ||
) |
Tries to close a loop using the given scan with the scans from the given sensor
pScan | scan |
rSensorName | name of sensor |
Definition at line 1551 of file OpenMapper.cpp.
Scan matcher for loop closures
Definition at line 723 of file OpenMapper.h.
OpenMapper* karto::MapperGraph::m_pOpenMapper [private] |
Mapper of this graph
Definition at line 718 of file OpenMapper.h.
Traversal algorithm to find near linked scans
Definition at line 728 of file OpenMapper.h.