slam_ekf_base.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef ECL_SLAM_EKF_BASE_HPP_
#define ECL_SLAM_EKF_BASE_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <vector>
#include <ecl/exceptions/standard_exception.hpp>
#include <ecl/time/duration.hpp>
#include <ecl/time/stopwatch.hpp>
#include <ecl/linear_algebra.hpp>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace ecl {

/*****************************************************************************
** Interfaces
*****************************************************************************/
template<unsigned int MeasurementDimension, unsigned int PoseDimension,
                 unsigned int ObservationDimension, unsigned int FeatureDimension>
class SlamEkfBase {
public:
        static const unsigned int measurement_dimension = MeasurementDimension; 
        static const unsigned int pose_dimension = PoseDimension;               
        static const unsigned int observation_dimension = ObservationDimension; 
        static const unsigned int feature_dimension = FeatureDimension;         
        typedef linear_algebra::Matrix<double,measurement_dimension,1> Measurement;             
        typedef linear_algebra::Matrix<double,pose_dimension,1> Pose;                                                   
        typedef linear_algebra::Matrix<double,pose_dimension,pose_dimension> PoseVariance;              
        typedef linear_algebra::Matrix<double,measurement_dimension,measurement_dimension> OdometryNoise; 
        typedef linear_algebra::Matrix<double,pose_dimension,pose_dimension> OdometryMotionJacobian;            
        typedef linear_algebra::Matrix<double,measurement_dimension,pose_dimension> OdometryNoiseJacobian;      
        typedef linear_algebra::Matrix<double,observation_dimension,1> Observation;                          
        typedef linear_algebra::Matrix<double,observation_dimension,pose_dimension+feature_dimension> ObservationJacobian; 
        typedef linear_algebra::Matrix<double,observation_dimension,observation_dimension> ObservationNoise; 
        typedef linear_algebra::Matrix<double,feature_dimension,1> Feature;                                  
        typedef linear_algebra::Matrix<double,feature_dimension,feature_dimension> FeatureVariance;          
        typedef linear_algebra::Matrix<double,feature_dimension,feature_dimension> FeatureVarianceJacobian;  
        typedef linear_algebra::Matrix<double,feature_dimension,pose_dimension> FeatureCoVarianceJacobian;   
        /*********************
        ** Initialisation
        **********************/
        SlamEkfBase(const unsigned int &initial_capacity = 30);
        virtual ~SlamEkfBase() {};
        void init();
        void init(const unsigned int &initial_capacity);

        /*********************
        ** Modding Storage
        **********************/
        int insert(const Feature &initial_state, const FeatureVariance &initial_variance) ecl_assert_throw_decl(StandardException);
        int insert(const Feature &initial_state,
                        const FeatureVariance &initial_variance,
                        const FeatureVarianceJacobian &variance_jacobian,
                        const FeatureCoVarianceJacobian &covariance_jacobian
                        ) ecl_assert_throw_decl(StandardException);
        bool remove(const unsigned int &correspondence);
        void reserve(const unsigned int& new_capacity) ecl_assert_throw_decl(StandardException);
        void completeCovariance( linear_algebra::MatrixXd & cov );

        /*********************
        ** Accessors
        **********************/
        Pose pose() { return state.block(0,0,pose_dimension,1); } 
        Feature feature(const unsigned int &correspondence ) { return state.block(correspondence, 0, feature_dimension, 1); } 
        unsigned int size() const { return number_features; } 
        linear_algebra::VectorXd state;
        linear_algebra::MatrixXd covariance;

private:
        unsigned int number_features;
        unsigned int capacity();                              
};

/*****************************************************************************
** Implementation [Initialisation]
*****************************************************************************/
template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::SlamEkfBase(const unsigned int &initial_capacity) :
        number_features(0.0)
{ init(initial_capacity); }
template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
void SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::init() {
        unsigned int dimension = state.rows();
        state = linear_algebra::VectorXd::Zero(dimension);
        covariance = linear_algebra::MatrixXd::Zero(dimension,dimension);
        number_features = 0;
}
template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
void SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::init(const unsigned int &initial_capacity) {
        unsigned int dimension = pose_dimension + initial_capacity*feature_dimension;
        state = linear_algebra::VectorXd::Zero(dimension);
        covariance = linear_algebra::MatrixXd::Zero(dimension,dimension);
        number_features = 0;
}
/*****************************************************************************
** Implementation [SlamEkfBase Mods]
*****************************************************************************/
template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
int SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::insert(
                const linear_algebra::Matrix<double,feature_dimension,1> &initial_state,
                const linear_algebra::Matrix<double,feature_dimension,feature_dimension> &initial_variance) ecl_assert_throw_decl(StandardException) {

        ecl_assert_throw(number_features < capacity(), StandardException(LOC,MemoryError,"The filter does not have any spare storage space (use reserve() to allocate more)."));
        bool found_unused_correspondence = false;
        unsigned int index = pose_dimension;

        while ( (!found_unused_correspondence) && ( index < static_cast<unsigned int>(state.rows()) ) ) {
                if ( covariance(index,index) == 0.0 ) {
                        found_unused_correspondence = true;
                } else {
                        index += feature_dimension;
                }
        }
        if ( found_unused_correspondence ) {
                state.block(index,0,feature_dimension,1) = initial_state;
                covariance.block(index,index,feature_dimension,feature_dimension) = initial_variance;
                ++number_features;
                return index;
        } else {
                return -1; // will only reach here in release mode (debug mode throws before it can get here)
        }
}


template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
void SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::completeCovariance( linear_algebra::MatrixXd & cov )
{
        // getting matrix size
        int target_matrix_size = pose_dimension + feature_dimension*number_features;
        int current_matrix_size = covariance.rows();
        int cnt_rows(0);
        int cnt_cols(0);

        cov.resize( target_matrix_size, target_matrix_size );

        for( int i=0; i<current_matrix_size; i++ )
        {
                 if( covariance(i,i) == 0.0 )
                 {
                         continue;
                 }
                cnt_cols=0;
                for( int j=0; j<current_matrix_size; j++ )
                {
                        if( covariance(j,j) == 0.0 )
                        {
                                continue;
                        }
                        else
                        {
                                cov(cnt_rows,cnt_cols++) = covariance(i,j);
                        }
                }

                // next row
                cnt_rows += 1;
        }

        // you have complete covariance matrix without zero part
}


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

        ecl_assert_throw(number_features < capacity(), StandardException(LOC,MemoryError,"The filter does not have any spare storage space (use reserve() to allocate more)."));
        bool found_unused_correspondence = false;
        int index = pose_dimension;
        /*********************
        ** Find Unused Slot
        **********************/
        while ( (!found_unused_correspondence) && ( index < state.rows() ) ) {
                if ( covariance(index,index) == 0.0 ) {
                        found_unused_correspondence = true;
                } else {
                        index += feature_dimension;
                }
        }
        /*********************
        ** Initialise
        **********************/
        if ( found_unused_correspondence ) {
                state.block(index,0,feature_dimension,1) = initial_state;
                /*********************
                ** Landmark Variance
                **********************/
                covariance.block(index,index,feature_dimension,feature_dimension) =
                                covariance_jacobian*covariance.block(0,0,pose_dimension,pose_dimension)*covariance_jacobian.transpose() +
                                variance_jacobian*initial_variance*variance_jacobian.transpose();

                /*********************
                ** Robot-LM Covariance
                **********************/
                linear_algebra::Matrix<double,feature_dimension,pose_dimension> landmark_robot_covariance = covariance_jacobian*covariance.block(0,0,3,3);
                covariance.block(index,0,feature_dimension,pose_dimension) = landmark_robot_covariance;
                covariance.block(0,index,pose_dimension,feature_dimension) = landmark_robot_covariance.transpose();
                /*********************
                ** LM-LM Covariance
                **********************/
                for ( unsigned int i = 0; i < capacity(); ++i ) {
                        int idx = pose_dimension + feature_dimension*i;
                        if ( idx != index ) {
                                FeatureVarianceJacobian landmark_covariance = covariance_jacobian*covariance.block(0,idx,pose_dimension,feature_dimension);
                                covariance.block(index,idx,feature_dimension,feature_dimension) = landmark_covariance;
                                covariance.block(idx,index,feature_dimension,feature_dimension) = landmark_covariance.transpose();
                        }
                }
                ++number_features;
                return index;
        } else {
                return -1; // will only reach here in release mode (debug mode throws before it can get here)
        }
}

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
bool SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::remove(const unsigned int &correspondence) {
        for ( unsigned int i = 0; i < feature_dimension; ++i ) {
                state[correspondence+i] = 0.0;
        }
        for ( unsigned int i = 0; i < feature_dimension; ++i ) {
                for ( int j = 0; j < covariance.cols(); ++j ) {
                        covariance(correspondence+i,j) = 0.0;
                        covariance(j,correspondence+i) = 0.0;
                }
        }
        --number_features;
        return true;
}
template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
void SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::reserve(const unsigned int& new_capacity) ecl_assert_throw_decl(StandardException) {

        unsigned int current_capacity = capacity();
        unsigned int current_dimension = pose_dimension+current_capacity*feature_dimension;
        unsigned int new_dimension = pose_dimension+new_capacity*feature_dimension;

        ecl_assert_throw( new_capacity > current_capacity, StandardException(LOC,InvalidInputError,"This class has a simple policy for safety, new size should be greater than the current size (unless reinitialising)."));

        state.conservativeResize(new_dimension);
        for ( unsigned int i = current_dimension; i < new_dimension; ++i ) {
                state[i] = 0.0;
        }
        covariance.conservativeResize(new_dimension,new_dimension);
        // Directly setting is faster than using covariance.block(...) = Zero... - is there a faster method?
        for ( unsigned int i = 0; i < current_dimension; ++i )  {
                for ( unsigned int j = current_dimension; j < new_dimension; ++j ) {
                        covariance(i,j) = 0.0;
                }
        }
        for ( unsigned int i = current_dimension; i < new_dimension; ++i )  {
                for ( unsigned int j = 0; j < new_dimension; ++j ) {
                        covariance(i,j) = 0.0;
                }
        }
}

/*****************************************************************************
** Implementation [Private]
*****************************************************************************/

template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
unsigned int SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::capacity() {
        return (state.size()-pose_dimension)/feature_dimension;
}

} // namespace ecl

#endif /* ECL_SLAM_EKF_BASE_HPP_ */