#include <Slam.h>

Inheritance diagram for isam::Slam:

Public Member Functions
void	add_factor (Factor *factor)
void	add_node (Node *node)
virtual int	batch_optimization ()
double	chi2 (Selector s=ESTIMATE)
const Covariances &	covariances ()
virtual const SparseSystem &	get_R () const
virtual SparseSystem	jacobian ()
virtual SparseSystem	jacobian_numerical_columnwise ()
virtual SparseSystem	jacobian_partial (int last_n)
double	local_chi2 (int last_n)
double	normalized_chi2 ()
virtual void	print_stats ()
Properties	properties ()
void	remove_factor (Factor *factor)
void	remove_node (Node *node)
void	save (const std::string fname) const
void	set_cost_function (cost_func_t cost_func)
void	set_properties (Properties prop)
	Slam ()
virtual UpdateStats	update ()
Eigen::VectorXd	weighted_errors (Selector s=ESTIMATE)
virtual	~Slam ()
Protected Attributes
int	_dim_measure
int	_dim_nodes
int	_num_new_measurements
int	_num_new_rows
Optimizer	_opt
Private Member Functions
void	apply_exmap (const Eigen::VectorXd &x)
virtual void	batch_optimization_step ()
void	estimate_to_linpoint ()
virtual void	incremental_update ()
void	linpoint_to_estimate ()
void	self_exmap (const Eigen::VectorXd &x)
void	swap_estimates ()
void	update_starts ()
Private Attributes
cost_func_t	_cost_func
Covariances	_covariances
Properties	_prop
bool	_require_batch
int	_step
Friends
class	Covariances

Detailed Description

The actual SLAM interface.

Definition at line 66 of file Slam.h.

Constructor & Destructor Documentation

isam::Slam::Slam ( )

Default constructor.

Definition at line 69 of file Slam.cpp.

isam::Slam::~Slam ( ) [virtual]

Destructor.

Definition at line 80 of file Slam.cpp.

Member Function Documentation

void isam::Slam::add_factor ( Factor * factor ) [virtual]

Adds a factor (measurement) to the graph.

Parameters:

factor Pointer to new factor.

Reimplemented from isam::Graph.

Definition at line 96 of file Slam.cpp.

void isam::Slam::add_node ( Node * node ) [virtual]

Adds a node (variable) to the graph.

Parameters:

node	Pointer to new node.

Reimplemented from isam::Graph.

Definition at line 91 of file Slam.cpp.

void isam::Slam::apply_exmap ( const Eigen::VectorXd & x ) [private, virtual]

Apply a delta vector to the linearization point and store result as new estimate.

Implements isam::OptimizationInterface.

Definition at line 216 of file Slam.cpp.

int isam::Slam::batch_optimization ( ) [virtual]

Fully solve the system, iterating until convergence.

Returns:: Number of iterations performed.

Definition at line 198 of file Slam.cpp.

void isam::Slam::batch_optimization_step ( ) [private, virtual]

Resolve the system with linearization based on current estimate; perform variable reordering for efficiency.

Definition at line 147 of file Slam.cpp.

double isam::Slam::chi2 ( Selector s = ESTIMATE )

Weighted sum of squared errors, by default at the current estimate.

Definition at line 266 of file Slam.cpp.

const Covariances & isam::Slam::covariances ( )

Returns the Covariances object associated with this Slam object.

Returns:: Covariances object for access to estimation covariances.

Definition at line 391 of file Slam.cpp.

void isam::Slam::estimate_to_linpoint ( ) [private, virtual]

Set linearization point to current estimate.

Implements isam::OptimizationInterface.

Definition at line 242 of file Slam.cpp.

const SparseSystem & isam::Slam::get_R ( ) const [virtual]

Returns the current factor matrix.

Definition at line 291 of file Slam.cpp.

void isam::Slam::incremental_update ( ) [private, virtual]

Update the system with any newly added measurements. The measurements will be appended to the existing factor matrix, and the factor is transformed into triangular form again using Givens rotations. Very efficient for exploration O(1), but can be more expensive otherwise >O(n).

Definition at line 128 of file Slam.cpp.

SparseSystem isam::Slam::jacobian ( ) [virtual]

Returns the measurement Jacobian of the SLAM system.

Returns:: Measurement Jacobian.

Implements isam::OptimizationInterface.

Definition at line 379 of file Slam.cpp.

SparseSystem isam::Slam::jacobian_numerical_columnwise ( ) [virtual]

Calculate the full Jacobian numerical (fast column-wise procedure).

Definition at line 297 of file Slam.cpp.

SparseSystem isam::Slam::jacobian_partial ( int last_n ) [virtual]

Returns the last n rows of the measurement Jacobian of the SLAM system.

Parameters:

last_n Only return Jacobians of last n measurements (-1 returns all)

Returns:: Measurement Jacobian.

Definition at line 395 of file Slam.cpp.

void isam::Slam::linpoint_to_estimate ( ) [private, virtual]

Set current estimate to the linearization point (needed in dog leg).

Implements isam::OptimizationInterface.

Definition at line 236 of file Slam.cpp.

double isam::Slam::local_chi2 ( int last_n )

Calculates the chi2 error of the last_n constraints.

Definition at line 270 of file Slam.cpp.

double isam::Slam::normalized_chi2 ( )

Calculates the normalized chi-square value (weighted sum of squared errors divided by degrees of freedom [# measurements - # variables]) for the estimate x.

Definition at line 287 of file Slam.cpp.

void isam::Slam::print_stats ( ) [virtual]

Print statistics for debugging.

Definition at line 434 of file Slam.cpp.

Properties isam::Slam::properties ( ) [inline]

Returns a copy of the current properties.

Definition at line 92 of file Slam.h.

void isam::Slam::remove_factor ( Factor * factor ) [virtual]

Removes an factor (measurement) from the graph. Note that the factor itself is not deallocated. Be careful not to leave unconnected nodes behind.

Parameters:

factor Pointer to factor.

Reimplemented from isam::Graph.

Definition at line 118 of file Slam.cpp.

void isam::Slam::remove_node ( Node * node ) [virtual]

Removes a node (variable) and all adjacent factors from the graph. Note that the node itself is not deallocated.

Parameters:

node	Pointer to node.

Reimplemented from isam::Graph.

Definition at line 107 of file Slam.cpp.

void isam::Slam::save ( const std::string fname ) const

Saves the graph (nodes and factors).

Parameters:

fname Filename with optional path to save graph to.

Definition at line 84 of file Slam.cpp.

void isam::Slam::self_exmap ( const Eigen::VectorXd & x ) [private, virtual]

Apply a delta vector directly to the linearization point.

Implements isam::OptimizationInterface.

Definition at line 226 of file Slam.cpp.

void isam::Slam::set_cost_function ( cost_func_t cost_func )

Sets a cost function different from the default (quadratic).

Parameters:

cost_func Pointer to cost function, see util.h for a list of robust cost functions. Instead of cost_squared, use NULL, which avoids calculating square roots.

Definition at line 212 of file Slam.cpp.

void isam::Slam::set_properties ( Properties prop ) [inline]

Sets new properties.

Definition at line 99 of file Slam.h.

void isam::Slam::swap_estimates ( ) [private, virtual]

Exchange linearization point and current estimate (needed in dog leg).

Implements isam::OptimizationInterface.

Definition at line 248 of file Slam.cpp.

UpdateStats isam::Slam::update ( ) [virtual]

Update the graph by finding new solution; depending on properties this might simply be a Givens update, could include a solve step, or be a full batch step with reordering.