MRPT  2.0.0
List of all members | Classes | Public Member Functions | Static Public Member Functions | Public Attributes | Protected Types | Protected Member Functions | Static Protected Member Functions | Protected Attributes | Static Protected Attributes
mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T > Class Template Reference

Detailed Description

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >

Levenberg-Marquardt non-linear graph slam optimization scheme.

Current decider optimizes the graph according to the graphslam::optimize_spa_levmarq method. Refer to the latter for more details on the implementation.

The configuration parameters of the current decider class are listed below. These parameters are parsed from an external .ini configuration file from their respective section. If an optional parameter is not found a default value is used instead. If a mandatory/required parameter is not found a runtime error is raised. For a sample configuration file see $mrpt/share/mrpt/config_files/graphslam-engine/laser_odometry.ini (Modify $mrpt according to the path of your mrpt source directory).

Definition at line 117 of file CLevMarqGSO.h.

#include <mrpt/graphslam/GSO/CLevMarqGSO.h>

Inheritance diagram for mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >:

Classes

struct  GraphVisualizationParams
 struct for holding the graph visualization-related variables in a compact form More...
 
struct  OptimizationParams
 Struct for holding the optimization-related variables in a compact form. More...
 

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 InfMat = mrpt::math::CMatrixFixed< double, constraint_t::state_length, constraint_t::state_length >
 
using grandpa = mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >
 
using parent = mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T >
 

Public Member Functions

 CLevMarqGSO ()
 
 ~CLevMarqGSO () override=default
 
bool updateState (mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) override
 Generic method for fetching the incremental action-observations (or observation-only) measurements. More...
 
void initializeVisuals () override
 Initialize visual objects in CDisplayWindow (e.g. More...
 
void updateVisuals () override
 Update the relevant visual features in CDisplayWindow. More...
 
void notifyOfWindowEvents (const std::map< std::string, bool > &events_occurred) override
 Get a list of the window events that happened since the last call. More...
 
void loadParams (const std::string &source_fname) override
 Load the necessary for the decider/optimizer configuration parameters. More...
 
void printParams () const override
 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 override
 Fill the provided string with a detailed report of the decider/optimizer state. More...
 
bool justFullyOptimizedGraph () const override
 Used by the caller to query for possible full graph optimization on the latest optimizer run. 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 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

OptimizationParams opt_params
 Parameters relevant to the optimizatio nfo the graph. More...
 
GraphVisualizationParams viz_params
 Parameters relevant to the visualization of the graph. More...
 

Protected Types

enum  OptimizationPolicy { OptimizationPolicy::IgnoreLoopClosures = 0, OptimizationPolicy::UseLoopClosures, OptimizationPolicy::Count }
 Enumeration that defines the behaviors towards using or ignoring a newly added loop closure to fully optimize the graph. More...
 

Protected Member Functions

void _optimizeGraph (bool is_full_update=false)
 Optimize the given graph. More...
 
void optimizeGraph () override
 Wrapper around _optimizeGraph which first locks the section and then calls the _optimizeGraph method. More...
 
bool checkForLoopClosures ()
 Check if a loop closure edge was added in the graph. More...
 
bool checkForFullOptimization ()
 Decide whether to issue a full graph optimization. More...
 
void initGraphVisualization ()
 Initialize objects relateed to the Graph Visualization. More...
 
void updateGraphVisualization ()
 Called internally for updating the visualization scene for the graph building procedure. More...
 
void toggleGraphVisualization ()
 Toggle the graph visualization on and off. More...
 
void fitGraphInView ()
 Set the camera parameters of the CDisplayWindow3D so that the whole graph is viewed in the window. More...
 
void updateOptDistanceVisualization ()
 Update the position of the disk indicating the distance in which Levenberg-Marquardt graph optimization is executed. More...
 
void toggleOptDistanceVisualization ()
 toggle the optimization distance object on and off More...
 
void getNearbyNodesOf (std::set< mrpt::graphs::TNodeID > *nodes_set, const mrpt::graphs::TNodeID &cur_nodeID, double distance)
 Get a list of the nodeIDs whose position is within a certain distance to the specified nodeID. More...
 
virtual void assertVisualsVars ()
 Handy function for making all the visuals assertions in a compact manner. More...
 
void initOptDistanceVisualization ()
 Initialize the Disk/Sphere used for visualizing the optimization distance. More...
 
mrpt::opengl::CRenderizable::Ptr initOptDistanceVisualizationInternal (const mrpt::poses::CPose2D &p_unused)
 Setup the corresponding Disk/Sphere instance. More...
 
mrpt::opengl::CRenderizable::Ptr initOptDistanceVisualizationInternal (const mrpt::poses::CPose3D &p_unused)
 

Static Protected Member Functions

static void levMarqFeedback (const GRAPH_T &graph, const size_t iter, const size_t max_iter, const double cur_sq_error)
 Feedback of the Levenberg-Marquardt graph optimization procedure. More...
 

Protected Attributes

bool m_first_time_call {false}
 
bool m_has_read_config {false}
 
bool registered_new_node
 
bool m_autozoom_active {true}
 
size_t m_last_total_num_of_nodes {5}
 
std::thread m_thread_optimize
 
OptimizationPolicy m_optimization_policy
 
bool m_just_fully_optimized_graph {false}
 Indicates whether a full graph optimization was just issued. More...
 
size_t m_min_nodes_for_optimization {3}
 Minimum number of nodes before we try optimizing the graph. More...
 
GRAPH_T * m_graph = nullptr
 Pointer to the graph that is under construction. More...
 
std::mutex * m_graph_section = nullptr
 
mrpt::system::CTimeLogger m_time_logger
 Time logger instance. More...
 
std::string m_class_name = "CRegistrationDeciderOrOptimizer"
 Name of the class instance. More...
 
bool is_mr_slam_class = false
 Boolean indicating if the current class can be used in multi-robot SLAM operations. More...
 
VerbosityLevel m_min_verbosity_level {LVL_INFO}
 Provided messages with VerbosityLevel smaller than this value shall be ignored. More...
 
Smart Full-Optimization Command

Instead of issuing a full optimization every time a loop closure is detected, ignore current loop closure when enough consecutive loop closures have already been utilised.

This avoids the added computational cost that is needed for optimizing the graph without reducing the accuracy of the overall operation

size_t m_max_used_consec_lcs
 Number of maximum cosecutive loop closures that are allowed to be issued. More...
 
size_t m_curr_used_consec_lcs {0}
 Number of consecutive loop closures that are currently registered. More...
 
size_t m_max_ignored_consec_lcs
 Number of consecutive loop closures to ignore after m_max_used_consec_lcs have already been issued. More...
 
size_t m_curr_ignored_consec_lcs {0}
 Consecutive Loop Closures that have currently been ignored. More...
 
Visuals-related variables methods
mrpt::graphslam::CWindowManagerm_win_manager = nullptr
 Pointer to the CWindowManager object used to store visuals-related instances. More...
 
mrpt::gui::CDisplayWindow3Dm_win = nullptr
 Window to use. More...
 
mrpt::graphslam::CWindowObserverm_win_observer = nullptr
 CWindowObserver object for monitoring various visual-oriented events. More...
 
bool m_initialized_visuals = false
 

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 {true}
 [Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically. More...
 
bool logging_enable_keep_record {false}
 [Default=false] Enables storing all messages into an internal list. More...
 
void logStr (const VerbosityLevel level, std::string_view msg_str) const
 Main method to add the specified message string to the logger. More...
 
void logFmt (const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
 Alternative logging method, which mimics the printf behavior. More...
 
void void logCond (const VerbosityLevel level, bool cond, const std::string &msg_str) const
 Log the given message only if the condition is satisfied. More...
 
void setLoggerName (const std::string &name)
 Set the name of the COutputLogger instance. More...
 
std::string getLoggerName () const
 Return the name of the COutputLogger instance. More...
 
void setMinLoggingLevel (const VerbosityLevel level)
 Set the minimum logging level for which the incoming logs are going to be taken into account. More...
 
void setVerbosityLevel (const VerbosityLevel level)
 alias of setMinLoggingLevel() More...
 
VerbosityLevel getMinLoggingLevel () const
 
bool isLoggingLevelVisible (VerbosityLevel level) const
 
void getLogAsString (std::string &log_contents) const
 Fill the provided string with the contents of the logger's history in std::string representation. More...
 
std::string getLogAsString () const
 Get the history of COutputLogger instance in a string representation. More...
 
void writeLogToFile (const std::string *fname_in=nullptr) const
 Write the contents of the COutputLogger instance to an external file. More...
 
void dumpLogToConsole () const
 Dump the current contents of the COutputLogger instance in the terminal window. More...
 
std::string getLoggerLastMsg () const
 Return the last Tmsg instance registered in the logger history. More...
 
void getLoggerLastMsg (std::string &msg_str) const
 Fill inputtted string with the contents of the last message in history. More...
 
void loggerReset ()
 Reset the contents of the logger instance. More...
 
void logRegisterCallback (output_logger_callback_t userFunc)
 
bool logDeregisterCallback (output_logger_callback_t userFunc)
 

Member Typedef Documentation

◆ constraint_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
using mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::constraint_t = typename GRAPH_T::constraint_t

Handy typedefs.

Definition at line 125 of file CLevMarqGSO.h.

◆ grandpa

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
using mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::grandpa = mrpt::graphslam::CRegistrationDeciderOrOptimizer<GRAPH_T>

Definition at line 130 of file CLevMarqGSO.h.

◆ InfMat

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
using mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::InfMat = mrpt::math::CMatrixFixed< double, constraint_t::state_length, constraint_t::state_length>

Definition at line 129 of file CLevMarqGSO.h.

◆ parent

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
using mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::parent = mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_T>

Definition at line 131 of file CLevMarqGSO.h.

◆ pose_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
using mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::pose_t = typename GRAPH_T::constraint_t::type_value

type of underlying poses (2D/3D)

Definition at line 127 of file CLevMarqGSO.h.

Member Enumeration Documentation

◆ OptimizationPolicy

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
enum mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationPolicy
strongprotected

Enumeration that defines the behaviors towards using or ignoring a newly added loop closure to fully optimize the graph.

Enumerator
IgnoreLoopClosures 
UseLoopClosures 
Count 

Definition at line 340 of file CLevMarqGSO.h.

Constructor & Destructor Documentation

◆ CLevMarqGSO()

template<class GRAPH_T >
mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::CLevMarqGSO ( )

Definition at line 15 of file CLevMarqGSO_impl.h.

References mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::initializeLoggers().

Here is the call graph for this function:

◆ ~CLevMarqGSO()

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::~CLevMarqGSO ( )
overridedefault

Member Function Documentation

◆ _optimizeGraph()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::_optimizeGraph ( bool  is_full_update = false)
protected

Optimize the given graph.

Wrapper around the graphslam::optimize_spa_levmarq method

See also
optimize_spa_levmarq, optimizeGraph
Parameters
[in]full_updateImpose that method optimizes the whole graph

Definition at line 395 of file CLevMarqGSO_impl.h.

References mrpt::system::LVL_DEBUG, MRPT_END, MRPT_START, mrpt::graphslam::optimize_graph_spa_levmarq(), mrpt::system::CTicTac::Tac(), and mrpt::system::CTicTac::Tic().

Here is the call graph for this function:

◆ assertVisualsVars()

template<class GRAPH_T >
void mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::assertVisualsVars ( )
protectedvirtualinherited

Handy function for making all the visuals assertions in a compact manner.

Definition at line 74 of file CRegistrationDeciderOrOptimizer_impl.h.

◆ checkForFullOptimization()

template<class GRAPH_T >
bool mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::checkForFullOptimization ( )
protected

Decide whether to issue a full graph optimization.

In case N consecutive full optimizations have been issued, skip some of the next as they slow down the overall execution and they don't reduce the overall error

Returns
True for issuing a full graph optimization, False otherwise

Definition at line 509 of file CLevMarqGSO_impl.h.

References MRPT_LOG_DEBUG_STREAM, and MRPT_LOG_WARN_STREAM.

◆ checkForLoopClosures()

template<class GRAPH_T >
bool mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::checkForLoopClosures ( )
protected

Check if a loop closure edge was added in the graph.

Match the previously registered edges in the graph with the current. If there is a node difference in any new edge greater than LC_min_nodeid_diff (see .ini parameter) then new constraint is considered a Loop Closure

Returns
True if any of the newly added edges is considered a loop closure

Definition at line 465 of file CLevMarqGSO_impl.h.

References mrpt::system::LVL_DEBUG, MRPT_END, and MRPT_START.

◆ dumpLogToConsole()

void COutputLogger::dumpLogToConsole ( ) const
inherited

Dump the current contents of the COutputLogger instance in the terminal window.

See also
writeToFile

Definition at line 190 of file COutputLogger.cpp.

◆ fitGraphInView()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::fitGraphInView ( )
inlineprotected

Set the camera parameters of the CDisplayWindow3D so that the whole graph is viewed in the window.

Warning
Method assumes that the COpenGLinstance is not locked prior to this call, so make sure you have issued CDisplayWindow3D::unlockAccess3DScene() before calling this method.

Definition at line 225 of file CLevMarqGSO_impl.h.

References ASSERTDEBMSG_, MRPT_END, and MRPT_START.

◆ getClassName()

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
std::string mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::getClassName ( ) const
inlineinherited

Definition at line 134 of file CRegistrationDeciderOrOptimizer.h.

◆ getDescriptiveReport()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::getDescriptiveReport ( std::string *  report_str) const
overridevirtual

Fill the provided string with a detailed report of the decider/optimizer state.

Report should include (part of) the following:

  • Timing of important methods
  • Properties fo class at the current time
  • Logging of commands until current time

Reimplemented from mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.

Definition at line 670 of file CLevMarqGSO_impl.h.

References MRPT_END, and MRPT_START.

◆ getLogAsString() [1/2]

void COutputLogger::getLogAsString ( std::string &  log_contents) const
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.

◆ getLogAsString() [2/2]

std::string COutputLogger::getLogAsString ( ) const
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().

Here is the caller graph for this function:

◆ getLoggerLastMsg() [1/2]

std::string COutputLogger::getLoggerLastMsg ( ) const
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().

Here is the call graph for this function:

◆ getLoggerLastMsg() [2/2]

void COutputLogger::getLoggerLastMsg ( std::string &  msg_str) const
inherited

Fill inputtted string with the contents of the last message in history.

Definition at line 201 of file COutputLogger.cpp.

◆ getLoggerName()

std::string COutputLogger::getLoggerName ( ) const
inherited

Return the name of the COutputLogger instance.

See also
setLoggerName

Definition at line 143 of file COutputLogger.cpp.

◆ getMinLoggingLevel()

VerbosityLevel mrpt::system::COutputLogger::getMinLoggingLevel ( ) const
inlineinherited

◆ getNearbyNodesOf()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::getNearbyNodesOf ( std::set< mrpt::graphs::TNodeID > *  nodes_set,
const mrpt::graphs::TNodeID cur_nodeID,
double  distance 
)
protected

Get a list of the nodeIDs whose position is within a certain distance to the specified nodeID.

Definition at line 602 of file CLevMarqGSO_impl.h.

References mrpt::math::distance(), MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ initGraphVisualization()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initGraphVisualization ( )
inlineprotected

Initialize objects relateed to the Graph Visualization.

Definition at line 137 of file CLevMarqGSO_impl.h.

References ASSERTDEBMSG_, MRPT_END, and MRPT_START.

◆ initializeLoggers()

template<class GRAPH_T >
void mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::initializeLoggers ( const std::string &  name)
virtualinherited

Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand.

Definition at line 22 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().

Here is the caller graph for this function:

◆ initializeVisuals()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initializeVisuals ( )
overridevirtual

Initialize visual objects in CDisplayWindow (e.g.

add an object to scene).

Exceptions
std::exceptionIf the method is called without having first provided a CDisplayWindow3D* to the class instance
See also
setWindowManagerPtr, updateVisuals

Reimplemented from mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.

Definition at line 60 of file CLevMarqGSO_impl.h.

References ASSERTDEB_, MRPT_END, and MRPT_START.

◆ initOptDistanceVisualization()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initOptDistanceVisualization ( )
protected

Initialize the Disk/Sphere used for visualizing the optimization distance.

Definition at line 264 of file CLevMarqGSO_impl.h.

References MRPT_END, and MRPT_START.

◆ initOptDistanceVisualizationInternal() [1/2]

template<class GRAPH_T >
mrpt::opengl::CRenderizable::Ptr mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initOptDistanceVisualizationInternal ( const mrpt::poses::CPose2D p_unused)
protected

Setup the corresponding Disk/Sphere instance.

Method overloads are used to overcome the C++ specialization restrictions

Returns
Disk/Sphere instance for 2D/3D SLAM respectively

Definition at line 306 of file CLevMarqGSO_impl.h.

◆ initOptDistanceVisualizationInternal() [2/2]

template<class GRAPH_T >
mrpt::opengl::CRenderizable::Ptr mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initOptDistanceVisualizationInternal ( const mrpt::poses::CPose3D p_unused)
protected

Definition at line 321 of file CLevMarqGSO_impl.h.

◆ isLoggingLevelVisible()

bool mrpt::system::COutputLogger::isLoggingLevelVisible ( VerbosityLevel  level) const
inlineinherited

Definition at line 202 of file system/COutputLogger.h.

References mrpt::system::COutputLogger::m_min_verbosity_level.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().

Here is the caller graph for this function:

◆ isMultiRobotSlamClass()

template<class GRAPH_T >
bool mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::isMultiRobotSlamClass ( )
inherited

Definition at line 123 of file CRegistrationDeciderOrOptimizer_impl.h.

◆ justFullyOptimizedGraph()

template<class GRAPH_T >
bool mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::justFullyOptimizedGraph ( ) const
overridevirtual

Used by the caller to query for possible full graph optimization on the latest optimizer run.

Reimplemented from mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T >.

Definition at line 589 of file CLevMarqGSO_impl.h.

◆ levMarqFeedback()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::levMarqFeedback ( const GRAPH_T &  graph,
const size_t  iter,
const size_t  max_iter,
const double  cur_sq_error 
)
staticprotected

Feedback of the Levenberg-Marquardt graph optimization procedure.

Definition at line 595 of file CLevMarqGSO_impl.h.

◆ loadParams()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::loadParams ( const std::string &  source_fname)
overridevirtual

Load the necessary for the decider/optimizer configuration parameters.

Reimplemented from mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.

Definition at line 639 of file CLevMarqGSO_impl.h.

References MRPT_END, MRPT_LOG_DEBUG, MRPT_START, mrpt::config::CConfigFileBase::read_int(), and setMinLoggingLevel().

Here is the call graph for this function:

◆ logCond()

void COutputLogger::logCond ( const VerbosityLevel  level,
bool  cond,
const std::string &  msg_str 
) const
inherited

Log the given message only if the condition is satisfied.

See also
log, logFmt

Definition at line 131 of file COutputLogger.cpp.

◆ logDeregisterCallback()

bool COutputLogger::logDeregisterCallback ( output_logger_callback_t  userFunc)
inherited
Returns
true if an entry was found and deleted.

Definition at line 291 of file COutputLogger.cpp.

References getAddress(), and mrpt::system::COutputLogger::m_listCallbacks.

Here is the call graph for this function:

◆ logFmt()

void COutputLogger::logFmt ( const VerbosityLevel  level,
const char *  fmt,
  ... 
) const
inherited

◆ loggerReset()

void COutputLogger::loggerReset ( )
inherited

Reset the contents of the logger instance.

Called upon construction.

Definition at line 206 of file COutputLogger.cpp.

References mrpt::system::LVL_INFO.

◆ logging_levels_to_colors()

std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > & COutputLogger::logging_levels_to_colors ( )
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().

Here is the caller graph for this function:

◆ logging_levels_to_names()

std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > & COutputLogger::logging_levels_to_names ( )
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().

Here is the caller graph for this function:

◆ logRegisterCallback()

void COutputLogger::logRegisterCallback ( output_logger_callback_t  userFunc)
inherited

Definition at line 278 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::m_listCallbacks.

◆ logStr()

void COutputLogger::logStr ( const VerbosityLevel  level,
std::string_view  msg_str 
) const
inherited

Main method to add the specified message string to the logger.

See also
logCond, logFmt

Definition at line 72 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::TMsg::body, mrpt::system::COutputLogger::TMsg::dumpToConsole(), mrpt::system::COutputLogger::TMsg::level, mrpt::system::COutputLogger::TMsg::name, and mrpt::system::COutputLogger::TMsg::timestamp.

Referenced by mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper(), and mrpt::system::CTimeLoggerSaveAtDtor::~CTimeLoggerSaveAtDtor().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ notifyOfWindowEvents()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::notifyOfWindowEvents ( const std::map< std::string, bool > &  events_occurred)
overridevirtual

Get a list of the window events that happened since the last call.

Reimplemented from mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.

Definition at line 89 of file CLevMarqGSO_impl.h.

References MRPT_END, MRPT_LOG_DEBUG_STREAM, and MRPT_START.

◆ optimizeGraph()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::optimizeGraph ( )
overrideprotectedvirtual

Wrapper around _optimizeGraph which first locks the section and then calls the _optimizeGraph method.

Used in multithreaded optimization

See also
_optimizeGraph()

Implements mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T >.

Definition at line 374 of file CLevMarqGSO_impl.h.

References mrpt::system::LVL_DEBUG, MRPT_END, MRPT_LOG_DEBUG_STREAM, and MRPT_START.

Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateState().

Here is the caller graph for this function:

◆ printParams()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::printParams ( ) const
overridevirtual

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 631 of file CLevMarqGSO_impl.h.

◆ setClassName()

template<class GRAPH_T >
void mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::setClassName ( const std::string &  name)
virtualinherited

Definition at line 35 of file CRegistrationDeciderOrOptimizer_impl.h.

◆ setCriticalSectionPtr()

template<class GRAPH_T >
void mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::setCriticalSectionPtr ( std::mutex *  graph_section)
virtualinherited

Fetch a std::mutex for locking the GRAPH_T resource.

Handy for realising multithreading in the derived classes.

Warning
Beware that prior to the decider/optimizer public method call, the CCriticalSection will already be locked from CGraphSlamEngine_t instance, but this isn't effective in multithreaded implementations where the decider/optimizer itself has to lock the function at which the extra thread runs.

Definition at line 58 of file CRegistrationDeciderOrOptimizer_impl.h.

◆ setGraphPtr()

template<class GRAPH_T>
void mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::setGraphPtr ( GRAPH_T *  graph)
virtualinherited

Fetch the graph on which the decider/optimizer will work on.

Definition at line 116 of file CRegistrationDeciderOrOptimizer_impl.h.

◆ setLoggerName()

void COutputLogger::setLoggerName ( const std::string &  name)
inherited

◆ setMinLoggingLevel()

void COutputLogger::setMinLoggingLevel ( const VerbosityLevel  level)
inherited

Set the minimum logging level for which the incoming logs are going to be taken into account.

String messages with specified VerbosityLevel smaller than the min, will not be outputted to the screen and neither will a record of them be stored in by the COutputLogger instance

Definition at line 144 of file COutputLogger.cpp.

Referenced by mrpt::maps::CRandomFieldGridMap2D::enableVerbose(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), generic_kf_slam_test(), generic_pf_test(), generic_rbpf_slam_test(), mrpt::apps::RawlogGrabberApp::initialize(), mrpt::hwdrivers::CHokuyoURG::initialize(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::loadParams(), mrpt::apps::CGridMapAlignerApp::run(), mrpt::apps::RBPF_SLAM_App_Base::run(), and mrpt::apps::ICP_SLAM_App_Base::run().

Here is the caller graph for this function:

◆ setVerbosityLevel()

void COutputLogger::setVerbosityLevel ( const VerbosityLevel  level)
inherited

◆ setWindowManagerPtr()

template<class GRAPH_T >
void mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::setWindowManagerPtr ( mrpt::graphslam::CWindowManager win_manager)
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 44 of file CRegistrationDeciderOrOptimizer_impl.h.

◆ toggleGraphVisualization()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::toggleGraphVisualization ( )
protected

Toggle the graph visualization on and off.

Definition at line 208 of file CLevMarqGSO_impl.h.

References MRPT_END, and MRPT_START.

◆ toggleOptDistanceVisualization()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::toggleOptDistanceVisualization ( )
protected

toggle the optimization distance object on and off

Definition at line 357 of file CLevMarqGSO_impl.h.

References MRPT_END, and MRPT_START.

◆ updateGraphVisualization()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateGraphVisualization ( )
inlineprotected

Called internally for updating the visualization scene for the graph building procedure.

Definition at line 157 of file CLevMarqGSO_impl.h.

References ASSERTDEBMSG_, mrpt::format(), mrpt::system::LVL_DEBUG, MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ updateOptDistanceVisualization()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateOptDistanceVisualization ( )
inlineprotected

Update the position of the disk indicating the distance in which Levenberg-Marquardt graph optimization is executed.

Definition at line 338 of file CLevMarqGSO_impl.h.

References ASSERTDEBMSG_, MRPT_END, and MRPT_START.

◆ updateState()

template<class GRAPH_T >
bool mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateState ( mrpt::obs::CActionCollection::Ptr  action,
mrpt::obs::CSensoryFrame::Ptr  observations,
mrpt::obs::CObservation::Ptr  observation 
)
overridevirtual

Generic method for fetching the incremental action-observations (or observation-only) measurements.

Returns
True if operation was successful. Criteria for Success depend on the decider/optimizer implementing this method

Implements mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.

Definition at line 23 of file CLevMarqGSO_impl.h.

References MRPT_END, MRPT_START, and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::optimizeGraph().

Here is the call graph for this function:

◆ updateVisuals()

template<class GRAPH_T >
void mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateVisuals ( )
overridevirtual

Update the relevant visual features in CDisplayWindow.

Exceptions
std::exceptionIf the method is called without having first provided a CDisplayWindow3D* to the class instance
See also
setWindowManagerPtr, initializeVisuals

Reimplemented from mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.

Definition at line 73 of file CLevMarqGSO_impl.h.

References MRPT_END, and MRPT_START.

◆ writeLogToFile()

void COutputLogger::writeLogToFile ( const std::string *  fname_in = nullptr) const
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

See also
dumpToConsole, getAsString

Definition at line 165 of file COutputLogger.cpp.

References ASSERTMSG_, and mrpt::format().

Here is the call graph for this function:

Member Data Documentation

◆ header_sep

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
const std::string mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::header_sep
staticprotectedinherited
Initial value:
=
std::string(80, '-')

Separator string to be used in debugging messages.

Definition at line 171 of file CRegistrationDeciderOrOptimizer.h.

Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().

◆ is_mr_slam_class

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
bool mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::is_mr_slam_class = false
protectedinherited

Boolean indicating if the current class can be used in multi-robot SLAM operations.

Definition at line 167 of file CRegistrationDeciderOrOptimizer.h.

◆ logging_enable_console_output

bool mrpt::system::COutputLogger::logging_enable_console_output {true}
inherited

[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically.

Definition at line 240 of file system/COutputLogger.h.

◆ logging_enable_keep_record

bool mrpt::system::COutputLogger::logging_enable_keep_record {false}
inherited

[Default=false] Enables storing all messages into an internal list.

See also
writeLogToFile, getLogAsString

Definition at line 243 of file system/COutputLogger.h.

◆ m_autozoom_active

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
bool mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_autozoom_active {true}
protected

Definition at line 328 of file CLevMarqGSO.h.

◆ m_class_name

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
std::string mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::m_class_name = "CRegistrationDeciderOrOptimizer"
protectedinherited

Name of the class instance.

Definition at line 163 of file CRegistrationDeciderOrOptimizer.h.

Referenced by mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_t >::getClassName().

◆ m_curr_ignored_consec_lcs

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
size_t mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_curr_ignored_consec_lcs {0}
protected

Consecutive Loop Closures that have currently been ignored.

See also
m_max_ignored_consec_lcs

Definition at line 379 of file CLevMarqGSO.h.

◆ m_curr_used_consec_lcs

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
size_t mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_curr_used_consec_lcs {0}
protected

Number of consecutive loop closures that are currently registered.

See also
m_max_used_consec_lcs

Definition at line 368 of file CLevMarqGSO.h.

◆ m_first_time_call

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
bool mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_first_time_call {false}
protected

Definition at line 325 of file CLevMarqGSO.h.

◆ m_graph

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
GRAPH_T* mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::m_graph = nullptr
protectedinherited

Pointer to the graph that is under construction.

Definition at line 142 of file CRegistrationDeciderOrOptimizer.h.

◆ m_graph_section

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
std::mutex* mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::m_graph_section = nullptr
protectedinherited

Definition at line 143 of file CRegistrationDeciderOrOptimizer.h.

◆ m_has_read_config

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
bool mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_has_read_config {false}
protected

Definition at line 326 of file CLevMarqGSO.h.

◆ m_initialized_visuals

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
bool mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::m_initialized_visuals = false
protectedinherited

Definition at line 157 of file CRegistrationDeciderOrOptimizer.h.

◆ m_just_fully_optimized_graph

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
bool mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_just_fully_optimized_graph {false}
protected

Indicates whether a full graph optimization was just issued.

Definition at line 385 of file CLevMarqGSO.h.

◆ m_last_total_num_of_nodes

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
size_t mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_last_total_num_of_nodes {5}
protected

Definition at line 332 of file CLevMarqGSO.h.

◆ m_max_ignored_consec_lcs

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
size_t mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_max_ignored_consec_lcs
protected

Number of consecutive loop closures to ignore after m_max_used_consec_lcs have already been issued.

See also
m_curr_ignored_consec_lcs, m_max_used_consec_lcs

Definition at line 374 of file CLevMarqGSO.h.

◆ m_max_used_consec_lcs

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
size_t mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_max_used_consec_lcs
protected

Number of maximum cosecutive loop closures that are allowed to be issued.

See also
m_curr_used_consec_lcs, m_max_ignored_consec_lcs

Definition at line 363 of file CLevMarqGSO.h.

◆ m_min_nodes_for_optimization

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
size_t mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_min_nodes_for_optimization {3}
protected

Minimum number of nodes before we try optimizing the graph.

Definition at line 388 of file CLevMarqGSO.h.

◆ m_min_verbosity_level

VerbosityLevel mrpt::system::COutputLogger::m_min_verbosity_level {LVL_INFO}
protectedinherited

Provided messages with VerbosityLevel smaller than this value shall be ignored.

Definition at line 253 of file system/COutputLogger.h.

Referenced by mrpt::system::COutputLogger::getMinLoggingLevel(), and mrpt::system::COutputLogger::isLoggingLevelVisible().

◆ m_optimization_policy

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
OptimizationPolicy mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_optimization_policy
protected

Definition at line 347 of file CLevMarqGSO.h.

◆ m_thread_optimize

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
std::thread mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::m_thread_optimize
protected

Definition at line 335 of file CLevMarqGSO.h.

◆ m_time_logger

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
mrpt::system::CTimeLogger mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::m_time_logger
protectedinherited

◆ m_win

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
mrpt::gui::CDisplayWindow3D* mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::m_win = nullptr
protectedinherited

Window to use.

Definition at line 153 of file CRegistrationDeciderOrOptimizer.h.

◆ m_win_manager

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
mrpt::graphslam::CWindowManager* mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::m_win_manager = nullptr
protectedinherited

Pointer to the CWindowManager object used to store visuals-related instances.

Definition at line 151 of file CRegistrationDeciderOrOptimizer.h.

◆ m_win_observer

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
mrpt::graphslam::CWindowObserver* mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::m_win_observer = nullptr
protectedinherited

CWindowObserver object for monitoring various visual-oriented events.

Definition at line 156 of file CRegistrationDeciderOrOptimizer.h.

◆ opt_params

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
OptimizationParams mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::opt_params

Parameters relevant to the optimizatio nfo the graph.

Definition at line 217 of file CLevMarqGSO.h.

◆ registered_new_node

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
bool mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::registered_new_node
protected

Definition at line 327 of file CLevMarqGSO.h.

◆ report_sep

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
const std::string mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::report_sep
staticprotectedinherited
Initial value:
=
std::string(2, '\n')

Definition at line 172 of file CRegistrationDeciderOrOptimizer.h.

Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().

◆ viz_params

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
GraphVisualizationParams mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::viz_params

Parameters relevant to the visualization of the graph.

Definition at line 219 of file CLevMarqGSO.h.




Page generated by Doxygen 1.8.14 for MRPT 2.0.0 Git: b38439d21 Tue Mar 31 19:58:06 2020 +0200 at miƩ abr 1 00:50:30 CEST 2020