Go to the documentation of this file.00001
00006 #ifndef _Particle_filter_3D_h_
00007 #define _Particle_filter_3D_h_
00008 #include <stdio.h>
00009 #include <stdlib.h>
00010 #include <math.h>
00011 #include <time.h>
00012 #include <string.h>
00013 #include <vector>
00014 #include <Eigen/Core>
00015 #include "ndt_mcl/PoseParticle.h"
00016 #include "ndt_mcl/ownRandom.h"
00017
00018
00019 class ParticleFilter3D{
00020 public:
00021 std::vector<PoseParticle> pcloud;
00022 ownRandom myrand;
00023
00024 ParticleFilter3D(){}
00025
00030 void initializeNormalRandom(unsigned int NumParticles, double mx, double my, double mz, double mroll, double mpitch, double myaw,
00031 double vx, double vy, double vz, double vroll, double vpitch, double vyaw);
00032
00036 void SIRUpdate();
00037
00038 unsigned int size(){return pcloud.size();}
00039
00045 void normalize();
00046
00047 void predict(Eigen::Affine3d Tmotion, double vx, double vy, double vz, double vroll, double vpitch, double vyaw);
00048
00049 Eigen::Affine3d getMean();
00050
00051
00052
00056 inline Eigen::Affine3d xyzrpy2affine(double x, double y, double z, double roll, double pitch, double yaw){
00057 Eigen::Matrix3d m;
00058 m = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
00059 * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
00060 * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
00061
00062 Eigen::Translation3d v(x,y,z);
00063 return (v*m);
00064 }
00065
00066
00067
00068 private:
00069
00070
00071 };
00072
00073 #include "ndt_mcl/impl/ParticleFilter3D.hpp"
00074
00075 #endif