MRPT
1.9.9
|
ICP-based Fixed Intervals Node Registration.
Current Decider is meant for adding nodes in 2D datasets recorded using a laser range finder or RGB-D camera (e.g. Kinect). No odometry data from encoders is needed. Using ICP to match consecutive RangeScan measurements, the decider keeps track of the pose transformation since the last registered node. If the norm or the angle of the latter surpasses certain thresholds (which are read from an external .ini file) then a new node is added to the graph)
Decider does not guarantee thread safety when accessing the GRAPH_T resource. This is handled by the CGraphSlamEngine class.
Definition at line 85 of file CICPCriteriaNRD.h.
#include <mrpt/graphslam/NRD/CICPCriteriaNRD.h>
Classes | |
struct | TParams |
Public Types | |
using | constraint_t = typename GRAPH_T::constraint_t |
Handy typedefs. More... | |
using | pose_t = typename GRAPH_T::constraint_t::type_value |
type of underlying poses (2D/3D). More... | |
using | global_pose_t = typename GRAPH_T::global_pose_t |
using | InfMat = mrpt::math::CMatrixFixedNumeric< double, constraint_t::state_length, constraint_t::state_length > |
using | range_ops_t = mrpt::graphslam::deciders::CRangeScanOps< GRAPH_T > |
Typedef for accessing methods of the RangeScanRegistrationDecider parent class. More... | |
using | decider_t = CICPCriteriaNRD< GRAPH_T > |
self type More... | |
using | parent_t = mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > |
Node Registration Decider. More... | |
using | inf_mat_t = mrpt::math::CMatrixFixedNumeric< double, constraint_t::state_length, constraint_t::state_length > |
Public Member Functions | |
CICPCriteriaNRD () | |
Class constructor. More... | |
~CICPCriteriaNRD () | |
Class destructor. More... | |
void | loadParams (const std::string &source_fname) |
Load the necessary for the decider/optimizer configuration parameters. More... | |
void | printParams () const |
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact way. More... | |
void | getDescriptiveReport (std::string *report_str) const |
Fill the provided string with a detailed report of the decider/optimizer state. More... | |
bool | updateState (mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) |
Update the decider state using the latest dataset measurements. More... | |
bool | updateState2D (mrpt::obs::CObservation2DRangeScan::Ptr observation) |
Specialized updateState method used solely when dealing with 2DRangeScan information. More... | |
bool | updateState3D (mrpt::obs::CObservation3DRangeScan::Ptr observation) |
Specialized updateState method used solely when dealing with 3DRangeScan information. More... | |
virtual global_pose_t | getCurrentRobotPosEstimation () const |
Getter method for fetching the currently estimated robot position. More... | |
virtual void | setWindowManagerPtr (mrpt::graphslam::CWindowManager *win_manager) |
Fetch a CWindowManager pointer. More... | |
virtual void | setCriticalSectionPtr (std::mutex *graph_section) |
Fetch a std::mutex for locking the GRAPH_T resource. More... | |
virtual void | initializeVisuals () |
Initialize visual objects in CDisplayWindow (e.g. More... | |
virtual void | updateVisuals () |
Update the relevant visual features in CDisplayWindow. More... | |
virtual void | notifyOfWindowEvents (const std::map< std::string, bool > &events_occurred) |
Get a list of the window events that happened since the last call. More... | |
virtual void | setGraphPtr (GRAPH_T *graph) |
Fetch the graph on which the decider/optimizer will work on. More... | |
virtual void | initializeLoggers (const std::string &name) |
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand. More... | |
virtual void | setClassName (const std::string &name) |
bool | isMultiRobotSlamClass () |
std::string | getClassName () const |
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 | |
TParams | params |
Protected Member Functions | |
bool | checkRegistrationCondition () |
Check whether a new node should be registered in the graph. More... | |
bool | checkRegistrationCondition2D () |
Specialized checkRegistrationCondtion method used solely when dealing with 2DRangeScan information. More... | |
bool | checkRegistrationCondition3D () |
Specialized checkRegistrationCondition method used solely when dealing with 3DRangeScan information. More... | |
void | resetPDF (constraint_t *c) |
Reset the given PDF method and assign a fixed high-certainty Covariance/Information matrix. More... | |
virtual void | assertVisualsVars () |
Handy function for making all the visuals assertions in a compact manner. More... | |
void | getICPEdge (const mrpt::obs::CObservation2DRangeScan &from, const mrpt::obs::CObservation2DRangeScan &to, constraint_t *rel_edge, const mrpt::poses::CPose2D *initial_pose=nullptr, mrpt::slam::CICP::TReturnInfo *icp_info=nullptr) |
Align the 2D range scans provided and fill the potential edge that can transform the one into the other. More... | |
void | getICPEdge (const mrpt::obs::CObservation3DRangeScan &from, const mrpt::obs::CObservation3DRangeScan &to, constraint_t *rel_edge, const mrpt::poses::CPose2D *initial_pose=nullptr, mrpt::slam::CICP::TReturnInfo *icp_info=nullptr) |
Align the 3D range scans provided and find the potential edge that can transform the one into the other. More... | |
void | decimatePointsMap (mrpt::maps::CPointsMap *m, size_t keep_point_every=4, size_t low_lim=0) |
Reduce the size of the given CPointsMap by keeping one out of "keep_point_every" points. More... | |
bool | convert3DTo2DRangeScan (mrpt::obs::CObservation3DRangeScan::Ptr &scan3D_in, mrpt::obs::CObservation2DRangeScan::Ptr *scan2D_out=nullptr) |
Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method. More... | |
bool | registerNewNodeAtEnd (const typename GRAPH_T::constraint_t &constraint) |
Utility methods for adding new poses to the graph. More... | |
bool | registerNewNodeAtEnd () |
Same goal as the previous method - uses the m_since_prev_node_PDF as the constraint at the end. More... | |
virtual void | addNodeAnnotsToPose (global_pose_t *pose) const |
Get a global_pose_t and fill the NODE_ANNOTATIONS-related fields. More... | |
Protected Attributes | |
bool | m_is_using_3DScan |
constraint_t | m_latest_odometry_PDF |
Odometry rigid-body transformation since the last accepted LaserScan. More... | |
pose_t | m_curr_odometry_only_pose |
pose_t estimation using only odometry information. More... | |
pose_t | m_last_odometry_only_pose |
pose_t estimation using only odometry information. More... | |
TSlidingWindow | m_mahal_distance_ICP_odom |
Keeps track of the last N measurements between the ICP edge and the corresponding odometry measurements. More... | |
bool | m_use_angle_difference_node_reg |
bool | m_use_distance_node_reg |
int | m_times_used_ICP |
How many times we used the ICP Edge instead of Odometry edge. More... | |
int | m_times_used_odom |
How many times we used the Odometry Edge instead of the ICP edge. More... | |
mrpt::graphs::TNodeID | m_prev_registered_nodeID |
Store the last registered NodeID. More... | |
constraint_t | m_since_prev_node_PDF |
Tracking the PDF of the current position of the robot with regards to the <b previous registered node. More... | |
inf_mat_t | m_init_inf_mat |
Initial information matrix for paths. More... | |
GRAPH_T * | m_graph |
Pointer to the graph that is under construction. More... | |
std::mutex * | m_graph_section |
mrpt::system::CTimeLogger | m_time_logger |
Time logger instance. More... | |
std::string | m_class_name |
Name of the class instance. More... | |
bool | is_mr_slam_class |
Boolean indicating if the current class can be used in multi-robot SLAM operations. More... | |
VerbosityLevel | m_min_verbosity_level |
Provided messages with VerbosityLevel smaller than this value shall be ignored. More... | |
mrpt::obs::CObservation2DRangeScan::Ptr | m_last_laser_scan2D |
handy laser scans to use in the class methods More... | |
mrpt::obs::CObservation2DRangeScan::Ptr | m_curr_laser_scan2D |
Current LaserScan. More... | |
mrpt::obs::CObservation3DRangeScan::Ptr | m_last_laser_scan3D |
mrpt::obs::CObservation3DRangeScan::Ptr | m_curr_laser_scan3D |
Visuals-related variables methods | |
mrpt::graphslam::CWindowManager * | m_win_manager |
Pointer to the CWindowManager object used to store visuals-related instances. More... | |
mrpt::gui::CDisplayWindow3D * | m_win |
Window to use. More... | |
mrpt::graphslam::CWindowObserver * | m_win_observer |
CWindowObserver object for monitoring various visual-oriented events. More... | |
bool | m_initialized_visuals |
Static Protected Attributes | |
static const std::string | header_sep |
Separator string to be used in debugging messages. More... | |
static const std::string | report_sep |
Logging methods | |
bool | logging_enable_console_output |
[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 |
[Default=false] Enables storing all messages into an internal list. More... | |
void | logStr (const VerbosityLevel level, const std::string &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=NULL) 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) |
using mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::constraint_t = typename GRAPH_T::constraint_t |
using mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::decider_t = CICPCriteriaNRD<GRAPH_T> |
self type
Definition at line 107 of file CICPCriteriaNRD.h.
using mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::global_pose_t = typename GRAPH_T::global_pose_t |
Definition at line 99 of file CICPCriteriaNRD.h.
|
inherited |
Definition at line 47 of file CNodeRegistrationDecider.h.
using mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::InfMat = mrpt::math::CMatrixFixedNumeric< double, constraint_t::state_length, constraint_t::state_length> |
Definition at line 102 of file CICPCriteriaNRD.h.
using mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::parent_t = mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T> |
Node Registration Decider.
Definition at line 110 of file CICPCriteriaNRD.h.
using mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::pose_t = typename GRAPH_T::constraint_t::type_value |
type of underlying poses (2D/3D).
Definition at line 98 of file CICPCriteriaNRD.h.
using mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::range_ops_t = mrpt::graphslam::deciders::CRangeScanOps<GRAPH_T> |
Typedef for accessing methods of the RangeScanRegistrationDecider parent class.
Definition at line 106 of file CICPCriteriaNRD.h.
mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD | ( | ) |
Class constructor.
Definition at line 19 of file CICPCriteriaNRD_impl.h.
References mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::initializeLoggers(), mrpt::system::COutputLogger::logFmt(), mrpt::system::LVL_DEBUG, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_is_using_3DScan, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_latest_odometry_PDF, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_times_used_ICP, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_times_used_odom, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_use_angle_difference_node_reg, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_use_distance_node_reg, and mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T >::resetPDF().
mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::~CICPCriteriaNRD | ( | ) |
Class destructor.
Definition at line 41 of file CICPCriteriaNRD_impl.h.
|
protectedvirtualinherited |
Get a global_pose_t and fill the NODE_ANNOTATIONS-related fields.
Definition at line 145 of file CNodeRegistrationDecider_impl.h.
|
protectedvirtualinherited |
Handy function for making all the visuals assertions in a compact manner.
Definition at line 95 of file CRegistrationDeciderOrOptimizer_impl.h.
|
protectedvirtual |
Check whether a new node should be registered in the graph.
This should be the key-method in any implementation of this interface. Should call registerNewNodeAtEnd method if the registration condition is satisfied.
Reimplemented from mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T >.
Definition at line 245 of file CICPCriteriaNRD_impl.h.
References mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T >::m_since_prev_node_PDF, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_use_angle_difference_node_reg, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_use_distance_node_reg, MRPT_END, MRPT_LOG_DEBUG, MRPT_START, mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T >::registerNewNodeAtEnd(), and mrpt::math::wrapToPi().
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D().
|
protected |
Specialized checkRegistrationCondtion method used solely when dealing with 2DRangeScan information.
Definition at line 149 of file CICPCriteriaNRD_impl.h.
References mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition(), mrpt::graphslam::deciders::CRangeScanOps< GRAPH_T >::getICPEdge(), mrpt::slam::CICP::TReturnInfo::goodness, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_curr_laser_scan2D, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_curr_odometry_only_pose, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_last_laser_scan2D, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_last_odometry_only_pose, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_latest_odometry_PDF, mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T >::m_since_prev_node_PDF, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_times_used_ICP, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_times_used_odom, MRPT_END, MRPT_LOG_DEBUG, MRPT_LOG_DEBUG_FMT, MRPT_START, mrpt::slam::CICP::TReturnInfo::nIterations, and mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T >::resetPDF().
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState2D().
|
protected |
Specialized checkRegistrationCondition method used solely when dealing with 3DRangeScan information.
Definition at line 238 of file CICPCriteriaNRD_impl.h.
References THROW_EXCEPTION.
|
protectedinherited |
Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method.
Definition at line 143 of file CRangeScanOps_impl.h.
References MRPT_END, and MRPT_START.
|
protectedinherited |
Reduce the size of the given CPointsMap by keeping one out of "keep_point_every" points.
Definition at line 109 of file CRangeScanOps_impl.h.
References mrpt::maps::CPointsMap::applyDeletionMask(), min, MRPT_END, MRPT_START, and mrpt::maps::CPointsMap::size().
|
inherited |
Dump the current contents of the COutputLogger instance in the terminal window.
Definition at line 190 of file COutputLogger.cpp.
|
inlineinherited |
Definition at line 137 of file CRegistrationDeciderOrOptimizer.h.
|
virtualinherited |
Getter method for fetching the currently estimated robot position.
In single-robot situations this is most likely going to be the last registered node position + an position/uncertainty increment from that position
Reimplemented in mrpt::graphslam::deciders::CEmptyNRD< GRAPH_T >.
Definition at line 152 of file CNodeRegistrationDecider_impl.h.
|
virtual |
Fill the provided string with a detailed report of the decider/optimizer state.
Report should include (part of) the following:
Reimplemented from mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T >.
Definition at line 314 of file CICPCriteriaNRD_impl.h.
References mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T >::getDescriptiveReport(), mrpt::system::COutputLogger::getLogAsString(), mrpt::system::CTimeLogger::getStatsAsText(), mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::header_sep, mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::m_time_logger, MRPT_END, MRPT_START, and mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::report_sep.
|
protectedinherited |
Align the 2D range scans provided and fill the potential edge that can transform the one into the other.
User can optionally ask that additional information be returned in a TReturnInfo struct
Definition at line 16 of file CRangeScanOps_impl.h.
References mrpt::maps::CMetricMap::insertObservation(), MRPT_END, and MRPT_START.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D().
|
protectedinherited |
Align the 3D range scans provided and find the potential edge that can transform the one into the other.
Fills the 2D part (rel_edge) of the 3D constraint between the scans, since we are interested in computing the 2D alignment. User can optionally ask that additional information be returned in a TReturnInfo struct
Definition at line 51 of file CRangeScanOps_impl.h.
References ASSERTDEBMSG_, mrpt::format(), mrpt::obs::CObservation3DRangeScan::hasRangeImage, mrpt::maps::CMetricMap::insertObservation(), MRPT_END, and MRPT_START.
|
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 200 of file system/COutputLogger.h.
References mrpt::system::COutputLogger::m_min_verbosity_level.
Referenced by mrpt::maps::CRandomFieldGridMap2D::isEnabledVerbose(), and mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().
|
virtualinherited |
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand.
Definition at line 41 of file CRegistrationDeciderOrOptimizer_impl.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::CICPCriteriaERD(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::CLevMarqGSO().
|
virtualinherited |
Initialize visual objects in CDisplayWindow (e.g.
add an object to scene).
std::exception | If the method is called without having first provided a CDisplayWindow3D* to the class instance |
Reimplemented in mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >, and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >.
Definition at line 88 of file CRegistrationDeciderOrOptimizer_impl.h.
|
inlineinherited |
Definition at line 201 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 |
Definition at line 144 of file CRegistrationDeciderOrOptimizer_impl.h.
|
virtual |
Load the necessary for the decider/optimizer configuration parameters.
Reimplemented from mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.
Definition at line 281 of file CICPCriteriaNRD_impl.h.
References mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::loadParams(), MRPT_END, MRPT_LOG_DEBUG, MRPT_START, and mrpt::system::COutputLogger::setMinLoggingLevel().
|
inherited |
Log the given message only if the condition is satisfied.
Definition at line 131 of file COutputLogger.cpp.
|
inherited |
Definition at line 290 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::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(), 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 277 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(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().
|
virtualinherited |
Get a list of the window events that happened since the last call.
Method in derived classes is automatically called from the CGraphSlamEngine_t instance. After that, decider/optimizer should just fetch the parameters that it is interested in.
Reimplemented in mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >, and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >.
Definition at line 109 of file CRegistrationDeciderOrOptimizer_impl.h.
|
virtual |
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact way.
Reimplemented from mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.
Definition at line 302 of file CICPCriteriaNRD_impl.h.
References MRPT_END, MRPT_START, and mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::printParams().
|
protectedinherited |
Utility methods for adding new poses to the graph.
Add a new constraint at the end of the graph.
[in] | constraint | Constraint transformation from the latest registered to the new node. |
Definition at line 58 of file CNodeRegistrationDecider_impl.h.
|
protectedinherited |
Same goal as the previous method - uses the m_since_prev_node_PDF as the constraint at the end.
Definition at line 121 of file CNodeRegistrationDecider_impl.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition().
|
protectedinherited |
Reset the given PDF method and assign a fixed high-certainty Covariance/Information matrix.
Definition at line 132 of file CNodeRegistrationDecider_impl.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD().
|
virtualinherited |
Definition at line 56 of file CRegistrationDeciderOrOptimizer_impl.h.
|
virtualinherited |
Fetch a std::mutex for locking the GRAPH_T resource.
Handy for realising multithreading in the derived classes.
Definition at line 79 of file CRegistrationDeciderOrOptimizer_impl.h.
|
virtualinherited |
Fetch the graph on which the decider/optimizer will work on.
Definition at line 137 of file CRegistrationDeciderOrOptimizer_impl.h.
|
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(), and mrpt::graphslam::CWindowManager::initCWindowManager().
|
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(), mrpt::hwdrivers::CHokuyoURG::initialize(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::loadParams().
|
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(), and mrpt::math::ransac_detect_2D_lines().
|
virtualinherited |
Fetch a CWindowManager pointer.
CWindowManager instance should contain a CDisplayWindow3D* and, optionally, a CWindowObserver pointer so that interaction with the window is possible
Definition at line 65 of file CRegistrationDeciderOrOptimizer_impl.h.
|
virtual |
Update the decider state using the latest dataset measurements.
Order of calls: updateState (calls) ==> updateState2D/3D ==> checkRegistrationCondition2D/3D ==> CheckRegistrationCondition
Implements mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T >.
Definition at line 46 of file CICPCriteriaNRD_impl.h.
References mrpt::poses::CPosePDFGaussianInf::copyFrom(), mrpt::system::CTimeLogger::enter(), IS_CLASS, mrpt::system::CTimeLogger::leave(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_curr_odometry_only_pose, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_last_odometry_only_pose, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_latest_odometry_PDF, mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::m_time_logger, MRPT_END, MRPT_START, MRPT_UNUSED_PARAM, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState2D(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState3D().
bool mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState2D | ( | mrpt::obs::CObservation2DRangeScan::Ptr | observation | ) |
Specialized updateState method used solely when dealing with 2DRangeScan information.
Definition at line 126 of file CICPCriteriaNRD_impl.h.
References mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_curr_laser_scan2D, mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::m_last_laser_scan2D, MRPT_END, and MRPT_START.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState().
bool mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState3D | ( | mrpt::obs::CObservation3DRangeScan::Ptr | observation | ) |
Specialized updateState method used solely when dealing with 3DRangeScan information.
Definition at line 230 of file CICPCriteriaNRD_impl.h.
References THROW_EXCEPTION.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState().
|
virtualinherited |
Update the relevant visual features in CDisplayWindow.
std::exception | If the method is called without having first provided a CDisplayWindow3D* to the class instance |
Reimplemented in mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >, and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >.
Definition at line 103 of file CRegistrationDeciderOrOptimizer_impl.h.
|
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().
|
staticprotectedinherited |
Separator string to be used in debugging messages.
Definition at line 173 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().
|
protectedinherited |
Boolean indicating if the current class can be used in multi-robot SLAM operations.
Definition at line 169 of file CRegistrationDeciderOrOptimizer.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 239 of file system/COutputLogger.h.
|
inherited |
[Default=false] Enables storing all messages into an internal list.
Definition at line 242 of file system/COutputLogger.h.
|
protectedinherited |
Name of the class instance.
Definition at line 165 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_t >::getClassName().
|
protected |
Current LaserScan.
Set during the new measurements acquisition in updateState method
Definition at line 201 of file CICPCriteriaNRD.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState2D().
|
protected |
Definition at line 204 of file CICPCriteriaNRD.h.
|
protected |
pose_t estimation using only odometry information.
Definition at line 218 of file CICPCriteriaNRD.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState().
|
protectedinherited |
Pointer to the graph that is under construction.
Definition at line 144 of file CRegistrationDeciderOrOptimizer.h.
|
protectedinherited |
Definition at line 145 of file CRegistrationDeciderOrOptimizer.h.
|
protectedinherited |
Initial information matrix for paths.
Large values for this indicate that I am sure of the corresponding (initial) pose
Definition at line 131 of file CNodeRegistrationDecider.h.
|
protectedinherited |
Definition at line 159 of file CRegistrationDeciderOrOptimizer.h.
|
protected |
Definition at line 190 of file CICPCriteriaNRD.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD().
|
protected |
handy laser scans to use in the class methods
2D LaserScan corresponding to the latest registered node in the graph
Definition at line 197 of file CICPCriteriaNRD.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState2D().
|
protected |
Definition at line 203 of file CICPCriteriaNRD.h.
|
protected |
pose_t estimation using only odometry information.
Resets next time an ICP edge/Odometry measurement is utilized for updating the estimated robot position.
Definition at line 225 of file CICPCriteriaNRD.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState().
|
protected |
Odometry rigid-body transformation since the last accepted LaserScan.
Decider can use it to smoothen the trajectory in the case of high noise in the laser measurements
Definition at line 213 of file CICPCriteriaNRD.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState().
|
protected |
Keeps track of the last N measurements between the ICP edge and the corresponding odometry measurements.
Use the last odometry rigid body transformation instead of the ICP edge if the mahalanobis distance between them is greater than this limit.
Definition at line 233 of file CICPCriteriaNRD.h.
|
protectedinherited |
Provided messages with VerbosityLevel smaller than this value shall be ignored.
Definition at line 252 of file system/COutputLogger.h.
Referenced by mrpt::system::COutputLogger::getMinLoggingLevel(), and mrpt::system::COutputLogger::isLoggingLevelVisible().
|
protectedinherited |
Store the last registered NodeID.
We don't store its pose since it will most likely change due to calls to the graph-optimization procedure / dijkstra_node_estimation
Definition at line 121 of file CNodeRegistrationDecider.h.
|
protectedinherited |
Tracking the PDF of the current position of the robot with regards to the <b previous registered node.
Definition at line 125 of file CNodeRegistrationDecider.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D().
|
protectedinherited |
Time logger instance.
Definition at line 163 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState().
|
protected |
How many times we used the ICP Edge instead of Odometry edge.
Definition at line 240 of file CICPCriteriaNRD.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD().
|
protected |
How many times we used the Odometry Edge instead of the ICP edge.
Definition at line 242 of file CICPCriteriaNRD.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD().
|
protected |
Definition at line 236 of file CICPCriteriaNRD.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD().
|
protected |
Definition at line 237 of file CICPCriteriaNRD.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD().
|
protectedinherited |
Window to use.
Definition at line 155 of file CRegistrationDeciderOrOptimizer.h.
|
protectedinherited |
Pointer to the CWindowManager object used to store visuals-related instances.
Definition at line 153 of file CRegistrationDeciderOrOptimizer.h.
|
protectedinherited |
CWindowObserver object for monitoring various visual-oriented events.
Definition at line 158 of file CRegistrationDeciderOrOptimizer.h.
TParams mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::params |
Definition at line 171 of file CICPCriteriaNRD.h.
|
staticprotectedinherited |
Definition at line 174 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().
Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020 |