#include <OpenMapper.h>
Graph SLAM mapper. Creates a map given a set of localized laser scans. The current Karto implementation is a high-performance scan-matching algorithm that constructs a map from individual scans and corrects for errors in the range and odometry data.
The mapper supports multi-robot mapping from a common starting point. NOTE: When using multiple sensors, the range thresholds of all sensors must be the same.
The following parameters can be set on the Mapper.
UseScanMatching (Parameter<kt_bool>)
When set to true, the mapper will use a scan-matching algorithm. In most real-world situations this should be set to true so that the mapper algorithm can correct for noise and errors in odometry and scan data. In some simulator environments where the simulated scan and odometry data are very accurate, the scan-matching algorithm can produce worse results. In those cases, set to false to improve results. Default value is true.
UseScanBarycenter (Parameter<kt_bool>)
When set to true, the mapper will use the scan's barycenter (the centroid of the point readings) as the position of the scan when calculating distances between scans when determining if scans are far (or near) "enough" to each other. Default value is true.
MinimumTravelDistance (Parameter<kt_double> - meters)
Sets the minimum travel distance between scans. If a new scan's position is more than this value from the previous scan, the mapper will use the data from the new scan. Otherwise, it will discard the new scan if it also does not meet the minimum change in heading requirement. For performance reasons, generally it is a good idea to only process scans if the robot has moved a reasonable amount. Default value is 0.2 (meters).
MinimumTravelHeading (Parameter<kt_double> - radians)
Sets the minimum heading change between scans. If a new scan's heading is more than this value from the previous scan, the mapper will use the data from the new scan. Otherwise, it will discard the new scan if it also does not meet the minimum travel distance requirement. For performance reasons, generally it is a good idea to only process scans if the robot has moved a reasonable amount. Default value is the equivalent value in radians of 20 degrees.
ScanBufferSize (Parameter<kt_int32u> - size)
Scan buffer size is the length of the scan chain stored for scan matching. This value should be set to approximately "scanBufferMaximumScanDistance" / "minimumTravelDistance". The idea is to get an area approximately 20 meters long for scan matching. For example, if we add scans every minimumTravelDistance == 0.3 meters, then "scanBufferSize" should be 20 / 0.3 = 67.) Default value is 70.
ScanBufferMaximumScanDistance (Parameter<kt_double> - meters)
Scan buffer maximum scan distance is the maximum distance between the first and last scans in the scan chain stored for matching. Default value is 20.0.
UseResponseExpansion (Parameter<kt_bool>)
Whether to increase the search space if no good matches are initially found. Default value is false.
DistanceVariancePenalty (Parameter<kt_double> - meters^2)
Variance of penalty for deviating from odometry when scan-matching. Use lower values if the robot's odometer is more accurate. Default value is 0.3.
MinimumDistancePenalty (Parameter<kt_double>)
Minimum value of the penalty multiplier so scores do not become too small. Default value is 0.5.
AngleVariancePenalty (Parameter<kt_double> - radians^2)
Variance of penalty for deviating from odometry when scan-matching. Use lower values if the robot's odometer is more accurate. Default value is the equivalent value in radians of 20 degrees^2.
MinimumAnglePenalty (Parameter<kt_double>)
Minimum value of the penalty multiplier so scores do not become too small. Angles are not very accurate and thus deviations from the odometer should not cause scores to drop precipitously. Default value is 0.9.
LinkMatchMinimumResponseFine (Parameter<kt_double> - probability (>= 0.0, <= 1.0))
Scans are linked only if the correlation response value is greater than this value. Default value is 0.6
LinkScanMaximumDistance (Parameter<kt_double> - meters)
Maximum distance between linked scans. Scans that are farther apart will not be linked regardless of the correlation response value. If this number is large, many scans will be considered "near". This can cause the mapper to aggressively try to close loops with scans that aren't linked to the current scan even though they aren't that close. Default value is 5.0 meters.
CorrelationSearchSpaceDimension (Parameter<kt_double> - meters)
The size of the search grid used by the matcher when matching sequential scans. Default value is 0.3 meters which tells the matcher to use a 30cm x 30cm grid.
CorrelationSearchSpaceResolution (Parameter<kt_double> - meters)
The resolution (size of a grid cell) of the correlation grid. Default value is 0.01 meters.
CorrelationSearchSpaceSmearDeviation (Parameter<kt_double> - meters)
The point readings are smeared by this value in X and Y to create a smoother response. Use higher values for environments that do not have crisp outlines (e.g., outdoor environments with bushes) or where the ground is uneven and rough (but flat overall). Default value is 0.03 meters.
CoarseSearchAngleOffset (Parameter<kt_double> - radians)
The range of angles to search in each direction from the robot's heading during a coarse search. Default value is the equivalent value in radians of 20 degrees.
FineSearchAngleOffset (Parameter<kt_double> - radians)
The range of angles to search in each direction from the robot's heading during a finer search. Default value is the equivalent value in radians of 0.2 degrees.
CoarseAngleResolution (Parameter<kt_double> - meters)
Resolution of angles to search during a coarse search Default value is the equivalent value in radians of 2 degrees.
LoopSearchSpaceDimension (Parameter<kt_double> - meters)
The size of the search grid used by the matcher when detecting loop closures. Default value is 8.0 meters which tells the matcher to use a 8m x 8m grid.
LoopSearchSpaceResolution (Parameter<kt_double> - meters)
The resolution (size of a grid cell) of the correlation grid used by the matcher to determine a possible loop closure. Default value is 0.05 meters.
LoopSearchSpaceSmearDeviation (Parameter<kt_double> - meters)
The point readings are smeared by this value in X and Y to create a smoother response. Used by the matcher to determine a possible loop closure match. Default value is 0.03 meters.
LoopSearchMaximumDistance (Parameter<kt_double> - meters)
Scans less than this distance from the current position will be considered for a match in loop closure. Default value is 4.0 meters.
LoopMatchMinimumChainSize (Parameter<kt_int32u>)
When the loop closure detection finds a candidate it must be part of a large set of linked scans. If the chain of scans is less than this value, we do not attempt to close the loop. Default value is 10.
LoopMatchMaximumVarianceCoarse (Parameter<kt_double>)
The co-variance values for a possible loop closure have to be less than this value to consider a viable solution. This applies to the coarse search. Default value is 0.16.
LoopMatchMinimumResponseCoarse (Parameter<kt_double> - probability (>= 0.0, <= 1.0))
If response is larger then this, then initiate loop closure search at the coarse resolution. Default value is 0.7.
LoopMatchMinimumResponseFine (Parameter<kt_double> - probability (>= 0.0, <= 1.0))
If response is larger then this, then initiate loop closure search at the fine resolution. Default value is 0.7.
Definition at line 1531 of file OpenMapper.h.
karto::OpenMapper::OpenMapper | ( | kt_bool | multiThreaded = true | ) |
Default constructor
multiThreaded | Default constructor |
Definition at line 2159 of file OpenMapper.cpp.
karto::OpenMapper::OpenMapper | ( | const char * | pName, |
kt_bool | multiThreaded = true |
||
) |
Create a Mapper with a name
pName | mapper name |
multiThreaded | Default constructor |
Definition at line 2174 of file OpenMapper.cpp.
karto::OpenMapper::OpenMapper | ( | const OpenMapper & | ) | [private] |
const LocalizedObjectList karto::OpenMapper::GetAllProcessedObjects | ( | ) | const |
Returns all processed objects added to the mapper. NOTE: The returned objects have their corrected pose updated.
Definition at line 2433 of file OpenMapper.cpp.
const LocalizedLaserScanList karto::OpenMapper::GetAllProcessedScans | ( | ) | const |
Returns all processed scans added to the mapper. NOTE: The returned scans have their corrected pose updated.
Definition at line 2421 of file OpenMapper.cpp.
MapperGraph * karto::OpenMapper::GetGraph | ( | ) | const [virtual] |
ScanMatcher * karto::OpenMapper::GetLoopScanMatcher | ( | ) | const |
Gets the loop scan matcher
Definition at line 2466 of file OpenMapper.cpp.
MapperSensorManager* karto::OpenMapper::GetMapperSensorManager | ( | ) | const [inline] |
ScanSolver * karto::OpenMapper::GetScanSolver | ( | ) | const |
Gets scan optimizer used by mapper when closing the loop
Definition at line 2446 of file OpenMapper.cpp.
Gets the sequential scan matcher
Definition at line 2461 of file OpenMapper.cpp.
kt_bool karto::OpenMapper::HasMovedEnough | ( | LocalizedLaserScan * | pScan, |
LocalizedLaserScan * | pLastScan | ||
) | const [private] |
Tests if the scan is "sufficiently far" from the last scan added.
pScan | scan to be checked |
pLastScan | last scan added to mapper |
Definition at line 2393 of file OpenMapper.cpp.
void karto::OpenMapper::Initialize | ( | kt_double | rangeThreshold | ) |
Allocates memory needed for mapping. Please Reset() the mapper if range threshold on the laser range finder is changed after the mapper is initialized.
rangeThreshold | range threshold |
Definition at line 2233 of file OpenMapper.cpp.
void karto::OpenMapper::InitializeParameters | ( | ) | [private] |
Definition at line 2196 of file OpenMapper.cpp.
kt_bool karto::OpenMapper::IsInitialized | ( | ) | [inline] |
Returns initialized flag for mapper
Definition at line 1714 of file OpenMapper.h.
kt_bool karto::OpenMapper::IsMultiThreaded | ( | ) | [inline] |
Is mapper multi threaded
Definition at line 1589 of file OpenMapper.h.
const OpenMapper& karto::OpenMapper::operator= | ( | const OpenMapper & | ) | [private] |
kt_bool karto::OpenMapper::Process | ( | Object * | pObject | ) | [virtual] |
Processes an object. If the object is a scan, the scan must be identified with a range finder sensor. Once added to a map, the corrected pose information in the localized object will be updated to the correct pose as determined by the mapper.
pObject | object |
Reimplemented from karto::Module.
Definition at line 2269 of file OpenMapper.cpp.
void karto::OpenMapper::Reset | ( | ) | [virtual] |
Resets the mapper. Deallocates memory allocated in Initialize()
Reimplemented from karto::Module.
Definition at line 2253 of file OpenMapper.cpp.
virtual void karto::OpenMapper::ScanMatched | ( | LocalizedLaserScan * | pScan | ) | [inline, protected, virtual] |
Hook called after scan matching and before trying loop closure
pScan | scan that was scan-matched |
Definition at line 1687 of file OpenMapper.h.
virtual void karto::OpenMapper::ScanMatchingEnd | ( | LocalizedLaserScan * | pScan | ) | [inline, protected, virtual] |
Hook called after scan matching and closing any loops
pScan | scan that was scan-matched |
Definition at line 1693 of file OpenMapper.h.
void karto::OpenMapper::SetScanSolver | ( | ScanSolver * | pSolver | ) |
Sets scan optimizer used by mapper when closing the loop
pSolver | solver |
Definition at line 2451 of file OpenMapper.cpp.
kt_bool karto::OpenMapper::TryCloseLoop | ( | LocalizedLaserScan * | pScan, |
const Identifier & | rSensorName | ||
) | [inline] |
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 1677 of file OpenMapper.h.
friend class MapperGraph [friend] |
Definition at line 1533 of file OpenMapper.h.
friend class ScanMatcher [friend] |
Definition at line 1534 of file OpenMapper.h.
kt_bool karto::OpenMapper::m_Initialized [private] |
Definition at line 1731 of file OpenMapper.h.
kt_bool karto::OpenMapper::m_MultiThreaded [private] |
Definition at line 1732 of file OpenMapper.h.
Definition at line 1759 of file OpenMapper.h.
Definition at line 1773 of file OpenMapper.h.
Definition at line 1771 of file OpenMapper.h.
Definition at line 1768 of file OpenMapper.h.
Definition at line 1769 of file OpenMapper.h.
Definition at line 1770 of file OpenMapper.h.
Definition at line 1757 of file OpenMapper.h.
Definition at line 1772 of file OpenMapper.h.
MapperGraph* karto::OpenMapper::m_pGraph [private] |
Definition at line 1738 of file OpenMapper.h.
Definition at line 1762 of file OpenMapper.h.
Definition at line 1763 of file OpenMapper.h.
Definition at line 1787 of file OpenMapper.h.
Definition at line 1786 of file OpenMapper.h.
Definition at line 1788 of file OpenMapper.h.
Definition at line 1789 of file OpenMapper.h.
Definition at line 1785 of file OpenMapper.h.
Definition at line 1778 of file OpenMapper.h.
Definition at line 1779 of file OpenMapper.h.
Definition at line 1780 of file OpenMapper.h.
Definition at line 1736 of file OpenMapper.h.
Definition at line 1760 of file OpenMapper.h.
Definition at line 1758 of file OpenMapper.h.
Definition at line 1747 of file OpenMapper.h.
Definition at line 1748 of file OpenMapper.h.
Definition at line 1754 of file OpenMapper.h.
Parameter<kt_int32u>* karto::OpenMapper::m_pScanBufferSize [private] |
Definition at line 1753 of file OpenMapper.h.
SmartPointer<ScanSolver> karto::OpenMapper::m_pScanSolver [protected] |
Solver used to optimize scan poses on pose graph
Definition at line 1728 of file OpenMapper.h.
Definition at line 1734 of file OpenMapper.h.
Definition at line 1755 of file OpenMapper.h.
Parameter<kt_bool>* karto::OpenMapper::m_pUseScanBarycenter [private] |
Definition at line 1746 of file OpenMapper.h.
Parameter<kt_bool>* karto::OpenMapper::m_pUseScanMatching [private] |
Definition at line 1745 of file OpenMapper.h.
Event for generic mapper message. The mapper currently posts Message events when calculating possible loop closures and when rejecting a possible loop close.
Definition at line 1556 of file OpenMapper.h.
Event fires after a loop was closed.
Definition at line 1568 of file OpenMapper.h.
Event fires before a loop closure is about to happen. Calculating loop closure can be a CPU intensive task and the Mapper is blocking all Process calls. If mapper is running on "real" robot please either pause the robot or stop adding scans to the mapper.
Definition at line 1563 of file OpenMapper.h.
Event fires when the scans have been changed. Scans are changed after a loop closure and after any translation, rotation or transform.
Definition at line 1574 of file OpenMapper.h.