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 77 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::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr 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 (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 |
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... | |
std::mutex * | 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 |
Separator string to be used in debugging messages. More... | |
static const std::string | report_sep |
typedef GRAPH_T::constraint_t mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::constraint_t |
type of graph constraints
Definition at line 92 of file CFixedIntervalsNRD.h.
typedef GRAPH_T::global_pose_t mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::global_pose_t |
Definition at line 95 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 99 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 102 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 94 of file CFixedIntervalsNRD.h.
mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::CFixedIntervalsNRD | ( | ) |
Class constructor.
Definition at line 23 of file CFixedIntervalsNRD_impl.h.
mrpt::graphslam::deciders::CFixedIntervalsNRD< GRAPH_T >::~CFixedIntervalsNRD | ( | ) |
Class destructor.
Definition at line 31 of file CFixedIntervalsNRD_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 98 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 107 of file CFixedIntervalsNRD_impl.h.
References INVALID_NODEID, MRPT_END, and MRPT_START.
|
protected |
Definition at line 132 of file CFixedIntervalsNRD_impl.h.
References mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::distanceTo(), mrpt::poses::CPose2D::phi(), and mrpt::math::wrapToPi().
|
protected |
Definition at line 148 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 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
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 198 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 43 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 91 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 149 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 168 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::optimizers::CLevMarqGSO< GRAPH_T >, and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >.
Definition at line 112 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 188 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 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 59 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 82 of file CRegistrationDeciderOrOptimizer_impl.h.
|
virtualinherited |
Fetch the graph on which the decider/optimizer will work on.
Definition at line 140 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
Definition at line 68 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 39 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::optimizers::CLevMarqGSO< GRAPH_T >, and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >.
Definition at line 106 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 176 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 141 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 180 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 184 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 131 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 135 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 150 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.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019 |