PoseParticle.h

Go to the documentation of this file.

#ifndef _POSE_PARTICLE_H_
#define _POSE_PARTICLE_H_

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <string.h>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
class PoseParticle{
                public:
                                Eigen::Affine3d T; 
                                double p;                                       
                                double lik;                             
                                
                                PoseParticle(){
                                                p=0;
                                                lik=0;
                                }
                                PoseParticle(double x, double y, double z, double roll, double pitch, double yaw){
                                        this->set(x,y,z,roll,pitch,yaw);
                                }
                                
                                
                                void set(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);
                                        T = v*m;
                                }
                                void getRPY(double &roll, double &pitch, double &yaw){
                                                Eigen::Vector3d rot = T.rotation().eulerAngles(0,1,2);
                                                roll=rot[0];
                                                pitch=rot[1];
                                                yaw = rot[2];
                                }
                                
                                void getXYZ(double &x, double &y, double &z){
                                        Eigen::Vector3d tr = T.translation();
                                        x = tr[0];
                                        y = tr[1];
                                        z = tr[2];
                                }
                                
                                
                                
                        
                                
};

#endif