iSAM
isam::Slam Class Reference

#include <Slam.h>

Inheritance diagram for isam::Slam:
Collaboration diagram for isam::Slam:

List of all members.

Public Member Functions

 Slam ()
virtual ~Slam ()
Properties properties ()
void set_properties (Properties prop)
void save (const std::string fname) const
void add_node (Node *node)
void add_factor (Factor *factor)
void remove_node (Node *node)
void remove_factor (Factor *factor)
virtual UpdateStats update ()
virtual int batch_optimization ()
void set_cost_function (cost_func_t cost_func)
double normalized_chi2 ()
double local_chi2 (int last_n)
Eigen::VectorXd weighted_errors (Selector s=ESTIMATE)
double chi2 (Selector s=ESTIMATE)
virtual const SparseSystemget_R () const
virtual SparseSystem jacobian_numerical_columnwise ()
virtual SparseSystem jacobian_partial (int last_n)
virtual SparseSystem jacobian ()
const Covariancescovariances ()
virtual void print_stats ()

Protected Attributes

int _dim_nodes
int _dim_measure
int _num_new_measurements
int _num_new_rows
Optimizer _opt

Friends

class Covariances

Detailed Description

The actual SLAM interface.

Definition at line 66 of file Slam.h.


Constructor & Destructor Documentation

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:
factorPointer 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:
nodePointer to new node.

Reimplemented from isam::Graph.

Definition at line 91 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.

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.

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.

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

Returns the current factor matrix.

Definition at line 291 of file Slam.cpp.

Returns the measurement Jacobian of the SLAM system.

Returns:
Measurement Jacobian.

Implements isam::OptimizationInterface.

Definition at line 379 of file Slam.cpp.

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_nOnly return Jacobians of last n measurements (-1 returns all)
Returns:
Measurement Jacobian.

Definition at line 395 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.

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.

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:
factorPointer 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:
nodePointer 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:
fnameFilename with optional path to save graph to.

Definition at line 84 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_funcPointer 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.

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.

Returns:
Update statistics.

Definition at line 157 of file Slam.cpp.

VectorXd isam::Slam::weighted_errors ( Selector  s = ESTIMATE) [virtual]

Weighted non-squared error vector, by default at current estimate.

Implements isam::OptimizationInterface.

Definition at line 254 of file Slam.cpp.


The documentation for this class was generated from the following files:
 All Classes Files Functions Variables