Template Class CLevMarqGSO

Defined in File CLevMarqGSO.h

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

public mrpt::graphslam::optimizers::CGraphSlamOptimizer< typename mrpt::graphs::CNetworkOfPoses2DInf > (Template Class CGraphSlamOptimizer)

Class Documentation

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

Levenberg-Marquardt non-linear graph slam optimization scheme.

Unnamed Group

using constraint_t = typename GRAPH_T::constraint_t: Handy typedefs.

using pose_t = typename GRAPH_T::constraint_t::type_value: type of underlying poses (2D/3D)

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>

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.

size_t m_curr_used_consec_lcs = {0}

Number of consecutive loop closures that are currently registered.

See also

m_max_used_consec_lcs

size_t m_max_ignored_consec_lcs

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

size_t m_curr_ignored_consec_lcs = {0}

Consecutive Loop Closures that have currently been ignored.

See also

m_max_ignored_consec_lcs

Unnamed Group

void initOptDistanceVisualization(): Initialize the Disk/Sphere used for visualizing the optimization distance.

mrpt::viz::CVisualObject::Ptr initOptDistanceVisualizationInternal(const mrpt::poses::CPose2D &p_unused)

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

mrpt::viz::CVisualObject::Ptr initOptDistanceVisualizationInternal(const mrpt::poses::CPose3D &p_unused)

Public Functions

CLevMarqGSO()

~CLevMarqGSO() override = default

virtual 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.

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

virtual void initializeVisuals() override

Initialize visual objects in CDisplayWindow (e.g. add an object to scene).

See also

setWindowManagerPtr, updateVisuals

Throws:: std::exception – If the method is called without having first provided a CDisplayWindow3D* to the class instance

virtual void updateVisuals() override

Update the relevant visual features in CDisplayWindow.

See also

setWindowManagerPtr, initializeVisuals

Throws:: std::exception – If the method is called without having first provided a CDisplayWindow3D* to the class instance

virtual void notifyOfWindowEvents(const std::map<std::string, bool> &events_occurred) override: Get a list of the window events that happened since the last call.

virtual void loadParams(const std::string &source_fname) override: Load the necessary for the decider/optimizer configuration parameters.

virtual void printParams() const override: Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact way.

virtual void getDescriptiveReport(std::string *report_str) const override

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

virtual bool justFullyOptimizedGraph() const override: Used by the caller to query for possible full graph optimization on the latest optimizer run.

Public Members

OptimizationParams opt_params: Parameters relevant to the optimizatio nfo the graph.

GraphVisualizationParams viz_params: Parameters relevant to the visualization of the graph.

Protected Types

enum class OptimizationPolicy

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

Values:

enumerator IgnoreLoopClosures

enumerator UseLoopClosures

enumerator Count

Protected Functions

void _optimizeGraph(bool is_full_update = false)

Optimize the given graph.

Wrapper around the graphslam::optimize_spa_levmarq method

See also

optimize_spa_levmarq, optimizeGraph

Parameters:: full_update – [in] Impose that method optimizes the whole graph

virtual void optimizeGraph() override

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

Used in multithreaded optimization

See also

_optimizeGraph()

bool checkForLoopClosures()

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

bool checkForFullOptimization()

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

void initGraphVisualization(): Initialize objects relateed to the Graph Visualization.

void updateGraphVisualization(): Called internally for updating the visualization scene for the graph building procedure.

void toggleGraphVisualization(): Toggle the graph visualization on and off.

void fitGraphInView(): 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.

void updateOptDistanceVisualization(): Update the position of the disk indicating the distance in which Levenberg-Marquardt graph optimization is executed.

void toggleOptDistanceVisualization(): toggle the optimization distance object on and off

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.

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.

size_t m_min_nodes_for_optimization = {3}: Minimum number of nodes before we try optimizing the graph.

Protected Static 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.

struct GraphVisualizationParams : public mrpt::config::CLoadableOptions 

struct for holding the graph visualization-related variables in a compact form

Public Functions

GraphVisualizationParams()

~GraphVisualizationParams() override

virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override

virtual void dumpToTextStream(std::ostream &out) const override

Public Members

mrpt::containers::yaml cfg

bool visualize_optimized_graph

std::string keystroke_graph_toggle

std::string keystroke_graph_autofit

int text_index_graph

double offset_y_graph

struct OptimizationParams : public mrpt::config::CLoadableOptions 

Struct for holding the optimization-related variables in a compact form.

Public Functions

OptimizationParams()

~OptimizationParams() override

virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override

virtual void dumpToTextStream(std::ostream &out) const override

Public Members

mrpt::containers::yaml cfg

bool optimization_on_second_thread

double optimization_distance: optimize only for the nodes found in a certain distance from the current position. Optimize for the entire graph if set to1

double offset_y_optimization_distance

int text_index_optimization_distance

mrpt::img::TColor optimization_distance_color

std::string keystroke_optimization_distance: Keystroke to toggle the optimization distance on/off.

std::string keystroke_optimize_graph: Keystroke to manually trigger a full graph optimization.

int LC_min_nodeid_diff

GRAPH_T::edges_map_t last_pair_nodes_to_edge