Fixed Intervals Odometry-based Node Registration.
Determine whether to insert a new pose in the graph given the distance and angle thresholds. When the odometry readings indicate that any of the thresholds has been surpassed, with regards to the previous registered pose, a new node is added in the graph.
Current decider is a minimal, simple implementation of the CNodeRegistrationDecider interface which can be used for 2D datasets. Decider does not guarantee thread safety when accessing the GRAPH_T resource. This is handled by the CGraphSlamEngine instance.
Definition at line 74 of file CFixedIntervalsNRD.h.
#include <mrpt/graphslam/NRD/CFixedIntervalsNRD.h>
Classes | |
struct | TParams |
Parameters structure for managing the relevant to the decider variables in a compact manner. More... | |
Public Types | |
typedef mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > | node_reg |
Handy typedefs. More... | |
typedef GRAPH_T::constraint_t | constraint_t |
type of graph constraints More... | |
typedef GRAPH_T::constraint_t::type_value | pose_t |
type of underlying poses (2D/3D). More... | |
typedef GRAPH_T::global_pose_t | global_pose_t |
typedef mrpt::math::CMatrixFixedNumeric< double, constraint_t::state_length, constraint_t::state_length > | inf_mat_t |
typedef mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > | parent_t |
Node Registration Decider. More... | |
Public Member Functions | |
CFixedIntervalsNRD () | |
Class constructor. More... | |
~CFixedIntervalsNRD () | |
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::CActionCollectionPtr action, mrpt::obs::CSensoryFramePtr observations, mrpt::obs::CObservationPtr observation) |
Method makes use of the CActionCollection/CObservation to update the odometry estimation from the last inserted pose. 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 (mrpt::synch::CCriticalSection *graph_section) |
Fetch a mrpt::synch::CCriticalSection 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 |
Protected Member Functions | |
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... | |
Registration Conditions Specifiers | |
bool | checkRegistrationCondition () |
If estimated position surpasses the registration max values since the previous registered node, register a new node in the graph. More... | |
bool | checkRegistrationCondition (const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2) const |
bool | checkRegistrationCondition (const mrpt::poses::CPose3D &p1, const mrpt::poses::CPose3D &p2) const |
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 | |
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... | |
bool | m_observation_only_rawlog |
Keep track of whether we are reading from an observation-only rawlog file or from an action-observation rawlog. More... | |
mrpt::utils::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... | |
mrpt::synch::CCriticalSection * | m_graph_section |
mrpt::utils::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... | |
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 = std::string(80, '-') |
Separator string to be used in debugging messages. More... | |
static const std::string | report_sep = std::string(2, '\n') |
typedef GRAPH_T::constraint_t mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::constraint_t |
type of graph constraints
Definition at line 87 of file CFixedIntervalsNRD.h.
typedef GRAPH_T::global_pose_t mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::global_pose_t |
Definition at line 90 of file CFixedIntervalsNRD.h.
typedef mrpt::math::CMatrixFixedNumeric<double, constraint_t::state_length, constraint_t::state_length> mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::inf_mat_t |
Definition at line 94 of file CFixedIntervalsNRD.h.
typedef mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T> mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::node_reg |
typedef mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T> mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::parent_t |
Node Registration Decider.
Definition at line 96 of file CFixedIntervalsNRD.h.
typedef GRAPH_T::constraint_t::type_value mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::pose_t |
type of underlying poses (2D/3D).
Definition at line 89 of file CFixedIntervalsNRD.h.
mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::CFixedIntervalsNRD | ( | ) |
Class constructor.
Definition at line 19 of file CFixedIntervalsNRD_impl.h.
mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::~CFixedIntervalsNRD | ( | ) |
Class destructor.
Definition at line 26 of file CFixedIntervalsNRD_impl.h.
|
protectedvirtualinherited |
Get a global_pose_t and fill the NODE_ANNOTATIONS-related fields.
Definition at line 129 of file CNodeRegistrationDecider_impl.h.
|
protectedvirtualinherited |
Handy function for making all the visuals assertions in a compact manner.
Definition at line 87 of file CRegistrationDeciderOrOptimizer_impl.h.
|
protectedvirtual |
If estimated position surpasses the registration max values since the previous registered node, register a new node in the graph.
Reimplemented from mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T >.
Definition at line 91 of file CFixedIntervalsNRD_impl.h.
References INVALID_NODEID, MRPT_END, and MRPT_START.
|
protected |
Definition at line 112 of file CFixedIntervalsNRD_impl.h.
References mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::distanceTo(), mrpt::poses::CPose2D::phi(), and mrpt::math::wrapToPi().
|
protected |
Definition at line 127 of file CFixedIntervalsNRD_impl.h.
References mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::distanceTo(), mrpt::poses::CPose3D::pitch(), mrpt::poses::CPose3D::roll(), mrpt::math::wrapToPi(), and mrpt::poses::CPose3D::yaw().
|
inlineinherited |
Definition at line 138 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 134 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 175 of file CFixedIntervalsNRD_impl.h.
References MRPT_END, and MRPT_START.
|
virtualinherited |
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand.
Definition at line 38 of file CRegistrationDeciderOrOptimizer_impl.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::CICPCriteriaERD(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::CLevMarqGSO(), and mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::CLoopCloserERD().
|
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::deciders::CLoopCloserERD< GRAPH_T >, mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >, and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >.
Definition at line 81 of file CRegistrationDeciderOrOptimizer_impl.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::initializeVisuals(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initializeVisuals().
|
inherited |
Definition at line 132 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 144 of file CFixedIntervalsNRD_impl.h.
References MRPT_END, and MRPT_START.
|
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::deciders::CLoopCloserERD< GRAPH_T >, mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >, and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >.
Definition at line 99 of file CRegistrationDeciderOrOptimizer_impl.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::notifyOfWindowEvents(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::notifyOfWindowEvents().
|
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 166 of file CFixedIntervalsNRD_impl.h.
References MRPT_END, and MRPT_START.
|
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 45 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 106 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 116 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 52 of file CRegistrationDeciderOrOptimizer_impl.h.
|
virtualinherited |
Fetch a mrpt::synch::CCriticalSection for locking the GRAPH_T resource.
Handy for realising multithreading in the derived classes.
Definition at line 73 of file CRegistrationDeciderOrOptimizer_impl.h.
|
virtualinherited |
Fetch the graph on which the decider/optimizer will work on.
Definition at line 124 of file CRegistrationDeciderOrOptimizer_impl.h.
|
virtualinherited |
Fetch a CWindowManager pointer.
CWindowManager instance should contain a CDisplayWindow3D* and, optionally, a CWindowObserver pointer so that interaction with the window is possible
Reimplemented in mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >.
Definition at line 61 of file CRegistrationDeciderOrOptimizer_impl.h.
|
virtual |
Method makes use of the CActionCollection/CObservation to update the odometry estimation from the last inserted pose.
Implements mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T >.
Definition at line 32 of file CFixedIntervalsNRD_impl.h.
References mrpt::poses::CPose3DPDFGaussian::copyFrom(), IS_CLASS, MRPT_END, MRPT_START, and MRPT_UNUSED_PARAM.
|
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::deciders::CLoopCloserERD< GRAPH_T >, mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >, and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >.
Definition at line 94 of file CRegistrationDeciderOrOptimizer_impl.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateVisuals(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateVisuals().
|
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(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::getDescriptiveReport(), and mrpt::graphslam::optimizers::CLevMarqGSO< 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.
|
protectedinherited |
Name of the class instance.
Definition at line 167 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_t >::getClassName().
|
protected |
pose_t estimation using only odometry information.
Handy for observation-only rawlogs.
Definition at line 170 of file CFixedIntervalsNRD.h.
|
protectedinherited |
Pointer to the graph that is under construction.
Definition at line 146 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::_optimizeGraph(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::checkForLoopClosures(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition2D(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition3D(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::getNearbyNodesOf(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::getNearbyNodesOf(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::registerNewEdge(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateGraphVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateOptDistanceVisualization(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateState(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateState(), and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateVisuals().
|
protectedinherited |
Definition at line 147 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::optimizeGraph().
|
protectedinherited |
Initial information matrix for paths.
Large values for this indicate that I am sure of the corresponding (initial) pose
Definition at line 140 of file CNodeRegistrationDecider.h.
|
protectedinherited |
Definition at line 161 of file CRegistrationDeciderOrOptimizer.h.
|
protected |
pose_t estimation using only odometry information.
Handy for observation-only rawlogs.
Definition at line 174 of file CFixedIntervalsNRD.h.
|
protected |
Keep track of whether we are reading from an observation-only rawlog file or from an action-observation rawlog.
Definition at line 178 of file CFixedIntervalsNRD.h.
|
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 130 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 134 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::optimizers::CLevMarqGSO< GRAPH_T >::_optimizeGraph(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition2D(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition3D(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::getDescriptiveReport(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::getDescriptiveReport(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::initializeVisuals(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState(), and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateVisuals().
|
protectedinherited |
Window to use.
Definition at line 157 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::fitGraphInView(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::initializeVisuals(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initOptDistanceVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::toggleGraphVisualization(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::toggleLaserScansVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::toggleOptDistanceVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateGraphVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateOptDistanceVisualization(), and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateVisuals().
|
protectedinherited |
Pointer to the CWindowManager object used to store visuals-related instances.
Definition at line 155 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initGraphVisualization(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::initializeVisuals(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initOptDistanceVisualization(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::toggleLaserScansVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateGraphVisualization(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateOptDistanceVisualization().
|
protectedinherited |
CWindowObserver object for monitoring various visual-oriented events.
Definition at line 160 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initGraphVisualization(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::initializeVisuals(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initOptDistanceVisualization().
TParams mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::params |
Definition at line 142 of file CFixedIntervalsNRD.h.
|
staticprotectedinherited |
Definition at line 176 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::getDescriptiveReport(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::getDescriptiveReport().
Page generated by Doxygen 1.8.14 for MRPT 1.5.8 Git: f67d0f871 Wed Sep 25 18:32:17 2019 +0200 at lun oct 28 01:58:29 CET 2019 |