MRPT
2.0.1
|
MonteCarlo (Particle filter) localization wrapper class for CLI or custom applications: virtual base class, which can be used as base for custom user robotic applications.
Definition at line 27 of file MonteCarloLocalization_App.h.
#include <mrpt/apps/MonteCarloLocalization_App.h>
Public Member Functions | |
MonteCarloLocalization_Base () | |
Main API | |
void | initialize (int argc, const char **argv) |
Initializes the application from CLI parameters. More... | |
void | initialize (int argc, char **argv) |
void | run () |
Runs with the current parameter set. More... | |
Static Public Member Functions | |
static std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > & | logging_levels_to_colors () |
Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor. More... | |
static std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > & | logging_levels_to_names () |
Map from VerbosityLevels to their corresponding names. More... | |
Public Attributes | |
Parameters and options. See: initialize() | |
mrpt::config::CConfigFileMemory | params |
Populated in initialize(). More... | |
bool | allow_quit_on_esc_key = true |
If true, will watch the keyboard and quit when ESC is pushed. More... | |
bool | fill_out_estimated_path = false |
Whether to populate out_estimated_path. More... | |
Outputs and result variables | |
mrpt::poses::CPose3DInterpolator | out_estimated_path |
Controlled by flag fill_out_estimated_path More... | |
Static Public Attributes | |
static constexpr auto | sect = "LocalizationExperiment" |
Default name of the main configuration section in INI files for this app. More... | |
Protected Member Functions | |
template<class MONTECARLO_TYPE > | |
void | do_pf_localization () |
void | getGroundTruth (mrpt::poses::CPose2D &expectedPose, size_t rawlogEntry, const mrpt::math::CMatrixDouble >, const Clock::time_point &cur_time) |
void | prepareGT (const mrpt::math::CMatrixDouble >) |
virtual void | impl_initialize (int argc, const char **argv)=0 |
virtual std::string | impl_get_usage () const =0 |
virtual bool | impl_get_next_observations (mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &observations, mrpt::obs::CObservation::Ptr &observation)=0 |
Get next sensory data. More... | |
Protected Attributes | |
mrpt::poses::CPose2DInterpolator | GT_path |
VerbosityLevel | m_min_verbosity_level {LVL_INFO} |
Provided messages with VerbosityLevel smaller than this value shall be ignored. More... | |
Logging methods | |
bool | logging_enable_console_output {true} |
[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically. More... | |
bool | logging_enable_keep_record {false} |
[Default=false] Enables storing all messages into an internal list. More... | |
void | logStr (const VerbosityLevel level, std::string_view msg_str) const |
Main method to add the specified message string to the logger. More... | |
void | logFmt (const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3 |
Alternative logging method, which mimics the printf behavior. More... | |
void void | logCond (const VerbosityLevel level, bool cond, const std::string &msg_str) const |
Log the given message only if the condition is satisfied. More... | |
void | setLoggerName (const std::string &name) |
Set the name of the COutputLogger instance. More... | |
std::string | getLoggerName () const |
Return the name of the COutputLogger instance. More... | |
void | setMinLoggingLevel (const VerbosityLevel level) |
Set the minimum logging level for which the incoming logs are going to be taken into account. More... | |
void | setVerbosityLevel (const VerbosityLevel level) |
alias of setMinLoggingLevel() More... | |
VerbosityLevel | getMinLoggingLevel () const |
bool | isLoggingLevelVisible (VerbosityLevel level) const |
void | getLogAsString (std::string &log_contents) const |
Fill the provided string with the contents of the logger's history in std::string representation. More... | |
std::string | getLogAsString () const |
Get the history of COutputLogger instance in a string representation. More... | |
void | writeLogToFile (const std::string *fname_in=nullptr) const |
Write the contents of the COutputLogger instance to an external file. More... | |
void | dumpLogToConsole () const |
Dump the current contents of the COutputLogger instance in the terminal window. More... | |
std::string | getLoggerLastMsg () const |
Return the last Tmsg instance registered in the logger history. More... | |
void | getLoggerLastMsg (std::string &msg_str) const |
Fill inputtted string with the contents of the last message in history. More... | |
void | loggerReset () |
Reset the contents of the logger instance. More... | |
void | logRegisterCallback (output_logger_callback_t userFunc) |
bool | logDeregisterCallback (output_logger_callback_t userFunc) |
MonteCarloLocalization_Base::MonteCarloLocalization_Base | ( | ) |
Definition at line 57 of file MonteCarloLocalization_App.cpp.
References mrpt::system::COutputLogger::setLoggerName().
|
protected |
Definition at line 165 of file MonteCarloLocalization_App.cpp.
References mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::TOptions_6DOFModel::additional_std_XYZ, mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams::aperture, mrpt::serialization::archiveFrom(), mrpt::math::CVectorDynamic< T >::asEigen(), mrpt::math::CMatrixDynamic< T >::asEigen(), ASSERT_, ASSERT_DIRECTORY_EXISTS_, mrpt::maps::CMetricMap::clear(), mrpt::obs::CActionRobotMovement3D::computeFromOdometry(), mrpt::obs::CActionRobotMovement2D::computeFromOdometry(), mrpt::math::confidenceIntervals(), mrpt::math::cov(), mrpt::opengl::CGridPlaneXY::Create(), mrpt::system::createDirectory(), mrpt::system::dateTimeLocalToString(), mrpt::DEG2RAD(), mrpt::system::deleteFilesInDirectory(), mrpt::math::MatrixBase< Scalar, Derived >::det(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::distanceTo(), mrpt::maps::TSetOfMetricMapInitializers::dumpToTextStream(), mrpt::math::CVectorDynamic< T >::empty(), mrpt::opengl::COpenGLScene::enableFollowCamera(), mrpt::containers::end(), mrpt::bayes::CParticleFilter::TParticleFilterStats::ESS_beforeResample, mrpt::obs::CObservation2DRangeScanWithUncertainty::evaluateScanLikelihood(), mrpt::bayes::CParticleFilter::executeOn(), mrpt::system::extractFileExtension(), mrpt::system::fileExists(), mrpt::format(), mrpt::ptr_cast< CAST_TO >::from(), mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::gaussianModel, mrpt::maps::CMultiMetricMap::getAs3DObject(), mrpt::maps::CMultiMetricMap::getAsSimplePointsMap(), mrpt::opengl::COpenGLScene::getByName(), mrpt::system::os::getch(), mrpt::random::getRandomGenerator(), mrpt::obs::CObservation2DRangeScan::getScanRange(), mrpt::opengl::COpenGLScene::getViewport(), mrpt::hypot_fast(), mrpt::opengl::COpenGLScene::insert(), INVALID_TIMESTAMP, IS_CLASS, mrpt::system::os::kbhit(), mrpt::slam::TMonteCarloLocalizationParams::KLD_params, mrpt::slam::TKLDParams::loadFromConfigFile(), mrpt::bayes::CParticleFilter::TParticleFilterOptions::loadFromConfigFile(), mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile(), mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(), mrpt::math::MatrixVectorBase< Scalar, Derived >::loadFromTextFile(), mrpt::system::lowerCase(), mrpt::bayes::CParticleFilter::m_options, mrpt::maps::CMultiMetricMap::mapByClass(), mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams::maxRange, mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams::method, mrpt::slam::TMonteCarloLocalizationParams::metricMap, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdPHI, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdXY, mrpt::obs::CActionRobotMovement3D::TMotionModelOptions::mm6DOFModel, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::modelSelection, MRPT_LOAD_CONFIG_VAR, MRPT_LOG_INFO, MRPT_LOG_INFO_FMT, MRPT_LOG_INFO_STREAM, mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams::nRays, mrpt::io::CFileOutputStream::open(), params, mrpt::system::pause(), mrpt::poses::CPose2D::phi(), mrpt::io::CStream::printf(), mrpt::math::CVectorDynamic< T >::push_back(), mrpt::random::CRandomGenerator::randomize(), mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams::rangeNoiseStd, mrpt::obs::CObservation2DRangeScanWithUncertainty::rangeScan, mrpt::obs::CObservation2DRangeScanWithUncertainty::rangesCovar, mrpt::obs::CObservation2DRangeScanWithUncertainty::rangesMean, mrpt::opengl::COpenGLScene::removeObject(), mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams::rightToLeft, mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams::robotPose, mrpt::math::CMatrixDynamic< T >::rows(), mrpt::maps::CMultiMetricMap::saveMetricMapRepresentationToFile(), mrpt::math::MatrixVectorBase< Scalar, Derived >::saveToTextFile(), mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyResult::scanWithUncert, sect, mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams::sensorPose, mrpt::opengl::CCamera::setAzimuthDegrees(), mrpt::opengl::CCamera::setElevationDegrees(), mrpt::maps::CMultiMetricMap::setListOfMaps(), mrpt::opengl::CCamera::setOrthogonal(), mrpt::opengl::CCamera::setPointingAt(), mrpt::opengl::CCamera::setZoomDistance(), mrpt::maps::CSimpleMap::size(), mrpt::system::CTicTac::Tac(), THROW_EXCEPTION_FMT, mrpt::system::CTicTac::Tic(), mrpt::bayes::CParticleFilter::TParticleFilterStats::weightsVariance_beforeResample, win, mrpt::math::TPoint3D_data< T >::x, mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), mrpt::math::TPoint3D_data< T >::y, and mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y().
|
inherited |
Dump the current contents of the COutputLogger instance in the terminal window.
Definition at line 190 of file COutputLogger.cpp.
|
protected |
Definition at line 1151 of file MonteCarloLocalization_App.cpp.
References mrpt::math::CMatrixDynamic< T >::cols(), mrpt::poses::CPose2D::phi(), mrpt::math::CMatrixDynamic< T >::rows(), THROW_EXCEPTION, mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y().
|
inherited |
Fill the provided string with the contents of the logger's history in std::string representation.
Definition at line 154 of file COutputLogger.cpp.
|
inherited |
Get the history of COutputLogger instance in a string representation.
Definition at line 159 of file COutputLogger.cpp.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().
|
inherited |
Return the last Tmsg instance registered in the logger history.
Definition at line 195 of file COutputLogger.cpp.
References mrpt::system::COutputLogger::TMsg::getAsString().
|
inherited |
Fill inputtted string with the contents of the last message in history.
Definition at line 201 of file COutputLogger.cpp.
|
inherited |
Return the name of the COutputLogger instance.
Definition at line 143 of file COutputLogger.cpp.
|
inlineinherited |
Definition at line 201 of file system/COutputLogger.h.
References mrpt::system::COutputLogger::m_min_verbosity_level.
Referenced by mrpt::apps::RawlogGrabberApp::dump_verbose_info(), mrpt::maps::CRandomFieldGridMap2D::isEnabledVerbose(), mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), mrpt::apps::CGridMapAlignerApp::run(), mrpt::apps::RBPF_SLAM_App_Base::run(), and mrpt::apps::ICP_SLAM_App_Base::run().
|
protectedpure virtualinherited |
Get next sensory data.
Return false on any error, true if success.
Implemented in mrpt::apps::ICP_SLAM_App_Live, and mrpt::apps::DataSourceRawlog.
Referenced by mrpt::apps::ICP_SLAM_App_Base::run(), and mrpt::apps::RBPF_SLAM_App_Base::run().
|
protectedpure virtualinherited |
Implemented in mrpt::apps::MonteCarloLocalization_Rawlog, mrpt::apps::ICP_SLAM_App_Live, mrpt::apps::RBPF_SLAM_App_Rawlog, and mrpt::apps::ICP_SLAM_App_Rawlog.
Referenced by mrpt::apps::ICP_SLAM_App_Live::impl_initialize(), mrpt::apps::RBPF_SLAM_App_Base::initialize(), initialize(), and mrpt::apps::ICP_SLAM_App_Base::initialize().
|
protectedpure virtualinherited |
Implemented in mrpt::apps::MonteCarloLocalization_Rawlog, mrpt::apps::ICP_SLAM_App_Live, mrpt::apps::RBPF_SLAM_App_Rawlog, and mrpt::apps::ICP_SLAM_App_Rawlog.
Referenced by mrpt::apps::ICP_SLAM_App_Base::initialize(), mrpt::apps::RBPF_SLAM_App_Base::initialize(), and initialize().
void MonteCarloLocalization_Base::initialize | ( | int | argc, |
const char ** | argv | ||
) |
Initializes the application from CLI parameters.
Refer to the manpage of pf-localization. Throws on errors.
Definition at line 63 of file MonteCarloLocalization_App.cpp.
References argc, argv, ASSERT_FILE_EXISTS_, mrpt::io::file_get_contents(), mrpt::apps::BaseAppInitializableCLI::impl_get_usage(), mrpt::apps::BaseAppInitializableCLI::impl_initialize(), MRPT_END, mrpt::system::MRPT_getCompilationDate(), mrpt::system::MRPT_getVersion(), MRPT_LOG_INFO_FMT, MRPT_START, params, mrpt::config::CConfigFileMemory::setContent(), and THROW_EXCEPTION_FMT.
Referenced by generic_pf_test(), and initialize().
|
inline |
Definition at line 46 of file MonteCarloLocalization_App.h.
References argc, argv, and initialize().
|
inlineinherited |
Definition at line 202 of file system/COutputLogger.h.
References mrpt::system::COutputLogger::m_min_verbosity_level.
Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().
|
inherited |
Log the given message only if the condition is satisfied.
Definition at line 131 of file COutputLogger.cpp.
|
inherited |
Definition at line 291 of file COutputLogger.cpp.
References getAddress(), and mrpt::system::COutputLogger::m_listCallbacks.
|
inherited |
Alternative logging method, which mimics the printf behavior.
Handy for not having to first use mrpt::format to pass a std::string message to logStr
Definition at line 91 of file COutputLogger.cpp.
Referenced by mrpt::hmtslam::CHMTSLAM::areaAbstraction(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD(), mrpt::hmtslam::CTopLCDetector_GridMatching::computeTopologicalObservationModel(), CGraphSlamHandler< GRAPH_T >::execute(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), CGraphSlamHandler< GRAPH_T >::initOutputDir(), CGraphSlamHandler< GRAPH_T >::initVisualization(), mrpt::nav::CNavigatorManualSequence::navigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), CGraphSlamHandler< GRAPH_T >::readConfigFname(), CGraphSlamHandler< GRAPH_T >::saveResults(), CGraphSlamHandler< GRAPH_T >::setResultsDirName(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), mrpt::hmtslam::CHMTSLAM::thread_3D_viewer(), mrpt::hmtslam::CHMTSLAM::thread_LSLAM(), mrpt::hmtslam::CHMTSLAM::thread_TBI(), and CGraphSlamHandler< GRAPH_T >::~CGraphSlamHandler().
|
inherited |
Reset the contents of the logger instance.
Called upon construction.
Definition at line 206 of file COutputLogger.cpp.
References mrpt::system::LVL_INFO.
|
staticinherited |
Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor.
Handy for coloring the input based on the verbosity of the message
Definition at line 47 of file COutputLogger.cpp.
References logging_levels_to_colors.
Referenced by mrpt::system::COutputLogger::TMsg::dumpToConsole().
|
staticinherited |
Map from VerbosityLevels to their corresponding names.
Handy for printing the current message VerbosityLevel along with the actual content
Definition at line 60 of file COutputLogger.cpp.
References logging_levels_to_names.
Referenced by mrpt::system::COutputLogger::TMsg::getAsString().
|
inherited |
Definition at line 278 of file COutputLogger.cpp.
References mrpt::system::COutputLogger::m_listCallbacks.
|
inherited |
Main method to add the specified message string to the logger.
Definition at line 72 of file COutputLogger.cpp.
References mrpt::system::COutputLogger::TMsg::body, mrpt::system::COutputLogger::TMsg::dumpToConsole(), mrpt::system::COutputLogger::TMsg::level, mrpt::system::COutputLogger::TMsg::name, and mrpt::system::COutputLogger::TMsg::timestamp.
Referenced by mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper(), and mrpt::system::CTimeLoggerSaveAtDtor::~CTimeLoggerSaveAtDtor().
|
protected |
Definition at line 1222 of file MonteCarloLocalization_App.cpp.
References mrpt::math::CMatrixDynamic< T >::cols(), mrpt::Clock::fromDouble(), and mrpt::math::CMatrixDynamic< T >::rows().
void MonteCarloLocalization_Base::run | ( | ) |
Runs with the current parameter set.
Throws on errors.
Definition at line 1257 of file MonteCarloLocalization_App.cpp.
References MRPT_END, MRPT_LOG_INFO, MRPT_START, params, and sect.
Referenced by generic_pf_test().
|
inherited |
Set the name of the COutputLogger instance.
Definition at line 138 of file COutputLogger.cpp.
Referenced by mrpt::slam::CMetricMapBuilderICP::CMetricMapBuilderICP(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::slam::CMonteCarloLocalization2D::CMonteCarloLocalization2D(), mrpt::slam::CMonteCarloLocalization3D::CMonteCarloLocalization3D(), mrpt::apps::ICP_SLAM_App_Base::ICP_SLAM_App_Base(), mrpt::apps::ICP_SLAM_App_Live::ICP_SLAM_App_Live(), mrpt::apps::ICP_SLAM_App_Rawlog::ICP_SLAM_App_Rawlog(), mrpt::graphslam::CWindowManager::initCWindowManager(), MonteCarloLocalization_Base(), mrpt::apps::RBPF_SLAM_App_Base::RBPF_SLAM_App_Base(), and mrpt::apps::RBPF_SLAM_App_Rawlog::RBPF_SLAM_App_Rawlog().
|
inherited |
Set the minimum logging level for which the incoming logs are going to be taken into account.
String messages with specified VerbosityLevel smaller than the min, will not be outputted to the screen and neither will a record of them be stored in by the COutputLogger instance
Definition at line 144 of file COutputLogger.cpp.
Referenced by mrpt::maps::CRandomFieldGridMap2D::enableVerbose(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), generic_kf_slam_test(), generic_pf_test(), generic_rbpf_slam_test(), mrpt::apps::RawlogGrabberApp::initialize(), mrpt::hwdrivers::CHokuyoURG::initialize(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::loadParams(), mrpt::apps::CGridMapAlignerApp::run(), mrpt::apps::RBPF_SLAM_App_Base::run(), and mrpt::apps::ICP_SLAM_App_Base::run().
|
inherited |
alias of setMinLoggingLevel()
Definition at line 149 of file COutputLogger.cpp.
Referenced by mrpt::nav::CAbstractNavigator::CAbstractNavigator(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::comms::CServerTCPSocket::CServerTCPSocket(), mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), mrpt::math::ransac_detect_2D_lines(), and mrpt::apps::ICP_SLAM_App_Base::run().
|
inherited |
Write the contents of the COutputLogger instance to an external file.
Upon call to this method, COutputLogger dumps the contents of all the logged commands so far to the specified external file. By default the filename is set to ${LOGGERNAME}.log except if the fname parameter is provided
Definition at line 165 of file COutputLogger.cpp.
References ASSERTMSG_, and mrpt::format().
bool mrpt::apps::MonteCarloLocalization_Base::allow_quit_on_esc_key = true |
If true, will watch the keyboard and quit when ESC is pushed.
Definition at line 65 of file MonteCarloLocalization_App.h.
Referenced by generic_pf_test().
bool mrpt::apps::MonteCarloLocalization_Base::fill_out_estimated_path = false |
Whether to populate out_estimated_path.
Definition at line 68 of file MonteCarloLocalization_App.h.
Referenced by generic_pf_test().
|
protected |
Definition at line 89 of file MonteCarloLocalization_App.h.
|
inherited |
[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically.
Definition at line 240 of file system/COutputLogger.h.
|
inherited |
[Default=false] Enables storing all messages into an internal list.
Definition at line 243 of file system/COutputLogger.h.
|
protectedinherited |
Provided messages with VerbosityLevel smaller than this value shall be ignored.
Definition at line 253 of file system/COutputLogger.h.
Referenced by mrpt::system::COutputLogger::getMinLoggingLevel(), and mrpt::system::COutputLogger::isLoggingLevelVisible().
mrpt::poses::CPose3DInterpolator mrpt::apps::MonteCarloLocalization_Base::out_estimated_path |
Controlled by flag fill_out_estimated_path
Definition at line 76 of file MonteCarloLocalization_App.h.
mrpt::config::CConfigFileMemory mrpt::apps::MonteCarloLocalization_Base::params |
Populated in initialize().
Can be replaced or manipulated by the user after that and before run() to change the parameters loaded from INI file.
Definition at line 62 of file MonteCarloLocalization_App.h.
Referenced by generic_pf_test(), and initialize().
|
static |
Default name of the main configuration section in INI files for this app.
Definition at line 36 of file MonteCarloLocalization_App.h.
Referenced by generic_pf_test(), and TEST().
Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020 |