Go to the documentation of this file.00001
00008
00009
00010
00011
00012 #ifndef ECL_SLAM_OBSERVATION_GENERIC_HPP_
00013 #define ECL_SLAM_OBSERVATION_GENERIC_HPP_
00014
00015
00016
00017
00018
00019 #include <vector>
00020 #include <ecl/linear_algebra.hpp>
00021
00022
00023
00024
00025
00026 namespace ecl {
00027 namespace slam_ekf {
00028
00029
00030
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
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 ¤t_pose ) = 0;
00102 virtual Noise noise() = 0;
00103 virtual Innovation innovation( const Observation &observation, const Feature &feature, const Pose ¤t_pose ) = 0;
00104
00105 };
00106
00107
00108 }
00109 }
00110
00111 #endif