131 template <
class GRAPH_T =
typename mrpt::graphs::CNetworkOfPoses2DInf>
141 typedef typename GRAPH_T::constraint_t::type_value
144 constraint_t::state_length>
164 const std::map<std::string, bool>& events_occurred);
247 const GRAPH_T& graph,
const size_t iter,
const size_t max_iter,
248 const double cur_sq_error);
340 std::set<mrpt::utils::TNodeID>* nodes_set,
void initGraphVisualization()
Initialize objects relateed to the Graph Visualization.
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.
std::string keystroke_graph_autofit
bool checkForFullOptimization()
Decide whether to issue a full graph optimization.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion)
This method load the options from a ".ini"-like file or memory-stored string list.
GraphVisualizationParams()
mrpt::opengl::CRenderizable::Ptr initOptDistanceVisualizationInternal(const mrpt::poses::CPose2D &p_unused)
Setup the corresponding Disk/Sphere instance.
void updateGraphVisualization()
Called internally for updating the visualization scene for the graph building procedure.
void toggleGraphVisualization()
Toggle the graph visualization on and off.
Struct for holding the optimization-related variables in a compact form.
FullOptimizationPolicy
Enumeration that defines the behaviors towards using or ignoring a newly added loop closure to fully ...
void optimizeGraph()
Wrapper around _optimizeGraph which first locks the section and then calls the _optimizeGraph method...
bool m_just_fully_optimized_graph
Indicates whether a full graph optimization was just issued.
std::shared_ptr< CRenderizable > Ptr
mrpt::utils::TParametersDouble cfg
bool justFullyOptimizedGraph() const
Used by the caller to query for possible full graph optimization on the latest optimizer run...
mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T > grandpa
std::string keystroke_optimize_graph
Keystroke to manually trigger a full graph optimization.
~GraphVisualizationParams()
mrpt::utils::TColor optimization_distance_color
double optimization_distance
optimize only for the nodes found in a certain distance from the current position.
void getNearbyNodesOf(std::set< mrpt::utils::TNodeID > *nodes_set, const mrpt::utils::TNodeID &cur_nodeID, double distance)
Get a list of the nodeIDs whose position is within a certain distance to the specified nodeID...
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
void toggleOptDistanceVisualization()
toggle the optimization distance object on and off
This class allows loading and storing values and vectors of different types from a configuration text...
OptimizationParams opt_params
Parameters relevant to the optimizatio nfo the graph.
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
void initOptDistanceVisualization()
Initialize the Disk/Sphere used for visualizing the optimization distance.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A numeric matrix of compile-time fixed size.
std::thread m_thread_optimize
GRAPH_T::edges_map_t last_pair_nodes_to_edge
uint64_t TNodeID
The type for node IDs in graphs of different types.
mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > parent
void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
Interface for implementing node/edge registration deciders or optimizer classes.
GRAPH_T::constraint_t constraint_t
Handy typedefs.
FullOptimizationPolicy m_optimization_policy
Should I fully optimize the graph on loop closure?
size_t m_min_nodes_for_optimization
Minimum number of nodes before we try optimizing the graph.
bool optimization_on_second_thread
int text_index_optimization_distance
std::shared_ptr< CSensoryFrame > Ptr
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
size_t m_max_ignored_consec_lcs
Number of consecutive loop closures to ignore after m_max_used_consec_lcs have already been issued...
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
size_t m_last_total_num_of_nodes
std::shared_ptr< CObservation > Ptr
mrpt::math::CMatrixFixedNumeric< double, constraint_t::state_length, constraint_t::state_length > InfMat
mrpt::utils::TParametersDouble cfg
GLsizei const GLchar ** string
bool visualize_optimized_graph
GRAPH_T::constraint_t::type_value pose_t
bool checkForLoopClosures()
Check if a loop closure edge was added in the graph.
size_t m_curr_ignored_consec_lcs
Consecutive Loop Closures that have currently been ignored.
std::string keystroke_optimization_distance
Keystroke to toggle the optimization distance on/off.
std::string keystroke_graph_toggle
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Interface for implementing graphSLAM optimizer classes.
std::shared_ptr< CActionCollection > Ptr
void updateOptDistanceVisualization()
Update the position of the disk indicating the distance in which Levenberg-Marquardt graph optimizati...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion)
This method load the options from a ".ini"-like file or memory-stored string list.
size_t m_max_used_consec_lcs
Number of maximum cosecutive loop closures that are allowed to be issued.
GLsizei GLsizei GLchar * source
GraphVisualizationParams viz_params
Parameters relevant to the visualization of the graph.
double offset_y_optimization_distance
size_t m_curr_used_consec_lcs
Number of consecutive loop closures that are currently registered.
Levenberg-Marquardt non-linear graph slam optimization scheme.
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
Generic method for fetching the incremental action/observation readings from the calling function...
void fitGraphInView()
Set the camera parameters of the CDisplayWindow3D so that the whole graph is viewed in the window...
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
struct for holding the graph visualization-related variables in a compact form
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void updateVisuals()
Update the relevant visual features in CDisplayWindow.
void _optimizeGraph(bool is_full_update=false)
Optimize the given graph.