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 89 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... | |
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 |
Public Attributes | |
TParams | params |
Static Public Attributes | |
static mrpt::system::TConsoleColor | logging_levels_to_colors [NUMBER_OF_VERBOSITY_LEVELS] |
Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor. More... | |
static std::string | logging_levels_to_names [NUMBER_OF_VERBOSITY_LEVELS] |
Map from VerbosityLevels to their corresponding names. More... | |
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... | |
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 |
Static Protected Attributes | |
static const std::string | header_sep |
Separator string to be used in debugging messages. More... | |
static const std::string | report_sep |
Private Member Functions | |
std::string | generateStringFromFormat (const char *fmt, va_list argp) const |
Helper method for generating a std::string instance from printf-like arguments. More... | |
Private Attributes | |
std::string | m_logger_name |
std::deque< TMsg > | m_history |
std::deque< output_logger_callback_t > | m_listCallbacks |
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 111 of file CICPCriteriaNRD.h.
using mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::global_pose_t = typename GRAPH_T::global_pose_t |
Definition at line 103 of file CICPCriteriaNRD.h.
|
inherited |
Definition at line 51 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 106 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 114 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 102 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 110 of file CICPCriteriaNRD.h.
|
privateinherited |
Definition at line 81 of file CRangeScanOps.h.
mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD |
Class constructor.
Definition at line 23 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 45 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 97 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 249 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 153 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 242 of file CICPCriteriaNRD_impl.h.
References THROW_EXCEPTION.
|
protectedinherited |
Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method.
Definition at line 147 of file CRangeScanOps_impl.h.
|
protectedinherited |
Reduce the size of the given CPointsMap by keeping one out of "keep_point_every" points.
Definition at line 113 of file CRangeScanOps_impl.h.
|
inherited |
Dump the current contents of the COutputLogger instance in the terminal window.
Definition at line 179 of file COutputLogger.cpp.
|
privateinherited |
Helper method for generating a std::string instance from printf-like arguments.
Definition at line 99 of file COutputLogger.cpp.
References mrpt::system::os::vsnprintf().
|
inlineinherited |
Definition at line 139 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
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 318 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 20 of file CRangeScanOps_impl.h.
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 55 of file CRangeScanOps_impl.h.
|
inherited |
Get the history of COutputLogger instance in a string representation.
Definition at line 148 of file COutputLogger.cpp.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().
|
inherited |
Fill the provided string with the contents of the logger's history in std::string representation.
Definition at line 143 of file COutputLogger.cpp.
|
inherited |
Return the last Tmsg instance registered in the logger history.
Definition at line 184 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 190 of file COutputLogger.cpp.
|
inherited |
Return the name of the COutputLogger instance.
Definition at line 132 of file COutputLogger.cpp.
Referenced by mrpt::system::COutputLogger::TMsg::TMsg().
|
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 43 of file CRegistrationDeciderOrOptimizer_impl.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD().
|
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 90 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 146 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 285 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 120 of file COutputLogger.cpp.
|
inherited |
Definition at line 287 of file COutputLogger.cpp.
References getAddress().
|
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 80 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 195 of file COutputLogger.cpp.
References mrpt::system::LVL_INFO.
|
inherited |
Definition at line 274 of file COutputLogger.cpp.
|
inherited |
Main method to add the specified message string to the logger.
Definition at line 61 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 111 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 306 of file CICPCriteriaNRD_impl.h.
References MRPT_END, MRPT_START, and mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::printParams().
|
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 |
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 |
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 58 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 81 of file CRegistrationDeciderOrOptimizer_impl.h.
|
virtualinherited |
Fetch the graph on which the decider/optimizer will work on.
Definition at line 139 of file CRegistrationDeciderOrOptimizer_impl.h.
|
inherited |
Set the name of the COutputLogger instance.
Definition at line 127 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 133 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 138 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 67 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 50 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 130 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 234 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 105 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 154 of file COutputLogger.cpp.
References ASSERTMSG_, and mrpt::format().
|
staticprotectedinherited |
Separator string to be used in debugging messages.
Definition at line 175 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 171 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.
|
staticinherited |
Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor.
Implementation file for the COutputLogger header class.
Handy for coloring the input based on the verbosity of the message
Definition at line 124 of file system/COutputLogger.h.
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 129 of file system/COutputLogger.h.
Referenced by mrpt::system::COutputLogger::TMsg::getAsString().
|
protectedinherited |
Name of the class instance.
Definition at line 167 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::CRegistrationDeciderOrOptimizer< typename mrpt::graphs::CNetworkOfPoses2DInf >::getClassName().
|
protected |
Current LaserScan.
Set during the new measurements acquisition in updateState method
Definition at line 205 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 208 of file CICPCriteriaNRD.h.
|
protected |
pose_t estimation using only odometry information.
Definition at line 222 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 146 of file CRegistrationDeciderOrOptimizer.h.
|
protectedinherited |
Definition at line 147 of file CRegistrationDeciderOrOptimizer.h.
|
mutableprivateinherited |
Definition at line 312 of file system/COutputLogger.h.
|
protectedinherited |
Initial information matrix for paths.
Large values for this indicate that I am sure of the corresponding (initial) pose
Definition at line 135 of file CNodeRegistrationDecider.h.
|
protectedinherited |
Definition at line 161 of file CRegistrationDeciderOrOptimizer.h.
|
protected |
Definition at line 194 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 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 207 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 229 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 217 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().
|
privateinherited |
Definition at line 314 of file system/COutputLogger.h.
|
privateinherited |
Definition at line 310 of file system/COutputLogger.h.
|
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 237 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 125 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 129 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 165 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 244 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 246 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 240 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 241 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 157 of file CRegistrationDeciderOrOptimizer.h.
|
protectedinherited |
Pointer to the CWindowManager object used to store visuals-related instances.
Definition at line 155 of file CRegistrationDeciderOrOptimizer.h.
|
protectedinherited |
CWindowObserver object for monitoring various visual-oriented events.
Definition at line 160 of file CRegistrationDeciderOrOptimizer.h.
TParams mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::params |
Definition at line 175 of file CICPCriteriaNRD.h.
|
staticprotectedinherited |
Definition at line 176 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST |