slam_ekf_base.hpp
Go to the documentation of this file.
00001 
00008 /*****************************************************************************
00009 ** Ifdefs
00010 *****************************************************************************/
00011 
00012 #ifndef ECL_SLAM_EKF_BASE_HPP_
00013 #define ECL_SLAM_EKF_BASE_HPP_
00014 
00015 /*****************************************************************************
00016 ** Includes
00017 *****************************************************************************/
00018 
00019 #include <vector>
00020 #include <ecl/exceptions/standard_exception.hpp>
00021 #include <ecl/time/duration.hpp>
00022 #include <ecl/time/stopwatch.hpp>
00023 #include <ecl/linear_algebra.hpp>
00024 
00025 /*****************************************************************************
00026 ** Namespaces
00027 *****************************************************************************/
00028 
00029 namespace ecl {
00030 
00031 /*****************************************************************************
00032 ** Interfaces
00033 *****************************************************************************/
00044 template<unsigned int MeasurementDimension, unsigned int PoseDimension,
00045                  unsigned int ObservationDimension, unsigned int FeatureDimension>
00046 class SlamEkfBase {
00047 public:
00048         static const unsigned int measurement_dimension = MeasurementDimension; 
00049         static const unsigned int pose_dimension = PoseDimension;               
00050         static const unsigned int observation_dimension = ObservationDimension; 
00051         static const unsigned int feature_dimension = FeatureDimension;         
00053         typedef linear_algebra::Matrix<double,measurement_dimension,1> Measurement;             
00054         typedef linear_algebra::Matrix<double,pose_dimension,1> Pose;                                                   
00055         typedef linear_algebra::Matrix<double,pose_dimension,pose_dimension> PoseVariance;              
00056         typedef linear_algebra::Matrix<double,measurement_dimension,measurement_dimension> OdometryNoise; 
00057         typedef linear_algebra::Matrix<double,pose_dimension,pose_dimension> OdometryMotionJacobian;            
00058         typedef linear_algebra::Matrix<double,measurement_dimension,pose_dimension> OdometryNoiseJacobian;      
00060         typedef linear_algebra::Matrix<double,observation_dimension,1> Observation;                          
00061         typedef linear_algebra::Matrix<double,observation_dimension,pose_dimension+feature_dimension> ObservationJacobian; 
00062         typedef linear_algebra::Matrix<double,observation_dimension,observation_dimension> ObservationNoise; 
00063         typedef linear_algebra::Matrix<double,feature_dimension,1> Feature;                                  
00064         typedef linear_algebra::Matrix<double,feature_dimension,feature_dimension> FeatureVariance;          
00065         typedef linear_algebra::Matrix<double,feature_dimension,feature_dimension> FeatureVarianceJacobian;  
00066         typedef linear_algebra::Matrix<double,feature_dimension,pose_dimension> FeatureCoVarianceJacobian;   
00068         /*********************
00069         ** Initialisation
00070         **********************/
00071         SlamEkfBase(const unsigned int &initial_capacity = 30);
00072         virtual ~SlamEkfBase() {};
00073         void init();
00074         void init(const unsigned int &initial_capacity);
00075 
00076         /*********************
00077         ** Modding Storage
00078         **********************/
00079         int insert(const Feature &initial_state, const FeatureVariance &initial_variance) ecl_assert_throw_decl(StandardException);
00080         int insert(const Feature &initial_state,
00081                         const FeatureVariance &initial_variance,
00082                         const FeatureVarianceJacobian &variance_jacobian,
00083                         const FeatureCoVarianceJacobian &covariance_jacobian
00084                         ) ecl_assert_throw_decl(StandardException);
00085         bool remove(const unsigned int &correspondence);
00086         void reserve(const unsigned int& new_capacity) ecl_assert_throw_decl(StandardException);
00087         void completeCovariance( linear_algebra::MatrixXd & cov );
00088 
00089         /*********************
00090         ** Accessors
00091         **********************/
00092         Pose pose() { return state.block(0,0,pose_dimension,1); } 
00093         Feature feature(const unsigned int &correspondence ) { return state.block(correspondence, 0, feature_dimension, 1); } 
00094         unsigned int size() const { return number_features; } 
00096         linear_algebra::VectorXd state;
00097         linear_algebra::MatrixXd covariance;
00098 
00099 private:
00100         unsigned int number_features;
00101         unsigned int capacity();                              
00102 };
00103 
00104 /*****************************************************************************
00105 ** Implementation [Initialisation]
00106 *****************************************************************************/
00113 template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
00114 SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::SlamEkfBase(const unsigned int &initial_capacity) :
00115         number_features(0.0)
00116 { init(initial_capacity); }
00122 template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
00123 void SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::init() {
00124         unsigned int dimension = state.rows();
00125         state = linear_algebra::VectorXd::Zero(dimension);
00126         covariance = linear_algebra::MatrixXd::Zero(dimension,dimension);
00127         number_features = 0;
00128 }
00135 template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
00136 void SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::init(const unsigned int &initial_capacity) {
00137         unsigned int dimension = pose_dimension + initial_capacity*feature_dimension;
00138         state = linear_algebra::VectorXd::Zero(dimension);
00139         covariance = linear_algebra::MatrixXd::Zero(dimension,dimension);
00140         number_features = 0;
00141 }
00142 /*****************************************************************************
00143 ** Implementation [SlamEkfBase Mods]
00144 *****************************************************************************/
00165 template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
00166 int SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::insert(
00167                 const linear_algebra::Matrix<double,feature_dimension,1> &initial_state,
00168                 const linear_algebra::Matrix<double,feature_dimension,feature_dimension> &initial_variance) ecl_assert_throw_decl(StandardException) {
00169 
00170         ecl_assert_throw(number_features < capacity(), StandardException(LOC,MemoryError,"The filter does not have any spare storage space (use reserve() to allocate more)."));
00171         bool found_unused_correspondence = false;
00172         unsigned int index = pose_dimension;
00173 
00174         while ( (!found_unused_correspondence) && ( index < static_cast<unsigned int>(state.rows()) ) ) {
00175                 if ( covariance(index,index) == 0.0 ) {
00176                         found_unused_correspondence = true;
00177                 } else {
00178                         index += feature_dimension;
00179                 }
00180         }
00181         if ( found_unused_correspondence ) {
00182                 state.block(index,0,feature_dimension,1) = initial_state;
00183                 covariance.block(index,index,feature_dimension,feature_dimension) = initial_variance;
00184                 ++number_features;
00185                 return index;
00186         } else {
00187                 return -1; // will only reach here in release mode (debug mode throws before it can get here)
00188         }
00189 }
00190 
00191 
00192 template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
00193 void SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::completeCovariance( linear_algebra::MatrixXd & cov )
00194 {
00195         // getting matrix size
00196         int target_matrix_size = pose_dimension + feature_dimension*number_features;
00197         int current_matrix_size = covariance.rows();
00198         int cnt_rows(0);
00199         int cnt_cols(0);
00200 
00201         cov.resize( target_matrix_size, target_matrix_size );
00202 
00203         for( int i=0; i<current_matrix_size; i++ )
00204         {
00205                  if( covariance(i,i) == 0.0 )
00206                  {
00207                          continue;
00208                  }
00209                 cnt_cols=0;
00210                 for( int j=0; j<current_matrix_size; j++ )
00211                 {
00212                         if( covariance(j,j) == 0.0 )
00213                         {
00214                                 continue;
00215                         }
00216                         else
00217                         {
00218                                 cov(cnt_rows,cnt_cols++) = covariance(i,j);
00219                         }
00220                 }
00221 
00222                 // next row
00223                 cnt_rows += 1;
00224         }
00225 
00226         // you have complete covariance matrix without zero part
00227 }
00228 
00229 
00248 template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
00249 int SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::insert(
00250                 const Feature &initial_state,
00251                 const FeatureVariance &initial_variance,
00252                 const FeatureVarianceJacobian &variance_jacobian,
00253                 const FeatureCoVarianceJacobian &covariance_jacobian
00254                 ) ecl_assert_throw_decl(StandardException) {
00255 
00256         ecl_assert_throw(number_features < capacity(), StandardException(LOC,MemoryError,"The filter does not have any spare storage space (use reserve() to allocate more)."));
00257         bool found_unused_correspondence = false;
00258         int index = pose_dimension;
00259         /*********************
00260         ** Find Unused Slot
00261         **********************/
00262         while ( (!found_unused_correspondence) && ( index < state.rows() ) ) {
00263                 if ( covariance(index,index) == 0.0 ) {
00264                         found_unused_correspondence = true;
00265                 } else {
00266                         index += feature_dimension;
00267                 }
00268         }
00269         /*********************
00270         ** Initialise
00271         **********************/
00272         if ( found_unused_correspondence ) {
00273                 state.block(index,0,feature_dimension,1) = initial_state;
00274                 /*********************
00275                 ** Landmark Variance
00276                 **********************/
00277                 covariance.block(index,index,feature_dimension,feature_dimension) =
00278                                 covariance_jacobian*covariance.block(0,0,pose_dimension,pose_dimension)*covariance_jacobian.transpose() +
00279                                 variance_jacobian*initial_variance*variance_jacobian.transpose();
00280 
00281                 /*********************
00282                 ** Robot-LM Covariance
00283                 **********************/
00284                 linear_algebra::Matrix<double,feature_dimension,pose_dimension> landmark_robot_covariance = covariance_jacobian*covariance.block(0,0,3,3);
00285                 covariance.block(index,0,feature_dimension,pose_dimension) = landmark_robot_covariance;
00286                 covariance.block(0,index,pose_dimension,feature_dimension) = landmark_robot_covariance.transpose();
00287                 /*********************
00288                 ** LM-LM Covariance
00289                 **********************/
00290                 for ( unsigned int i = 0; i < capacity(); ++i ) {
00291                         int idx = pose_dimension + feature_dimension*i;
00292                         if ( idx != index ) {
00293                                 FeatureVarianceJacobian landmark_covariance = covariance_jacobian*covariance.block(0,idx,pose_dimension,feature_dimension);
00294                                 covariance.block(index,idx,feature_dimension,feature_dimension) = landmark_covariance;
00295                                 covariance.block(idx,index,feature_dimension,feature_dimension) = landmark_covariance.transpose();
00296                         }
00297                 }
00298                 ++number_features;
00299                 return index;
00300         } else {
00301                 return -1; // will only reach here in release mode (debug mode throws before it can get here)
00302         }
00303 }
00304 
00317 template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
00318 bool SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::remove(const unsigned int &correspondence) {
00319         for ( unsigned int i = 0; i < feature_dimension; ++i ) {
00320                 state[correspondence+i] = 0.0;
00321         }
00322         for ( unsigned int i = 0; i < feature_dimension; ++i ) {
00323                 for ( int j = 0; j < covariance.cols(); ++j ) {
00324                         covariance(correspondence+i,j) = 0.0;
00325                         covariance(j,correspondence+i) = 0.0;
00326                 }
00327         }
00328         --number_features;
00329         return true;
00330 }
00341 template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
00342 void SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::reserve(const unsigned int& new_capacity) ecl_assert_throw_decl(StandardException) {
00343 
00344         unsigned int current_capacity = capacity();
00345         unsigned int current_dimension = pose_dimension+current_capacity*feature_dimension;
00346         unsigned int new_dimension = pose_dimension+new_capacity*feature_dimension;
00347 
00348         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)."));
00349 
00350         state.conservativeResize(new_dimension);
00351         for ( unsigned int i = current_dimension; i < new_dimension; ++i ) {
00352                 state[i] = 0.0;
00353         }
00354         covariance.conservativeResize(new_dimension,new_dimension);
00355         // Directly setting is faster than using covariance.block(...) = Zero... - is there a faster method?
00356         for ( unsigned int i = 0; i < current_dimension; ++i )  {
00357                 for ( unsigned int j = current_dimension; j < new_dimension; ++j ) {
00358                         covariance(i,j) = 0.0;
00359                 }
00360         }
00361         for ( unsigned int i = current_dimension; i < new_dimension; ++i )  {
00362                 for ( unsigned int j = 0; j < new_dimension; ++j ) {
00363                         covariance(i,j) = 0.0;
00364                 }
00365         }
00366 }
00367 
00368 /*****************************************************************************
00369 ** Implementation [Private]
00370 *****************************************************************************/
00371 
00372 template<unsigned int MeasurementDimension, unsigned int PoseDimension, unsigned int ObservationDimension, unsigned int FeatureDimension>
00373 unsigned int SlamEkfBase<MeasurementDimension, PoseDimension, ObservationDimension, FeatureDimension>::capacity() {
00374         return (state.size()-pose_dimension)/feature_dimension;
00375 }
00376 
00377 } // namespace ecl
00378 
00379 #endif /* ECL_SLAM_EKF_BASE_HPP_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


ecl_slam
Author(s): Daniel Stonier
autogenerated on Thu Apr 25 2013 15:26:27