slammer1d.cpp
Go to the documentation of this file.
00001 
00014 /*****************************************************************************
00015 ** Includes
00016 *****************************************************************************/
00017 
00018 #include <iostream>
00019 #include <string>
00020 #include <vector>
00021 #include <ecl/formatters.hpp>
00022 #include <ecl/utilities/parameter.hpp>
00023 #include <ecl/slam/slam_ekf_base.hpp>
00024 #include <ecl/slam/slam_ekf/odometry_generic.hpp>
00025 #include <ecl/slam/slam_ekf/observation_generic.hpp>
00026 
00027 /*****************************************************************************
00028 ** Using
00029 *****************************************************************************/
00030 
00031 using std::string;
00032 using std::vector;
00033 using ecl::Format;
00034 using ecl::RightAlign;
00035 using ecl::linear_algebra::Matrix;
00036 using ecl::linear_algebra::MatrixXd;
00037 using ecl::linear_algebra::Vector2d;
00038 using ecl::linear_algebra::Vector3d;
00039 using ecl::linear_algebra::VectorXd;
00040 using ecl::slam_ekf::GenericOdometryModel;
00041 using ecl::slam_ekf::GenericObservationModel;
00042 using ecl::SlamEkfBase;
00043 using ecl::Parameter;
00044 
00045 /*****************************************************************************
00046 ** Classes
00047 *****************************************************************************/
00048 
00049 namespace ecl {
00050 namespace demos {
00051 
00052 /******************************************
00053 ** LandMark
00054 *******************************************/
00055 
00056 class LandMark {
00057 public:
00058         static const unsigned int observation_dimension = 1;
00059         typedef Matrix<double,observation_dimension,1> Observation;
00060 
00061         friend class SlidingObservationModel;
00062 
00063         const unsigned int& id() const { return unique_id; }
00064 
00072         void observation(const Observation& obs) {
00073                 last_observation = obs;
00074         }
00075         const Observation& observation() const { return last_observation; }
00076 
00077 
00078         Parameter<unsigned int> correspondence;
00079         ~LandMark() {};
00080 
00081 private:
00082         LandMark(const unsigned int& corr) :
00083                 correspondence(corr)
00084         {
00085                 static unsigned int id_counter = 0;
00086                 unique_id = id_counter++;
00087         }
00088 
00089         Observation last_observation;
00090         unsigned int unique_id;
00091 };
00092 
00093 /******************************************
00094 ** Odometry
00095 *******************************************/
00096 
00097 typedef SlamEkfBase<1,1,1,2> SlidingFilterBase;
00098 
00099 class SlidingOdometryModel : public GenericOdometryModel< SlidingFilterBase > {
00100 public:
00101         SlidingOdometryModel(const double &noise_lvl, SlidingFilterBase &ekf_arrays) : GenericOdometryModel< SlidingFilterBase >(ekf_arrays), noise_level(noise_lvl) {}
00102 
00103 private:
00104         Pose poseUpdate(const Measurement &incoming ) {
00105                 Pose update = incoming;
00106                 return update;
00107         }
00108 
00109         MotionJacobian motionJacobian(const Measurement & /* incoming */) {
00110                 MotionJacobian jacobian;
00111                 jacobian << 1;
00112                 return jacobian;
00113         }
00114 
00115         NoiseJacobian noiseJacobian(const Measurement & /* incoming */) {
00116                 NoiseJacobian noise_jacobian;
00117                 noise_jacobian << 1;
00118                 return noise_jacobian;
00119         }
00120 
00121         Noise noise(const Measurement &incoming) {
00122                 Noise noise_matrix;
00123                 noise_matrix << noise_level*fabs(incoming[0])*noise_level*fabs(incoming[0]);
00124                 return noise_matrix;
00125         }
00126 
00127         double noise_level;
00128 };
00129 
00130 /******************************************
00131 ** LandMarkManager
00132 *******************************************/
00133 
00134 class SlidingObservationModel : public GenericObservationModel< SlidingFilterBase > {
00135 public:
00136         SlidingObservationModel( SlidingFilterBase &ekf_arrays) : GenericObservationModel< SlidingFilterBase >(ekf_arrays) {
00137                 initial_variance = Matrix<double,feature_dimension,feature_dimension>::Zero();
00138                 for ( unsigned int i = 0; i < feature_dimension; ++i ) {
00139                         initial_variance(i,i) = 20;
00140                 }
00141         }
00142         LandMark createLandMark(const Vector2d &initial_state) {
00143                 int correspondence = filter_base.insert(initial_state, initial_variance);
00144                 return LandMark(correspondence);
00145         }
00146         void deleteLandMark(const LandMark& landmark) {
00147                 filter_base.remove(landmark.correspondence());
00148         }
00149 
00150         Observation prediction( const Feature &feature, const Pose &current_pose ) {
00151                 double x_r = current_pose[0];
00152                 Observation pred;
00153                 pred << atan2(feature[1],feature[0]-x_r);
00154                 return pred;
00155         }
00156 
00157         Innovation innovation( const Observation &observation, const Feature &feature, const Pose &current_pose ) {
00158                 return prediction(feature, current_pose)-observation;
00159         }
00160 
00161 private:
00162 
00171         Jacobian observationJacobian( const Feature &feature, const Pose &current_pose ) {
00172                 Jacobian observation_jacobian;
00173                 double z_l = feature[1];
00174                 double dx = feature[0] - current_pose[0];
00175                 double dadxr = z_l/(z_l*z_l+dx*dx);
00176                 double dadxl = -dadxr;
00177                 double dadzl = dx/(z_l*z_l+dx*dx);
00178                 observation_jacobian << dadxr, dadxl, dadzl;
00179                 return observation_jacobian;
00180         }
00181 
00182         Noise noise() {
00183                         static Noise noise_matrix = Noise::Zero();
00184                         static bool noise_initialised = false;
00185                         if ( !noise_initialised ) {
00186                                 noise_matrix << 0.001;
00187                         }
00188                         return noise_matrix;
00189         }
00190 
00191         FeatureVariance initial_variance;
00192 };
00193 
00194 } // namespace demos
00195 } // namespace ecl
00196 
00197 /*****************************************************************************
00198 ** Using
00199 *****************************************************************************/
00200 
00201 using namespace ecl::demos;
00202 
00203 /*****************************************************************************
00204 ** Main
00205 *****************************************************************************/
00206 
00207 int main() {
00208     Format<string> string_format; string_format.width(8); string_format.align(RightAlign);
00209     Format<double> format; format.width(8); format.precision(2); format.align(RightAlign);
00210 
00211     std::cout << std::endl;
00212     std::cout << "***********************************************************" << std::endl;
00213     std::cout << "                   Construction" << std::endl;
00214     std::cout << "***********************************************************" << std::endl;
00215     std::cout << std::endl;
00216 
00217     const unsigned int initial_capacity = 5;
00218     SlidingFilterBase filter_base(initial_capacity);
00219 
00220     SlidingOdometryModel odometry_model(0.1, filter_base);
00221     SlidingObservationModel observation_model(filter_base);
00222 
00223     std::cout << std::endl;
00224     std::cout << "***********************************************************" << std::endl;
00225     std::cout << "                Adding a LandMark" << std::endl;
00226     std::cout << "***********************************************************" << std::endl;
00227     std::cout << std::endl;
00228 
00229     Vector2d initial_state;
00230     initial_state << 0.3, 2.6;
00231     LandMark landmark = observation_model.createLandMark(initial_state);
00232     initial_state << 0.6, 2.6;
00233     LandMark landmark_two = observation_model.createLandMark(initial_state);
00234     initial_state << 0.9, 2.6;
00235     LandMark landmark_three = observation_model.createLandMark(initial_state);
00236 
00237     std::cout << std::endl;
00238     std::cout << "***********************************************************" << std::endl;
00239     std::cout << "                 Deleting a landmark" << std::endl;
00240     std::cout << "***********************************************************" << std::endl;
00241     std::cout << std::endl;
00242 
00243     observation_model.deleteLandMark(landmark_two);
00244 
00245     std::cout << std::endl;
00246     std::cout << "***********************************************************" << std::endl;
00247     std::cout << "                   Update Loop" << std::endl;
00248     std::cout << "***********************************************************" << std::endl;
00249     std::cout << std::endl;
00250 
00251     vector<SlidingObservationModel::Observation> observations;
00252     vector<unsigned int> correspondences;
00253     SlidingOdometryModel::Measurement incoming;
00254     incoming << 0.1;
00255     for ( unsigned int i = 0; i < 10; ++i ) {
00256         observations.clear();
00257         correspondences.clear();
00258         odometry_model.updateFilter(incoming);
00259         if ( i == 4 ) {
00260             std::cout << std::endl;
00261             std::cout << "***********************************************************" << std::endl;
00262             std::cout << "                      Before" << std::endl;
00263             std::cout << "***********************************************************" << std::endl;
00264             std::cout << std::endl;
00265                 SlidingObservationModel::Observation obs;
00266                 obs << 1.65; // actual 1.648
00267                 observations.push_back(obs);
00268                 correspondences.push_back(landmark.correspondence());
00269                 obs << 1.42; // Actual 1.418
00270                 observations.push_back(obs);
00271                 correspondences.push_back(landmark_three.correspondence());
00272                 observation_model.updateFilter(observations, correspondences);
00273             std::cout << std::endl;
00274             std::cout << "***********************************************************" << std::endl;
00275             std::cout << "                      After" << std::endl;
00276             std::cout << "***********************************************************" << std::endl;
00277             std::cout << std::endl;
00278         }
00279     }
00280 
00281     std::cout << std::endl;
00282     std::cout << "***********************************************************" << std::endl;
00283     std::cout << "                      Results" << std::endl;
00284     std::cout << "***********************************************************" << std::endl;
00285     std::cout << std::endl;
00286 
00287     std::cout << "Pose Estimate: " << odometry_model.pose().transpose() << std::endl;
00288 
00289     std::cout << std::endl;
00290 
00291     filter_base.reserve(10);
00292 
00293     std::cout << std::endl;
00294     std::cout << "***********************************************************" << std::endl;
00295     std::cout << "                      Passed" << std::endl;
00296     std::cout << "***********************************************************" << std::endl;
00297     std::cout << std::endl;
00298 
00299     return 0;
00300 }
00301 
00302 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


ecl_navigation_apps
Author(s): Daniel Stonier
autogenerated on Thu Apr 25 2013 15:26:28