Public Types | Public Member Functions | Public Attributes | Static Public Attributes | Private Member Functions | Private Attributes
ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension > Class Template Reference

#include <slam_ekf_base.hpp>

List of all members.

Public Types

typedef linear_algebra::Matrix
< double, feature_dimension, 1 > 		Feature
 The feature vector.
typedef linear_algebra::Matrix
< double, feature_dimension,
pose_dimension > 		FeatureCoVarianceJacobian
 The feature covariance jacobian, used...
typedef linear_algebra::Matrix
< double, feature_dimension,
feature_dimension > 		FeatureVariance
 The feature variance matrix (pulled from the ekf matrix).
typedef linear_algebra::Matrix
< double, feature_dimension,
feature_dimension > 		FeatureVarianceJacobian
 The feature variance jacobian, used...
typedef linear_algebra::Matrix
< double,
measurement_dimension, 1 > 		Measurement
 The odometry measurement vector.
typedef linear_algebra::Matrix
< double,
observation_dimension, 1 > 		Observation
 The observation vector.
typedef linear_algebra::Matrix
< double,
observation_dimension,
pose_dimension+feature_dimension > 		ObservationJacobian
 The observation jacobian.
typedef linear_algebra::Matrix
< double,
observation_dimension,
observation_dimension > 		ObservationNoise
 The observation noise matrix.
typedef linear_algebra::Matrix
< double, pose_dimension,
pose_dimension > 		OdometryMotionJacobian
 The odometry motion jacobian, used...
typedef linear_algebra::Matrix
< double,
measurement_dimension,
measurement_dimension > 		OdometryNoise
 The odometry noise matrix.
typedef linear_algebra::Matrix
< double,
measurement_dimension,
pose_dimension > 		OdometryNoiseJacobian
 The odometry noise jacobian, used...
typedef linear_algebra::Matrix
< double, pose_dimension, 1 > 		Pose
 The pose vector.
typedef linear_algebra::Matrix
< double, pose_dimension,
pose_dimension > 		PoseVariance
 The pose variance matrix (pulled from the ekf matrix).

Public Member Functions

void completeCovariance (linear_algebra::MatrixXd &cov)
Feature feature (const unsigned int &correspondence)
 Returns the state for the specified feature.
void init ()
 Initialises the underlying storage elements with the current capacity.
void init (const unsigned int &initial_capacity)
 Initialises the underlying storage elements.
int insert (const Feature &initial_state, const FeatureVariance &initial_variance) ecl_assert_throw_decl(StandardException)
 Adds a new feature.
int insert (const Feature &initial_state, const FeatureVariance &initial_variance, const FeatureVarianceJacobian &variance_jacobian, const FeatureCoVarianceJacobian &covariance_jacobian) ecl_assert_throw_decl(StandardException)
 Adds a new feature.
Pose pose ()
 Returns the current pose.
bool remove (const unsigned int &correspondence)
 Removes feature information with the specified correspondence from the filter.
void reserve (const unsigned int &new_capacity) ecl_assert_throw_decl(StandardException)
 Increases the reserve capacity of the filter (in # landmarks).
unsigned int size () const
 Returns the number of features currently stored in the map.
 SlamEkfBase (const unsigned int &initial_capacity=30)
 Configures the filter with default motion and observation models.
virtual ~SlamEkfBase ()

Public Attributes

linear_algebra::MatrixXd covariance
linear_algebra::VectorXd state

Static Public Attributes

static const unsigned int feature_dimension = FeatureDimension
 Dimension of the feature itself.
static const unsigned int measurement_dimension = MeasurementDimension
 Dimension of the odometry measurement.
static const unsigned int observation_dimension = ObservationDimension
 Dimension of the feature observation.
static const unsigned int pose_dimension = PoseDimension
 Dimension of the odometry pose.

Private Member Functions

unsigned int capacity ()
 Returns the maximum number of features storable in the map.

Private Attributes

unsigned int number_features

Detailed Description

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
class ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >

This is mostly aligned with the theory in "Probabilistic Robotics" by Sebastian Thrun & co.

Todo:
Need concepts for the odometry model and measurement model. These will in turn decide the specs for the ekf.

Size is generally referred to as the number of landmarks currently stored.

Definition at line 46 of file slam_ekf_base.hpp.

Member Typedef Documentation

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,feature_dimension,1> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::Feature

The feature vector.

Definition at line 63 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,feature_dimension,pose_dimension> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::FeatureCoVarianceJacobian

The feature covariance jacobian, used...

Definition at line 66 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,feature_dimension,feature_dimension> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::FeatureVariance

The feature variance matrix (pulled from the ekf matrix).

Definition at line 64 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,feature_dimension,feature_dimension> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::FeatureVarianceJacobian

The feature variance jacobian, used...

Definition at line 65 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,measurement_dimension,1> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::Measurement

The odometry measurement vector.

Definition at line 53 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,observation_dimension,1> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::Observation

The observation vector.

Definition at line 60 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,observation_dimension,pose_dimension+feature_dimension> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::ObservationJacobian

The observation jacobian.

Definition at line 61 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,observation_dimension,observation_dimension> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::ObservationNoise

The observation noise matrix.

Definition at line 62 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,pose_dimension,pose_dimension> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::OdometryMotionJacobian

The odometry motion jacobian, used...

Definition at line 57 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,measurement_dimension,measurement_dimension> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::OdometryNoise

The odometry noise matrix.

Definition at line 56 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,measurement_dimension,pose_dimension> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::OdometryNoiseJacobian

The odometry noise jacobian, used...

Definition at line 58 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,pose_dimension,1> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::Pose

The pose vector.

Definition at line 54 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
typedef linear_algebra::Matrix<double,pose_dimension,pose_dimension> ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::PoseVariance

The pose variance matrix (pulled from the ekf matrix).

Definition at line 55 of file slam_ekf_base.hpp.

Constructor & Destructor Documentation

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::SlamEkfBase ( const unsigned int &  initial_capacity = 30)

Configures the filter with default motion and observation models.

This sets up the storage elements and default versions of the specified motion/observation models.

Definition at line 114 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
virtual ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::~SlamEkfBase ( ) [inline, virtual]

Definition at line 72 of file slam_ekf_base.hpp.

Member Function Documentation

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
unsigned int ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::capacity ( ) [private]

Returns the maximum number of features storable in the map.

Definition at line 373 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
void ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::completeCovariance ( linear_algebra::MatrixXd &  cov)

Definition at line 193 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
Feature ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::feature ( const unsigned int &  correspondence) [inline]

Returns the state for the specified feature.

Definition at line 93 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
void ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::init ( )

Initialises the underlying storage elements with the current capacity.

Use this to reinitialise without changing the current capacity.

Definition at line 123 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
void ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::init ( const unsigned int &  initial_capacity)

Initialises the underlying storage elements.

This is called by the constructors internally, but also useful if you need to reset the filter.

Definition at line 136 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
int ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::insert ( const Feature &  initial_state,
const FeatureVariance &  initial_variance 
)

Adds a new feature.

This adds a new feature to the filter. Practically, it just finds an empty line in the matrices and utilises that. If the filter is already full, it will do one of either:

  • debug mode : throw an exception
  • release mode : return -1 indicating failure to add.

Note that this does not set any covariances or skew the initial variance realistically. Should only use this if such rough initialisations are not of importance (i.e. you have plenty of time to settle & converge).

Parameters:
initial_state: initial mean of the new landmark.
initial_variance: initial variance of the new landmark.
Returns:
int : correspondence of the addition (-1 if failed).
Exceptions:
StandardException: throws if the filter is already full [debug mode only].

Definition at line 166 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
int ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::insert ( const Feature &  initial_state,
const FeatureVariance &  initial_variance,
const FeatureVarianceJacobian &  variance_jacobian,
const FeatureCoVarianceJacobian &  covariance_jacobian 
)

Adds a new feature.

This adds a new feature to the filter. Practically, it just finds an empty line in the matrices and utilises that. If the filter is already full, it will do one of either:

  • debug mode : throw an exception
  • release mode : return -1 indicating failure to add.
Parameters:
initial_state: initial mean of the new landmark.
initial_variance: initial variance of the new landmark.
variance_jacobian: jacobian used to initialise the landmark variance
covariance_jacobian: jacobian used to initialise robot-lm and lm-lm variances.
Returns:
int : correspondence of the addition (-1 if failed).
Exceptions:
StandardException: throws if the filter is already full [debug mode only].

Definition at line 249 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
Pose ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::pose ( ) [inline]

Returns the current pose.

Definition at line 92 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
bool ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::remove ( const unsigned int &  correspondence)

Removes feature information with the specified correspondence from the filter.

Removes the specified landmark from the filter. In practical terms, this just zeros the entries for that correspondence in both mean and variance storage containers.

Parameters:
correspondence: index of the feature to be removed.
Returns:
bool : success/failure of the removal.
Exceptions:
StandardException: throws if the filter is already full [debug mode only].

Definition at line 318 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
void ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::reserve ( const unsigned int &  new_capacity)

Increases the reserve capacity of the filter (in # landmarks).

This expands the underlying matrices, zero'ing out the new columns until landmarks are actually added. This is similar to the way in which std::vector behaves.

Parameters:
new_capacity: max number of landmarks to filter.
Exceptions:
StandardException: throws if the new size is smaller than current [debug mode only].

Definition at line 342 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
unsigned int ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::size ( ) const [inline]

Returns the number of features currently stored in the map.

Definition at line 94 of file slam_ekf_base.hpp.

Member Data Documentation

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
linear_algebra::MatrixXd ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::covariance

Definition at line 97 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
const unsigned int ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::feature_dimension = FeatureDimension [static]

Dimension of the feature itself.

Definition at line 51 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
const unsigned int ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::measurement_dimension = MeasurementDimension [static]

Dimension of the odometry measurement.

Definition at line 48 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
unsigned int ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::number_features [private]

Definition at line 100 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
const unsigned int ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::observation_dimension = ObservationDimension [static]

Dimension of the feature observation.

Definition at line 50 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
const unsigned int ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::pose_dimension = PoseDimension [static]

Dimension of the odometry pose.

Definition at line 49 of file slam_ekf_base.hpp.

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
linear_algebra::VectorXd ecl::SlamEkfBase< MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension >::state

Definition at line 96 of file slam_ekf_base.hpp.

The documentation for this class was generated from the following file:

ecl_slam
Author(s): Daniel Stonier (d.stonier@gmail.com)
autogenerated on Thu Jan 2 2014 11:12:28