SRBA: Sparser Relative Bundle Adjustment
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends
Classes | Public Member Functions | Static Public Member Functions | Public Attributes | Protected Attributes
srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS > Class Template Reference

Detailed Description

template<class KF2KF_POSE_TYPE, class LM_TYPE, class OBS_TYPE, class RBA_OPTIONS = RBA_OPTIONS_DEFAULT>
class srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >

The main class for the mrpt-srba: it defines a Relative Bundle-Adjustment (RBA) problem with (optionally, partially known) landmarks, the methods to update it with new observations and to optimize the relative poses with least squares optimizers.

The unknowns to be solved are:

The set of known data used to run the optimization comprises:

See http://mrpt.github.io/srba/ and `srba-guide` therein for a list of possible template arguments, code examples, etc.

Template Parameters:
KF2KF_POSE_TYPEThe parameterization of keyframe-to-keyframe relative poses (edges, problem unknowns).
LM_TYPEThe parameterization of relative positions of landmarks relative poses (edges).
OBS_TYPEThe type of observations.
RBA_OPTIONSA struct with nested typedefs which can be used to tune and customize the behavior of this class.

Definition at line 67 of file RbaEngine.h.

#include <RbaEngine.h>

List of all members.

Classes

struct  ExportGraphSLAM_Params
struct  TAllParameters
struct  TBFSEntryEdges
struct  TNewKeyFrameInfo
struct  TNumeric_dh_dAp_params
struct  TNumeric_dh_df_params
struct  TObsUsed
struct  TOpenGLRepresentationOptions
struct  TOptimizeExtraOutputInfo
struct  TOptimizeLocalAreaParams
struct  TSRBAParameters
struct  VisitorOptimizeLocalArea

Public Member Functions

 RbaEngine ()
void enable_time_profiler (bool enable=true)
const k2k_edges_deque_tget_k2k_edges () const
const TRelativeLandmarkPosMapget_known_feats () const
const TRelativeLandmarkPosMapget_unknown_feats () const
const rba_problem_state_tget_rba_state () const
rba_problem_state_tget_rba_state ()
mrpt::utils::CTimeLoggerget_time_profiler ()
void setVerbosityLevel (int level)
template<class SPARSEBLOCKHESSIAN >
size_t sparse_hessian_update_numeric (SPARSEBLOCKHESSIAN &H) const
void compute_jacobian_dh_dp (typename TSparseBlocksJacobians_dh_dAp::TEntry &jacob, const k2f_edge_t &observation, const k2k_edges_deque_t &k2k_edges, std::vector< const pose_flag_t * > *out_list_of_required_num_poses) const
void compute_jacobian_dh_df (typename TSparseBlocksJacobians_dh_df::TEntry &jacob, const k2f_edge_t &observation, std::vector< const pose_flag_t * > *out_list_of_required_num_poses) const
void gl_aux_draw_node (mrpt::opengl::CSetOfObjects &soo, const std::string &label, const float x, const float y) const
void compute_minus_gradient (Eigen::VectorXd &minus_grad, const std::vector< typename TSparseBlocksJacobians_dh_dAp::col_t * > &sparse_jacobs_Ap, const std::vector< typename TSparseBlocksJacobians_dh_df::col_t * > &sparse_jacobs_f, const vector_residuals_t &residuals, const std::map< size_t, size_t > &obs_global_idx2residual_idx) const
double reprojection_residuals (vector_residuals_t &residuals, const std::vector< TObsUsed > &observations) const
Main API methods
void define_new_keyframe (const typename traits_t::new_kf_observations_t &obs, TNewKeyFrameInfo &out_new_kf_info, const bool run_local_optimization=true)
void optimize_local_area (const TKeyFrameID root_id, const unsigned int win_size, TOptimizeExtraOutputInfo &out_info, const TOptimizeLocalAreaParams &params=TOptimizeLocalAreaParams(), const std::vector< size_t > &observation_indices_to_optimize=std::vector< size_t >())
void build_opengl_representation (const srba::TKeyFrameID root_keyframe, const TOpenGLRepresentationOptions &options, mrpt::opengl::CSetOfObjectsPtr out_scene, mrpt::opengl::CSetOfObjectsPtr out_root_tree=mrpt::opengl::CSetOfObjectsPtr()) const
bool save_graph_as_dot (const std::string &targetFileName, const bool all_landmarks=false) const
bool save_graph_top_structure_as_dot (const std::string &targetFileName, const bool set_node_coordinates) const
double eval_overall_squared_error () const
template<class POSE_GRAPH >
void get_global_graphslam_problem (POSE_GRAPH &global_graph, const ExportGraphSLAM_Params &params=ExportGraphSLAM_Params()) const
Extra API methods (for debugging, etc.)
void clear ()
TKeyFrameID alloc_keyframe ()
size_t create_kf2kf_edge (const TKeyFrameID new_kf_id, const TPairKeyFrameID &new_edge, const typename traits_t::new_kf_observations_t &obs, const pose_t &init_inv_pose_val=pose_t())
void create_complete_spanning_tree (const TKeyFrameID root_id, frameid2pose_map_t &span_tree, const size_t max_depth=std::numeric_limits< size_t >::max(), std::vector< bool > *aux_ws=NULL) const
bool find_path_bfs (const TKeyFrameID src_kf, const TKeyFrameID trg_kf, std::vector< TKeyFrameID > &found_path) const
const pose_tget_kf_relative_pose (const TKeyFrameID kf_query, const TKeyFrameID kf_reference) const
template<class KF_VISITOR , class FEAT_VISITOR , class K2K_EDGE_VISITOR , class K2F_EDGE_VISITOR >
void bfs_visitor (const TKeyFrameID root_id, const topo_dist_t max_distance, const bool rely_on_prebuilt_spanning_trees, KF_VISITOR &kf_visitor, FEAT_VISITOR &feat_visitor, K2K_EDGE_VISITOR &k2k_edge_visitor, K2F_EDGE_VISITOR &k2f_edge_visitor) const

Static Public Member Functions

template<class HESS_Ap , class HESS_f , class HESS_Apf , class JACOB_COLUMN_dh_dAp , class JACOB_COLUMN_dh_df >
static void sparse_hessian_build_symbolic (HESS_Ap &HAp, HESS_f &Hf, HESS_Apf &HApf, const std::vector< JACOB_COLUMN_dh_dAp * > &dh_dAp, const std::vector< JACOB_COLUMN_dh_df * > &dh_df)
static void numeric_dh_dAp (const array_pose_t &x, const TNumeric_dh_dAp_params &params, array_obs_t &y)
static void numeric_dh_df (const array_landmark_t &x, const TNumeric_dh_df_params &params, array_obs_t &y)
static void add_edge_ij_to_list_needed_roots (std::set< TKeyFrameID > &lst, const TKeyFrameID i, const TKeyFrameID j)
static double huber_kernel (double delta, const double kernel_param)

Public Attributes

const pose_t aux_null_pose
 A fixed SE(3) pose at the origin (used when we need a pointer or a reference to a "null transformation").
Public data fields
TAllParameters parameters
 Hierarchical struct with all parameters.
RBA_OPTIONS::edge_creation_policy_t edge_creation_policy
 The edge creation policy object.

Protected Member Functions

(Protected) Sub-algorithms
void determine_kf2kf_edges_to_create (const TKeyFrameID new_kf_id, const typename traits_t::new_kf_observations_t &obs, std::vector< TNewEdgeInfo > &new_k2k_edge_ids)
void optimize_edges (const std::vector< size_t > &run_k2k_edges, const std::vector< size_t > &run_feat_ids_in, TOptimizeExtraOutputInfo &out_info, const std::vector< size_t > &observation_indices_to_optimize=std::vector< size_t >())

Protected Attributes

int m_verbose_level
 0: None (only critical msgs), 1: verbose (default value), 2:even more verbose, 3: even more

Templatized typedef's

typedef RbaEngine
< KF2KF_POSE_TYPE, LM_TYPE,
OBS_TYPE, RBA_OPTIONS
rba_engine_t
typedef KF2KF_POSE_TYPE kf2kf_pose_t
typedef LM_TYPE landmark_t
typedef OBS_TYPE obs_t
typedef RBA_OPTIONS rba_options_t
typedef kf2kf_pose_t::se_traits_t se_traits_t
 The SE(2) or SE(3) traits struct (for Lie algebra log/exp maps, etc.)
typedef
rba_joint_parameterization_traits_t
< kf2kf_pose_t, landmark_t,
obs_t > 
traits_t
typedef jacobian_traits
< kf2kf_pose_t, landmark_t,
obs_t > 
jacobian_traits_t
typedef hessian_traits
< kf2kf_pose_t, landmark_t,
obs_t > 
hessian_traits_t
typedef kf2kf_pose_traits
< kf2kf_pose_t > 
kf2kf_pose_traits_t
typedef landmark_traits
< landmark_t
landmark_traits_t
typedef observation_traits< obs_t > observation_traits_t
typedef sensor_model
< landmark_t, obs_t > 
sensor_model_t
 The sensor model for the specified combination of LM parameterization + observation type.
typedef kf2kf_pose_t::pose_t pose_t
 The type of relative poses (e.g. mrpt::poses::CPose3D)
typedef TRBA_Problem_state
< KF2KF_POSE_TYPE, LM_TYPE,
OBS_TYPE, RBA_OPTIONS
rba_problem_state_t
typedef
rba_problem_state_t::k2f_edge_t 
k2f_edge_t
typedef
rba_problem_state_t::k2k_edge_t 
k2k_edge_t
typedef
rba_problem_state_t::k2k_edges_deque_t 
k2k_edges_deque_t
 A list (deque) of KF-to-KF edges (unknown relative poses).
typedef
kf2kf_pose_traits_t::pose_flag_t 
pose_flag_t
typedef
kf2kf_pose_traits_t::frameid2pose_map_t 
frameid2pose_map_t
typedef
kf2kf_pose_traits_t::TRelativePosesForEachTarget 
TRelativePosesForEachTarget
typedef
landmark_traits_t::TRelativeLandmarkPosMap 
TRelativeLandmarkPosMap
 An index of feature IDs and their relative locations.
typedef
landmark_traits_t::TRelativeLandmarkPos 
TRelativeLandmarkPos
 One landmark position (relative to its base KF)
typedef traits_t::keyframe_info keyframe_info
typedef
traits_t::new_kf_observation_t 
new_kf_observation_t
typedef
traits_t::new_kf_observations_t 
new_kf_observations_t
typedef
kf2kf_pose_traits_t::array_pose_t 
array_pose_t
typedef
landmark_traits_t::array_landmark_t 
array_landmark_t
typedef
observation_traits_t::array_obs_t 
array_obs_t
typedef
observation_traits_t::residual_t 
residual_t
typedef
observation_traits_t::vector_residuals_t 
vector_residuals_t
typedef jacobian_traits
< kf2kf_pose_t, landmark_t,
obs_t >
::TSparseBlocksJacobians_dh_dAp 
TSparseBlocksJacobians_dh_dAp
typedef jacobian_traits
< kf2kf_pose_t, landmark_t,
obs_t >
::TSparseBlocksJacobians_dh_df 
TSparseBlocksJacobians_dh_df
static const size_t REL_POSE_DIMS = kf2kf_pose_t::REL_POSE_DIMS
static const size_t LM_DIMS = landmark_t::LM_DIMS
static const size_t OBS_DIMS = obs_t::OBS_DIMS

Constructor & Destructor Documentation

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::RbaEngine ( )

Default constructor

Definition at line 19 of file rba_problem_common.h.


Member Function Documentation

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
TKeyFrameID srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::alloc_keyframe ( )

Users normally won't need to call this directly. Use define_new_keyframe() instead. This method appends an empty new keyframe to the data structures

Returns:
The ID of the new KF.
Note:
Runs in O(1)

Append an empty new keyframe to the data structures

Returns:
The ID of the new KF.
Note:
Runs in O(1)

Definition at line 19 of file alloc_keyframe.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
template<class KF_VISITOR , class FEAT_VISITOR , class K2K_EDGE_VISITOR , class K2F_EDGE_VISITOR >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::bfs_visitor ( const TKeyFrameID  root_id,
const topo_dist_t  max_distance,
const bool  rely_on_prebuilt_spanning_trees,
KF_VISITOR &  kf_visitor,
FEAT_VISITOR &  feat_visitor,
K2K_EDGE_VISITOR &  k2k_edge_visitor,
K2F_EDGE_VISITOR &  k2f_edge_visitor 
) const

Visits all k2k & k2f edges following a BFS starting at a given starting node and up to a given maximum depth. Only k2k edges are considered for BFS paths.

Definition at line 21 of file bfs_visitor.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::build_opengl_representation ( const srba::TKeyFrameID  root_keyframe,
const TOpenGLRepresentationOptions options,
mrpt::opengl::CSetOfObjectsPtr  out_scene,
mrpt::opengl::CSetOfObjectsPtr  out_root_tree = mrpt::opengl::CSetOfObjectsPtr() 
) const

Build an opengl representation of the current state of this RBA problem One of different representations can be generated: those opengl objects passed as NULL smart pointers will not be generated.

Parameters:
[out]out_sceneIf not a NULL smart pointer, at return will hold a 3D view of the current KF, neighbor KFs and landmarks.
[out]out_root_treeIf not a NULL smart pointer, at return will hold a schematic 2D view of the current KF and its spanning tree.

Definition at line 24 of file export_opengl.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::clear ( void  )

Reset the entire problem to an empty state (automatically called at construction)

Definition at line 29 of file rba_problem_common.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::compute_jacobian_dh_df ( typename TSparseBlocksJacobians_dh_df::TEntry &  jacob,
const k2f_edge_t observation,
std::vector< const pose_flag_t * > *  out_list_of_required_num_poses 
) const
		 *                       j,i                    lm_id,base_id
		 *           \partial  h            \partial  h
		 *                       l                      obs_frame_id
		 * dh_df = ------------------ = ---------------------------------
		 *
		 *            \partial  f            \partial  f
		 * 

Note: f=relative position of landmark with respect to its base kf

Definition at line 888 of file jacobians.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::compute_jacobian_dh_dp ( typename TSparseBlocksJacobians_dh_dAp::TEntry &  jacob,
const k2f_edge_t observation,
const k2k_edges_deque_t k2k_edges,
std::vector< const pose_flag_t * > *  out_list_of_required_num_poses 
) const
		  *                      j,i                    lm_id,base_id
		  *          \partial  h            \partial  h
		  *                      l                      obs_frame_id
		  * dh_dp = ------------------ = ---------------------------------
		  *                      d+1                    cur_id
		  *          \partial  p            \partial  p
		  *                      d                      stp.next_node
		  * 

See tech report: "A tutorial on SE(3) transformation parameterizations and on-manifold optimization", Jose-Luis Blanco, 2010

Definition at line 207 of file jacobians.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::create_complete_spanning_tree ( const TKeyFrameID  root_id,
frameid2pose_map_t &  span_tree,
const size_t  max_depth = std::numeric_limits<size_t>::max(),
std::vector< bool > *  aux_ws = NULL 
) const

Creates a numeric spanning tree between a given root keyframe and the entire graph, returning it into a user-supplied data structure Note that this method does NOT use the depth-limited spanning trees which are built incrementally with the graph. So, it takes an extra cost to call this method. For the other trees, see get_rba_state()

Parameters:
[in]root_idThe root keyframe
[out]span_treeThe output with all found relative poses. Its previous contents are cleared.
[in]max_depthDefaults to std::numeric_limits<size_t>::max() for infinity depth, can be set to a maximum desired depth from the root.
[in]aux_wsAuxiliary working space: Set to an uninitialized vector<bool> (it'll automatically initialized) if you want to call this method simultaneously from several threads. Otherwise, setting to NULL will automatically use one working space, reusable between succesive calls.
See also:
find_path_bfs

Definition at line 18 of file spantree_create_complete.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
size_t srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::create_kf2kf_edge ( const TKeyFrameID  new_kf_id,
const TPairKeyFrameID &  new_edge,
const typename traits_t::new_kf_observations_t obs,
const pose_t init_inv_pose_val = pose_t() 
)

Users normally won't need to call this directly. Use define_new_keyframe() instead. This method calls: 1) alloc_kf2kf_edge() for creating the data structures 2) spanning_tree.update_symbolic_new_node() for updating the spanning trees.

Note:
obs is passed just for fixing missing spanning trees when using the policy of pure linear graph.

Definition at line 15 of file create_kf2kf_edge.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::define_new_keyframe ( const typename traits_t::new_kf_observations_t obs,
TNewKeyFrameInfo out_new_kf_info,
const bool  run_local_optimization = true 
)

The most common entry point for SRBA: append a new keyframe (KF) to the map, automatically creates the required edges and optimize the local area around this new KF.

Parameters:
[in]obsAll the landmark observations gathered from this new KF (with data association already solved).
[out]out_new_kf_infoReturned information about the newly created KF.
[in]run_local_optimizationIf set to true (default), the local map around the new KF will be optimized (i.e. optimize_local_area() will be called automatically).

Definition at line 16 of file define_new_keyframe.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::determine_kf2kf_edges_to_create ( const TKeyFrameID  new_kf_id,
const typename traits_t::new_kf_observations_t obs,
std::vector< TNewEdgeInfo > &  new_k2k_edge_ids 
) [protected]

This method will call edge_creation_policy(), which has predefined algorithms but could be re-defined by the user in a derived class

Determines and creates the new kf2fk edges given the set of new observations:

Definition at line 17 of file determine_kf2kf_edges_to_create.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS = RBA_OPTIONS_DEFAULT>
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::enable_time_profiler ( bool  enable = true) [inline]

Enable or disables time profiling of all operations (default=enabled), which will be reported upon destruction

Definition at line 481 of file RbaEngine.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
double srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::eval_overall_squared_error ( ) const

Evaluates the quality of the overall map/landmark estimations, by computing the sum of the squared error contributions for all observations. For this, this method may have to compute *very long* shortest paths between distant keyframes if no loop-closure edges exist in order to evaluate the best approximation of relative coordinates between observing KFs and features' reference KFs.

The worst-case time consumed by this method is O(M*log(N) + N^2 + N E), N=# of KFs, E=# of edges, M=# of observations.

Definition at line 15 of file eval_overall_error.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS = RBA_OPTIONS_DEFAULT>
bool srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::find_path_bfs ( const TKeyFrameID  src_kf,
const TKeyFrameID  trg_kf,
std::vector< TKeyFrameID > &  found_path 
) const [inline]

An unconstrained breadth-first search (BFS) for the shortest path between two keyframes. Note that this method does NOT use the depth-limited spanning trees which are built incrementally with the graph. So, it takes the extra cost of really running a BFS algorithm. For the other precomputed trees, see get_rba_state() Edge direction is ignored during the search, i.e. as if we had an undirected graph of Keyframes. If both source and target KF coincide, an empty path is returned.

Returns:
true if a path was found.
Note:
Worst-case computational complexity is that of a BFS over the entire graph: O(V+E), V=number of nodes, E=number of edges.
See also:
create_complete_spanning_tree

Definition at line 375 of file RbaEngine.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
template<class POSE_GRAPH >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::get_global_graphslam_problem ( POSE_GRAPH &  global_graph,
const ExportGraphSLAM_Params params = ExportGraphSLAM_Params() 
) const

Build a graph-slam problem suitable for recovering the (consistent) global pose (vs. relative ones as are handled in SRBA) of each keyframe.

Note:
This version of the method doesn't account for the covariances of relative pose estimations in RBA.
See also:
mrpt::graphslam (for methods capable of optimizing the output graph of pose constraints)
Parameters:
[out]global_graphPrevious contents will be erased. The output global graph will be returned here, initialized with poses from a Dijkstra/Spanning-tree from the first KF.
Template Parameters:
POSE_GRAPHMust be an instance of mrpt::graphs::CNetworkOfPoses<>, e.g. CNetworkOfPoses2D (for 2D poses) or CNetworkOfPoses3D (for 3D).
Note:
This method is NOT O(1)

Exports all the keyframes and landmarks as a directed graph in DOT (graphviz) format

Definition at line 17 of file get_global_graphslam_problem.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS = RBA_OPTIONS_DEFAULT>
const pose_t* srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::get_kf_relative_pose ( const TKeyFrameID  kf_query,
const TKeyFrameID  kf_reference 
) const [inline]

Returns the up-to-date relative pose of Keyframe `kf_query` with respect to `kf_reference`, or NULL if the relative pose is not immediately available from any numeric spanning tree.

Definition at line 385 of file RbaEngine.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS = RBA_OPTIONS_DEFAULT>
mrpt::utils::CTimeLogger& srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::get_time_profiler ( ) [inline]

Access to the time profiler

Definition at line 492 of file RbaEngine.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS = RBA_OPTIONS_DEFAULT>
static double srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::huber_kernel ( double  delta,
const double  kernel_param 
) [inline, static]

pseudo-huber cost function

Definition at line 810 of file RbaEngine.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::numeric_dh_dAp ( const array_pose_t x,
const TNumeric_dh_dAp_params params,
array_obs_t y 
) [static]

Auxiliary method for numeric Jacobian: numerically evaluates the new observation "y" for a small increment "x" in a relative KF-to-KF pose

Numeric implementation of the partial Jacobian dh_dAp

Definition at line 90 of file jacobians.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::numeric_dh_df ( const array_landmark_t x,
const TNumeric_dh_df_params params,
array_obs_t y 
) [static]

Auxiliary method for numeric Jacobian: numerically evaluates the new observation "y" for a small increment "x" in a landmark position

Definition at line 170 of file jacobians.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::optimize_edges ( const std::vector< size_t > &  run_k2k_edges,
const std::vector< size_t > &  run_feat_ids_in,
TOptimizeExtraOutputInfo out_info,
const std::vector< size_t > &  observation_indices_to_optimize = std::vector<size_t>() 
) [protected]
Parameters:
observation_indices_to_optimizeIndices wrt rba_state.all_observations. An empty vector means use ALL the observations involving the selected unknowns.
See also:
optimize_local_area

Definition at line 44 of file optimize_edges.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::optimize_local_area ( const TKeyFrameID  root_id,
const unsigned int  win_size,
TOptimizeExtraOutputInfo out_info,
const TOptimizeLocalAreaParams params = TOptimizeLocalAreaParams(),
const std::vector< size_t > &  observation_indices_to_optimize = std::vector<size_t>() 
)

Runs least-squares optimization for all the unknowns within a given topological distance to a given KeyFrame.

Parameters:
[in]observation_indices_to_optimizeIndices wrt rba_state.all_observations. An empty vector means use ALL the observations involving the selected unknowns.
Note:
Extra options are available in parameters
See also:
define_new_keyframe, add_observation, optimize_edges

Definition at line 15 of file optimize_local_area.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
double srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::reprojection_residuals ( vector_residuals_t &  residuals,
const std::vector< TObsUsed > &  observations 
) const [inline]

reprojection_residuals

Definition at line 16 of file reprojection_residuals.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
bool srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::save_graph_as_dot ( const std::string &  targetFileName,
const bool  all_landmarks = false 
) const

Exports all the keyframes (and optionally all landmarks) as a directed graph in DOT (graphviz) format.

Returns:
false on any error writing to target file

Definition at line 17 of file export_dot.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
bool srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::save_graph_top_structure_as_dot ( const std::string &  targetFileName,
const bool  set_node_coordinates 
) const

Exports the "high-level" structure of the map as a directed graph in DOT (graphviz) format: like save_graph_as_dot but only exporting those KFs with more than one kf2kf edge. Optionally, using "global" coordinates (from a spanning tree) to make it more similar to the real-world structure.

Returns:
false on any error writing to target file

Definition at line 86 of file export_dot.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS = RBA_OPTIONS_DEFAULT>
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::setVerbosityLevel ( int  level) [inline]

Changes the verbosity level: 0=None (only critical msgs), 1=verbose, 2=so verbose you'll have to say "Stop!"

Definition at line 495 of file RbaEngine.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
template<class HESS_Ap , class HESS_f , class HESS_Apf , class JACOB_COLUMN_dh_dAp , class JACOB_COLUMN_dh_df >
void srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::sparse_hessian_build_symbolic ( HESS_Ap &  HAp,
HESS_f &  Hf,
HESS_Apf &  HApf,
const std::vector< JACOB_COLUMN_dh_dAp * > &  dh_dAp,
const std::vector< JACOB_COLUMN_dh_df * > &  dh_df 
) [static]

Rebuild the Hessian symbolic information from the given Jacobians.

Parameters:
[out]HOutput hessian (must be empty at input)

Rebuild the Hessian symbolic information from the given Jacobians Example of the expected template types:

  • HESS_Apf: MatrixBlockSparseCols<double,6,3,THessianSymbolicInfo<double,2,6,3>, false >
  • JACOB_COLUMN_dh_dAp: TSparseBlocksJacobians_dh_dAp::col_t = MatrixBlockSparseCols<double,2,6,TJacobianSymbolicInfo_dh_dAp, false>::col_t

Definition at line 22 of file sparse_hessian_build_symbolic.h.

template<class KF2KF_POSE_TYPE , class LM_TYPE , class OBS_TYPE , class RBA_OPTIONS >
template<class SPARSEBLOCKHESSIAN >
size_t srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::sparse_hessian_update_numeric ( SPARSEBLOCKHESSIAN &  H) const

Rebuild the Hessian symbolic information from the internal pointers to blocks of Jacobians. Only the upper triangle is filled-in (all what is needed for Cholesky) for square Hessians, in whole for rectangular ones (it depends on the symbolic decomposition, done elsewhere).

Template Parameters:
SPARSEBLOCKHESSIANcan be: TSparseBlocksHessian_6x6, TSparseBlocksHessian_3x3 or TSparseBlocksHessian_6x3
Returns:
The number of Jacobian multiplications skipped due to its observation being marked as "invalid"

Definition at line 22 of file sparse_hessian_update_numeric.h.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends