#include <factor_graph.hpp>

Inheritance diagram for mrob::FGraph:

Public Member Functions
factor_id_t	add_factor (std::shared_ptr< Factor > &factor)

factor_id_t	add_eigen_factor (std::shared_ptr< EigenFactor > &factor)

factor_id_t	add_node (std::shared_ptr< Node > &node)

std::shared_ptr< Node > &	get_node (factor_id_t key)

std::shared_ptr< Factor > &	get_factor (factor_id_t key)

std::shared_ptr< EigenFactor > &	get_eigen_factor (factor_id_t key)

void	print (bool complete=false) const

factor_id_t	number_nodes ()

factor_id_t	number_factors ()

uint_t	get_dimension_state ()

uint_t	get_dimension_obs ()

void	save_graph () const

void	load_graph ()

Protected Attributes
std::deque< std::shared_ptr< Node > >	nodes_

std::deque< std::shared_ptr< Node > >	active_nodes_

std::deque< std::shared_ptr< Factor > >	factors_

std::deque< std::shared_ptr< EigenFactor > >	eigen_factors_

uint_t	stateDim_

uint_t	obsDim_

Detailed Description

This class provides the general structure for encoding Factor Graphs and to support the implementation of the inference solution to the joint probability P(x,u,z). The solution to this joint probability is equivalent to a Nonlinear Least Squares (NLSQ) problem.

Factor Graphs are bipartite graphs, meaning that we express the relations from a set of vertices "nodes" which include our state variables through a set of vertices "factors", capturing the inherent distribution of the nodes variables due to observations. Bipartite is in the sense that edges of the graph are always from nodes to factors or vice versa.

We require two abstract classes,

Class Node
Class Factor. here see factor.hpp for the conventions on residuals, observations, etc.

XXX, actually key as addresses won't work in python interface. Better use id_t (uint) Both data containers are stored in vectors (XXX prev unordered sets) whose keys are their addresses. By doing this, we can iterate and quickly find elements in both data containers.

Each problem instantaition should implement methods for solving the graph and storing the necessary data, such as information matrix, factorizations, etc.

Member Function Documentation

◆ add_eigen_factor()

factor_id_t FGraph::add_eigen_factor ( std::shared_ptr< EigenFactor > & factor )

Adds an Eigen Factor, the special factor that is not formulated as a sum of residuals, but directly as a real value (eigenvalue) and therefore it requires a different processing, apart from the standard residual factors from above.

◆ add_factor()

factor_id_t FGraph::add_factor ( std::shared_ptr< Factor > & factor )

Adds a factor, if it is not already on the set. Note that the connecting nodes of the factor should be already specified when creating the factor.

This function includes the factor into its connected nodes.

Modifications of the structure of the graph are allowed by removing the factor and adding the new updated one.

returns factor id

◆ add_node()

factor_id_t FGraph::add_node ( std::shared_ptr< Node > & node )

Adds a node if it was not already on the set.

◆ get_eigen_factor()

std::shared_ptr< EigenFactor > & FGraph::get_eigen_factor ( factor_id_t key )

get_node returns the Eigen factor given the node id key, now a position on the data structure

◆ get_factor()

std::shared_ptr< Factor > & FGraph::get_factor ( factor_id_t key )

get_node returns the node given the node id key, now a position on the data structure

◆ get_node()

std::shared_ptr< Node > & FGraph::get_node ( factor_id_t key )

get_node returns the node given the node id key, now a position on the data structure

◆ number_nodes()

factor_id_t mrob::FGraph::number_nodes ( )

inline

FGraph information

Member Data Documentation

◆ nodes_

std::deque<std::shared_ptr<Node> > mrob::FGraph::nodes_

protected

XXX is set better than vector(deque) for what we are using them? Vector is much faster for direct access [], but needs allocation. We are also interested on having indices on nodes and factors. Set iterates ok O(1) and can remove elements nicely O(1).

For now we will use deque, but we will maintain abstraction in case we need to change since it has fast access and does no require memory allocation

◆ stateDim_

uint_t mrob::FGraph::stateDim_