ParticleFilter3D.h

Go to the documentation of this file.

#ifndef _Particle_filter_3D_h_
#define _Particle_filter_3D_h_
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <string.h>
#include <vector>
#include <Eigen/Core>
#include "ndt_mcl/PoseParticle.h" 
#include "ndt_mcl/ownRandom.h"


class ParticleFilter3D{
    public:
        std::vector<PoseParticle> pcloud;             
        ownRandom myrand;                                       

        ParticleFilter3D(){}

        void initializeNormalRandom(unsigned int NumParticles, double mx, double my, double mz, double mroll, double mpitch, double myaw,
                double vx, double vy, double vz, double vroll, double vpitch, double vyaw);

        void SIRUpdate();

        unsigned int size(){return pcloud.size();}

        void normalize();

        void predict(Eigen::Affine3d Tmotion, double vx, double vy, double vz, double vroll, double vpitch, double vyaw);

        Eigen::Affine3d getMean();
        //Eigen::Matrix<double,7,7> getCov();



        inline Eigen::Affine3d xyzrpy2affine(double x, double y, double z, double roll, double pitch, double yaw){
            Eigen::Matrix3d m;
            m = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
                * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
                * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());

            Eigen::Translation3d v(x,y,z);
            return (v*m);
        }



    private:


};


#endif