ecl_slam: odometry_generic.hpp Source File

Go to the documentation of this file.
00001 
00008 /*****************************************************************************
00009 ** Ifdefs
00010 *****************************************************************************/
00011 
00012 #ifndef ECL_SLAM_ODOMETRY_GENERIC_HPP_
00013 #define ECL_SLAM_ODOMETRY_GENERIC_HPP_
00014 
00015 /*****************************************************************************
00016 ** Includes
00017 *****************************************************************************/
00018 
00019 #include <ecl/linear_algebra.hpp>
00020 
00021 /*****************************************************************************
00022 ** Namespaces
00023 *****************************************************************************/
00024 
00025 namespace ecl {
00026 namespace slam_ekf {
00027 
00028 /*****************************************************************************
00029 ** Interfaces
00030 *****************************************************************************/
00031 
00032 template <typename FilterBase>
00033 class GenericOdometryModel {
00034 public:
00035         static const unsigned int measurement_dimension = FilterBase::measurement_dimension;
00036         static const unsigned int pose_dimension = FilterBase::pose_dimension;
00037 
00038         typedef typename FilterBase::Measurement Measurement;
00039         typedef typename FilterBase::Pose Pose;
00040         typedef typename FilterBase::OdometryMotionJacobian MotionJacobian;
00041         typedef typename FilterBase::OdometryNoiseJacobian NoiseJacobian;
00042         typedef typename FilterBase::OdometryNoise Noise;
00043 
00044         virtual void updateFilter(const Measurement &incoming) {
00045 
00046                 /******************************************
00047                 ** Calculations
00048                 *******************************************/
00049                 Pose current_pose = filter_base.state.block(0,0,pose_dimension,1);
00050                 MotionJacobian motion_jacobian = motionJacobian(incoming);
00051                 NoiseJacobian noise_jacobian = noiseJacobian(incoming);
00052                 ecl::linear_algebra::MatrixXd pose_lm_covariances;
00053                 pose_lm_covariances = motion_jacobian*filter_base.covariance.block(0,pose_dimension,pose_dimension,filter_base.covariance.cols()-pose_dimension);
00054 
00055                 /******************************************
00056                 ** Updates
00057                 *******************************************/
00058                 filter_base.state.block(0,0,pose_dimension,1) += poseUpdate(incoming);
00059                 filter_base.covariance.block(0,0,pose_dimension,pose_dimension) =  motion_jacobian*(filter_base.covariance.block(0,0,pose_dimension,pose_dimension))*motion_jacobian.transpose()
00060                                                 + noise_jacobian*noise(incoming)*noise_jacobian.transpose();
00061                 filter_base.covariance.block(0,pose_dimension,pose_dimension,filter_base.covariance.cols()-pose_dimension) = pose_lm_covariances;
00062                 filter_base.covariance.block(pose_dimension,0,filter_base.covariance.cols()-pose_dimension,pose_dimension) = pose_lm_covariances.transpose();
00063         }
00064 
00065         Pose pose() {
00066                 return filter_base.state.block(0,0,pose_dimension,1);
00067         }
00068 
00069 protected:
00070         GenericOdometryModel(FilterBase &ekf_arrays) : filter_base(ekf_arrays) {}
00071         virtual ~GenericOdometryModel() {}
00072 
00073         virtual Pose poseUpdate(const Measurement &incoming ) = 0;
00074         virtual MotionJacobian motionJacobian(const Measurement &  incoming ) = 0;
00075         virtual NoiseJacobian noiseJacobian(const Measurement & incoming ) = 0;
00076         virtual Noise noise(const Measurement &incoming) = 0;
00077 
00078         FilterBase &filter_base;
00079 };
00080 
00081 
00082 } // namespace slam_ekf
00083 } // namespace ecl
00084 
00085 #endif /* ECL_SLAM_ODOMETRY_GENERIC_HPP_ */