ecl_slam: ecl_slam: observation_generic.hpp Source File

00001 
00008 /*****************************************************************************
00009 ** Ifdefs
00010 *****************************************************************************/
00011 
00012 #ifndef ECL_SLAM_OBSERVATION_GENERIC_HPP_
00013 #define ECL_SLAM_OBSERVATION_GENERIC_HPP_
00014 
00015 /*****************************************************************************
00016 ** Includes
00017 *****************************************************************************/
00018 
00019 #include <vector>
00020 #include <ecl/linear_algebra.hpp>
00021 
00022 /*****************************************************************************
00023 ** Namespaces
00024 *****************************************************************************/
00025 
00026 namespace ecl {
00027 namespace slam_ekf {
00028 
00029 /*****************************************************************************
00030 ** Interfaces
00031 *****************************************************************************/
00035 template <typename FilterBase>
00036 class GenericObservationModel {
00037 public:
00038         static const unsigned int pose_dimension = FilterBase::pose_dimension;
00039         static const unsigned int observation_dimension = FilterBase::observation_dimension;
00040         static const unsigned int feature_dimension = FilterBase::feature_dimension;
00041 
00042         typedef typename FilterBase::Pose Pose;
00043         typedef typename FilterBase::Observation Observation;
00044         typedef typename FilterBase::Observation Innovation;
00045         typedef typename FilterBase::ObservationJacobian Jacobian;
00046         typedef typename FilterBase::ObservationNoise Noise;
00047         typedef typename FilterBase::Feature Feature;
00048         typedef typename FilterBase::FeatureVariance FeatureVariance;
00049 
00050         GenericObservationModel(FilterBase &ekf_arrays) : filter_base(ekf_arrays) {}
00051         virtual ~GenericObservationModel() {}
00052 
00066         virtual void updateFilter(const std::vector<Observation> &observations, const std::vector<unsigned int> &correspondences) {
00067 
00068                 if ( observations.empty() ) { return; }
00069                 // should also check if the observation size = corespondences size
00070 
00071                 const unsigned int M = observations.size();
00072 
00073                 ecl::linear_algebra::MatrixXd total_observation_jacobian = ecl::linear_algebra::MatrixXd::Zero(observation_dimension*M, filter_base.covariance.cols());
00074                 ecl::linear_algebra::VectorXd total_innovation = ecl::linear_algebra::VectorXd::Zero(observation_dimension*M);
00075                 ecl::linear_algebra::MatrixXd total_measurement_noise = ecl::linear_algebra::MatrixXd::Zero(observation_dimension*M, observation_dimension*M);
00076                 Jacobian observation_jacobian;
00077 
00078                 for ( unsigned int i = 0; i < M; ++i ) {
00079                         unsigned int correspondence = correspondences[i];
00080                         total_innovation.block(observation_dimension*i,0,observation_dimension,1) = innovation(observations[i],filter_base.feature(correspondence),pose());
00081                         observation_jacobian = observationJacobian(filter_base.feature(correspondence), pose());
00082                         total_observation_jacobian.block(observation_dimension*i,0,observation_dimension,pose_dimension) = observation_jacobian.block(0,0,observation_dimension,pose_dimension);
00083                         total_observation_jacobian.block(observation_dimension*i,correspondence,observation_dimension,feature_dimension) = observation_jacobian.block(0,0,observation_dimension,feature_dimension);
00084                         total_measurement_noise.block(observation_dimension*i,observation_dimension*i,observation_dimension,observation_dimension) = noise();
00085                 }
00086                 ecl::linear_algebra::MatrixXd total_measurement_uncertainty = total_observation_jacobian*filter_base.covariance*total_observation_jacobian.transpose()+total_measurement_noise;
00087                 ecl::linear_algebra::MatrixXd total_gains = filter_base.covariance*total_observation_jacobian.transpose()*total_measurement_uncertainty.inverse();
00088                 ecl::linear_algebra::VectorXd mean_updates = total_gains*total_innovation;
00089                 filter_base.state += mean_updates;
00090                 filter_base.covariance -= (total_gains*total_measurement_uncertainty*total_gains.transpose());
00091                 filter_base.covariance = 0.5*(filter_base.covariance+filter_base.covariance.adjoint());
00092         }
00093 
00094         Pose pose() {
00095                 return filter_base.state.block(0,0,pose_dimension,1);
00096         }
00097 
00098 protected:
00099         FilterBase &filter_base;
00100 
00101         virtual Jacobian observationJacobian( const Feature& feature, const Pose &current_pose ) = 0;
00102         virtual Noise noise() = 0;
00103         virtual Innovation innovation( const Observation &observation, const Feature &feature, const Pose &current_pose ) = 0;
00104 
00105 };
00106 
00107 
00108 } // namespace slam_ekf
00109 } // namespace ecl
00110 
00111 #endif /* ECL_SLAM_OBSERVATION_GENERIC_HPP_ */