Main MRPT website > C++ reference for MRPT 1.5.7
List of all members | Classes | Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes
mrpt::slam::CMetricMapBuilderICP Class Reference

Detailed Description

A class for very simple 2D SLAM based on ICP.

This is a non-probabilistic pose tracking algorithm. Map are stored as in files as binary dumps of "mrpt::maps::CSimpleMap" objects. The methods are thread-safe.

Definition at line 28 of file CMetricMapBuilderICP.h.

#include <mrpt/slam/CMetricMapBuilderICP.h>

Inheritance diagram for mrpt::slam::CMetricMapBuilderICP:
Inheritance graph

Classes

struct  TConfigParams
 Algorithm configuration params. More...
 
struct  TDist
 Traveled distances from last map update / ICP-based localization. More...
 

Public Member Functions

 CMetricMapBuilderICP ()
 Default constructor - Upon construction, you can set the parameters in ICP_options, then call "initialize". More...
 
virtual ~CMetricMapBuilderICP ()
 Destructor: More...
 
void initialize (const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), mrpt::poses::CPosePDF *x0=NULL) MRPT_OVERRIDE
 Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map. More...
 
mrpt::poses::CPose3DPDFPtr getCurrentPoseEstimation () const MRPT_OVERRIDE
 Returns a copy of the current best pose estimation as a pose PDF. More...
 
void setCurrentMapFile (const char *mapFile)
 Sets the "current map file", thus that map will be loaded if it exists or a new one will be created if it does not, and the updated map will be save to that file when destroying the object. More...
 
void processActionObservation (mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &in_SF) MRPT_OVERRIDE
 Appends a new action and observations to update this map: See the description of the class at the top of this page to see a more complete description. More...
 
void processObservation (const mrpt::obs::CObservationPtr &obs)
 The main method of this class: Process one odometry or sensor observation. More...
 
void getCurrentlyBuiltMap (mrpt::maps::CSimpleMap &out_map) const MRPT_OVERRIDE
 Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map. More...
 
void getCurrentMapPoints (std::vector< float > &x, std::vector< float > &y)
 Returns the 2D points of current local map. More...
 
const mrpt::maps::CMultiMetricMapgetCurrentlyBuiltMetricMap () const MRPT_OVERRIDE
 Returns the map built so far. More...
 
unsigned int getCurrentlyBuiltMapSize () MRPT_OVERRIDE
 Returns just how many sensory-frames are stored in the currently build map. More...
 
void saveCurrentEstimationToImage (const std::string &file, bool formatEMF_BMP=true) MRPT_OVERRIDE
 A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file. More...
 
void clear ()
 Clear all elements of the maps, and reset localization to (0,0,0deg). More...
 
void enableMapUpdating (bool enable)
 Enables or disables the map updating (default state is enabled) More...
 
void loadCurrentMapFromFile (const std::string &fileName)
 Load map (mrpt::maps::CSimpleMap) from a ".simplemap" file. More...
 
void saveCurrentMapToFile (const std::string &fileName, bool compressGZ=true) const
 Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file. More...
 

Public Attributes

TConfigParams ICP_options
 Options for the ICP-SLAM application. More...
 
CICP::TConfigParams ICP_params
 Options for the ICP algorithm itself. More...
 
TOptions options
 

Protected Member Functions

void enterCriticalSection ()
 Enter critical section for map updating. More...
 
void leaveCriticalSection ()
 Leave critical section for map updating. More...
 

Protected Attributes

mrpt::synch::CCriticalSection critZoneChangingMap
 Critical zones. More...
 

Private Member Functions

void accumulateRobotDisplacementCounters (const mrpt::poses::CPose2D &new_pose)
 
void resetRobotDisplacementCounters (const mrpt::poses::CPose2D &new_pose)
 

Private Attributes

mrpt::maps::CSimpleMap SF_Poses_seq
 The set of observations that leads to current map: More...
 
mrpt::maps::CMultiMetricMap metricMap
 The metric map representation as a points map: More...
 
std::string currentMapFile
 Current map file. More...
 
mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst
 The pose estimation by the alignment algorithm (ICP). More...
 
mrpt::math::CMatrixDouble33 m_lastPoseEst_cov
 Last pose estimation (covariance) More...
 
std::deque< mrpt::math::TPose2Dm_estRobotPath
 The estimated robot path: More...
 
mrpt::poses::CPose2D m_auxAccumOdometry
 
TDist m_distSinceLastICP
 
mrpt::aligned_containers< std::string, TDist >::map_t m_distSinceLastInsertion
 Indexed by sensor label. More...
 
bool m_there_has_been_an_odometry
 

Constructor & Destructor Documentation

◆ CMetricMapBuilderICP()

CMetricMapBuilderICP::CMetricMapBuilderICP ( )

Default constructor - Upon construction, you can set the parameters in ICP_options, then call "initialize".

Definition at line 32 of file CMetricMapBuilderICP.cpp.

References initialize().

◆ ~CMetricMapBuilderICP()

CMetricMapBuilderICP::~CMetricMapBuilderICP ( )
virtual

Member Function Documentation

◆ accumulateRobotDisplacementCounters()

void CMetricMapBuilderICP::accumulateRobotDisplacementCounters ( const mrpt::poses::CPose2D new_pose)
private

◆ clear()

void CMetricMapBuilder::clear ( )
inherited

Clear all elements of the maps, and reset localization to (0,0,0deg).

Definition at line 42 of file CMetricMapBuilder.cpp.

References mrpt::slam::CMetricMapBuilder::initialize(), and MRPT_LOG_DEBUG.

◆ enableMapUpdating()

void mrpt::slam::CMetricMapBuilder::enableMapUpdating ( bool  enable)
inlineinherited

Enables or disables the map updating (default state is enabled)

Definition at line 88 of file CMetricMapBuilder.h.

◆ enterCriticalSection()

void mrpt::slam::CMetricMapBuilder::enterCriticalSection ( )
inlineprotectedinherited

Enter critical section for map updating.

Definition at line 40 of file CMetricMapBuilder.h.

References mrpt::synch::CCriticalSection::enter().

Referenced by getCurrentMapPoints(), and ~CMetricMapBuilderICP().

◆ getCurrentlyBuiltMap()

void CMetricMapBuilderICP::getCurrentlyBuiltMap ( mrpt::maps::CSimpleMap out_map) const
virtual

Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.

Implements mrpt::slam::CMetricMapBuilder.

Definition at line 520 of file CMetricMapBuilderICP.cpp.

References SF_Poses_seq.

◆ getCurrentlyBuiltMapSize()

unsigned int CMetricMapBuilderICP::getCurrentlyBuiltMapSize ( )
virtual

Returns just how many sensory-frames are stored in the currently build map.

Implements mrpt::slam::CMetricMapBuilder.

Definition at line 535 of file CMetricMapBuilderICP.cpp.

References SF_Poses_seq, and mrpt::maps::CSimpleMap::size().

◆ getCurrentlyBuiltMetricMap()

const CMultiMetricMap * CMetricMapBuilderICP::getCurrentlyBuiltMetricMap ( ) const
virtual

Returns the map built so far.

NOTE that for efficiency a pointer to the internal object is passed, DO NOT delete nor modify the object in any way, if desired, make a copy of ir with "duplicate()".

Implements mrpt::slam::CMetricMapBuilder.

Definition at line 526 of file CMetricMapBuilderICP.cpp.

References metricMap.

◆ getCurrentMapPoints()

void CMetricMapBuilderICP::getCurrentMapPoints ( std::vector< float > &  x,
std::vector< float > &  y 
)

◆ getCurrentPoseEstimation()

CPose3DPDFPtr CMetricMapBuilderICP::getCurrentPoseEstimation ( ) const
virtual

◆ initialize()

void CMetricMapBuilderICP::initialize ( const mrpt::maps::CSimpleMap initialMap = mrpt::maps::CSimpleMap(),
mrpt::poses::CPosePDF x0 = NULL 
)
virtual

◆ leaveCriticalSection()

void mrpt::slam::CMetricMapBuilder::leaveCriticalSection ( )
inlineprotectedinherited

Leave critical section for map updating.

Definition at line 42 of file CMetricMapBuilder.h.

References mrpt::synch::CCriticalSection::leave().

Referenced by getCurrentMapPoints(), and ~CMetricMapBuilderICP().

◆ loadCurrentMapFromFile()

void CMetricMapBuilder::loadCurrentMapFromFile ( const std::string fileName)
inherited

◆ processActionObservation()

void CMetricMapBuilderICP::processActionObservation ( mrpt::obs::CActionCollection action,
mrpt::obs::CSensoryFrame in_SF 
)
virtual

Appends a new action and observations to update this map: See the description of the class at the top of this page to see a more complete description.

Parameters
actionThe estimation of the incremental pose change in the robot pose.
in_SFThe set of observations that robot senses at the new pose. See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options
See also
processObservation

Implements mrpt::slam::CMetricMapBuilder.

Definition at line 381 of file CMetricMapBuilderICP.cpp.

References mrpt::obs::CSensoryFrame::begin(), mrpt::poses::CPose2D::composeFrom(), mrpt::obs::CObservationOdometry::Create(), mrpt::obs::CSensoryFrame::end(), mrpt::obs::CActionCollection::getBestMovementEstimation(), m_auxAccumOdometry, and processObservation().

◆ processObservation()

void CMetricMapBuilderICP::processObservation ( const mrpt::obs::CObservationPtr &  obs)

The main method of this class: Process one odometry or sensor observation.

The new entry point of the algorithm (the old one was processActionObservation, which now is a wrapper to this method). See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options

Definition at line 118 of file CMetricMapBuilderICP.cpp.

References accumulateRobotDisplacementCounters(), mrpt::slam::CMetricMapsAlignmentAlgorithm::Align(), mrpt::maps::CPointsMap::TInsertionOptions::also_interpolate, mrpt::slam::CMetricMapBuilder::TOptions::alwaysInsertByClass, mrpt::slam::CMetricMapBuilderICP::TDist::ang, ASSERT_, ASSERTMSG_, mrpt::poses::CPose2D::asString(), mrpt::utils::CListOfClasses::contains(), mrpt::poses::CPosePDFGaussian::copyFrom(), mrpt::poses::CPosePDFGaussian::cov, mrpt::obs::CSensoryFrame::Create(), mrpt::slam::CMetricMapBuilder::critZoneChangingMap, mrpt::maps::CMultiMetricMap::ProxyFilterContainerByClass< SELECTED_CLASS_PTR, CONTAINER >::empty(), empty(), mrpt::slam::CMetricMapBuilder::TOptions::enableMapUpdating, mrpt::format(), mrpt::mrpt::format(), mrpt::system::formatTimeInterval(), mrpt::poses::CRobot2DPoseEstimator::getCurrentEstimate(), mrpt::poses::CRobot2DPoseEstimator::getLatestRobotPose(), mrpt::utils::CProbabilityDensityFunction< TDATA, STATE_LEN >::getMeanVal(), mrpt::slam::CICP::TReturnInfo::goodness, ICP_options, ICP_params, mrpt::maps::CSimpleMap::insert(), mrpt::slam::CMetricMapBuilderICP::TConfigParams::insertionAngDistance, mrpt::slam::CMetricMapBuilderICP::TConfigParams::insertionLinDistance, mrpt::maps::CPointsMap::insertionOptions, mrpt::maps::CMetricMap::insertObservationPtr(), INVALID_TIMESTAMP, IS_CLASS, IS_DERIVED, mrpt::maps::CMetricMap::isEmpty(), mrpt::slam::CMetricMapBuilderICP::TDist::lin, mrpt::slam::CMetricMapBuilderICP::TConfigParams::localizationAngDistance, mrpt::slam::CMetricMapBuilderICP::TConfigParams::localizationLinDistance, m_distSinceLastICP, m_distSinceLastInsertion, m_estRobotPath, mrpt::maps::CMultiMetricMap::m_gridMaps, m_lastPoseEst, m_lastPoseEst_cov, mrpt::maps::CMultiMetricMap::m_pointsMaps, m_there_has_been_an_odometry, mrpt::slam::CMetricMapBuilderICP::TConfigParams::matchAgainstTheGrid, mrpt::poses::CPosePDFGaussian::mean, metricMap, min, mrpt::maps::CPointsMap::TInsertionOptions::minDistBetweenLaserPoints, mrpt::slam::CMetricMapBuilderICP::TConfigParams::minICPgoodnessToAccept, MRPT_END, MRPT_LOG_DEBUG, MRPT_LOG_DEBUG_STREAM, MRPT_LOG_INFO, MRPT_LOG_INFO_STREAM, MRPT_LOG_WARN, MRPT_LOG_WARN_STREAM, MRPT_START, mrpt::slam::CICP::TReturnInfo::nIterations, mrpt::slam::CICP::options, mrpt::slam::CMetricMapBuilder::options, mrpt::poses::CRobot2DPoseEstimator::processUpdateNewOdometry(), mrpt::poses::CRobot2DPoseEstimator::processUpdateNewPoseLocalization(), resetRobotDisplacementCounters(), SF_Poses_seq, mrpt::maps::CMultiMetricMap::ProxyFilterContainerByClass< SELECTED_CLASS_PTR, CONTAINER >::size(), mrpt::utils::CTicTac::Tac(), mrpt::utils::CTicTac::Tic(), and mrpt::slam::CMetricMapBuilderICP::TDist::updatePose().

Referenced by processActionObservation().

◆ resetRobotDisplacementCounters()

void CMetricMapBuilderICP::resetRobotDisplacementCounters ( const mrpt::poses::CPose2D new_pose)
private

◆ saveCurrentEstimationToImage()

void CMetricMapBuilderICP::saveCurrentEstimationToImage ( const std::string file,
bool  formatEMF_BMP = true 
)
virtual

A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.

Parameters
fileThe output file name
formatEMF_BMPOutput format = true:EMF, false:BMP

Implements mrpt::slam::CMetricMapBuilder.

Definition at line 543 of file CMetricMapBuilderICP.cpp.

References ASSERT_, mrpt::utils::CEnhancedMetaFile::drawImage(), mrpt::utils::CEnhancedMetaFile::line(), m_estRobotPath, mrpt::maps::CMultiMetricMap::m_gridMaps, metricMap, MRPT_END, MRPT_START, mrpt::maps::CMultiMetricMap::ProxyFilterContainerByClass< SELECTED_CLASS_PTR, CONTAINER >::size(), and THROW_EXCEPTION.

◆ saveCurrentMapToFile()

void CMetricMapBuilder::saveCurrentMapToFile ( const std::string fileName,
bool  compressGZ = true 
) const
inherited

Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file.

Definition at line 82 of file CMetricMapBuilder.cpp.

References CFileGZOutputStream, mrpt::slam::CMetricMapBuilder::getCurrentlyBuiltMap(), and MRPT_LOG_INFO_STREAM.

Referenced by setCurrentMapFile().

◆ setCurrentMapFile()

void CMetricMapBuilderICP::setCurrentMapFile ( const char *  mapFile)

Sets the "current map file", thus that map will be loaded if it exists or a new one will be created if it does not, and the updated map will be save to that file when destroying the object.

Definition at line 406 of file CMetricMapBuilderICP.cpp.

References currentMapFile, mrpt::slam::CMetricMapBuilder::loadCurrentMapFromFile(), and mrpt::slam::CMetricMapBuilder::saveCurrentMapToFile().

Referenced by ~CMetricMapBuilderICP().

Member Data Documentation

◆ critZoneChangingMap

mrpt::synch::CCriticalSection mrpt::slam::CMetricMapBuilder::critZoneChangingMap
protectedinherited

◆ currentMapFile

std::string mrpt::slam::CMetricMapBuilderICP::currentMapFile
private

Current map file.

Definition at line 129 of file CMetricMapBuilderICP.h.

Referenced by setCurrentMapFile().

◆ ICP_options

TConfigParams mrpt::slam::CMetricMapBuilderICP::ICP_options

Options for the ICP-SLAM application.

See also
ICP_params

Definition at line 68 of file CMetricMapBuilderICP.h.

Referenced by initialize(), and processObservation().

◆ ICP_params

CICP::TConfigParams mrpt::slam::CMetricMapBuilderICP::ICP_params

Options for the ICP algorithm itself.

See also
ICP_options

Definition at line 69 of file CMetricMapBuilderICP.h.

Referenced by processObservation().

◆ m_auxAccumOdometry

mrpt::poses::CPose2D mrpt::slam::CMetricMapBuilderICP::m_auxAccumOdometry
private

Definition at line 138 of file CMetricMapBuilderICP.h.

Referenced by initialize(), and processActionObservation().

◆ m_distSinceLastICP

TDist mrpt::slam::CMetricMapBuilderICP::m_distSinceLastICP
private

◆ m_distSinceLastInsertion

mrpt::aligned_containers<std::string,TDist>::map_t mrpt::slam::CMetricMapBuilderICP::m_distSinceLastInsertion
private

◆ m_estRobotPath

std::deque<mrpt::math::TPose2D> mrpt::slam::CMetricMapBuilderICP::m_estRobotPath
private

The estimated robot path:

Definition at line 137 of file CMetricMapBuilderICP.h.

Referenced by initialize(), processObservation(), and saveCurrentEstimationToImage().

◆ m_lastPoseEst

mrpt::poses::CRobot2DPoseEstimator mrpt::slam::CMetricMapBuilderICP::m_lastPoseEst
private

The pose estimation by the alignment algorithm (ICP).

Last pose estimation (Mean)

Definition at line 132 of file CMetricMapBuilderICP.h.

Referenced by getCurrentPoseEstimation(), initialize(), and processObservation().

◆ m_lastPoseEst_cov

mrpt::math::CMatrixDouble33 mrpt::slam::CMetricMapBuilderICP::m_lastPoseEst_cov
private

Last pose estimation (covariance)

Definition at line 133 of file CMetricMapBuilderICP.h.

Referenced by getCurrentPoseEstimation(), and processObservation().

◆ m_there_has_been_an_odometry

bool mrpt::slam::CMetricMapBuilderICP::m_there_has_been_an_odometry
private

Definition at line 153 of file CMetricMapBuilderICP.h.

Referenced by initialize(), and processObservation().

◆ metricMap

mrpt::maps::CMultiMetricMap mrpt::slam::CMetricMapBuilderICP::metricMap
private

The metric map representation as a points map:

Definition at line 126 of file CMetricMapBuilderICP.h.

Referenced by getCurrentlyBuiltMetricMap(), getCurrentMapPoints(), initialize(), processObservation(), and saveCurrentEstimationToImage().

◆ options

TOptions mrpt::slam::CMetricMapBuilder::options
inherited

◆ SF_Poses_seq

mrpt::maps::CSimpleMap mrpt::slam::CMetricMapBuilderICP::SF_Poses_seq
private

The set of observations that leads to current map:

Definition at line 123 of file CMetricMapBuilderICP.h.

Referenced by getCurrentlyBuiltMap(), getCurrentlyBuiltMapSize(), initialize(), and processObservation().




Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019