PoseParticle.h
Go to the documentation of this file.
00001 
00006 #ifndef _POSE_PARTICLE_H_
00007 #define _POSE_PARTICLE_H_
00008 
00009 #include <stdio.h>
00010 #include <stdlib.h>
00011 #include <math.h>
00012 #include <time.h>
00013 #include <string.h>
00014 #include <vector>
00015 #include <Eigen/Core>
00016 #include <Eigen/Geometry>
00021 class PoseParticle{
00022                 public:
00023                                 Eigen::Affine3d T; 
00024                                 double p;                                       
00025                                 double lik;                             
00026                                 
00027                                 PoseParticle(){
00028                                                 p=0;
00029                                                 lik=0;
00030                                 }
00031                                 PoseParticle(double x, double y, double z, double roll, double pitch, double yaw){
00032                                         this->set(x,y,z,roll,pitch,yaw);
00033                                 }
00034                                 
00035                                 
00039                                 void set(double x, double y, double z, double roll, double pitch, double yaw){
00040                                         Eigen::Matrix3d m;
00041                                         m = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
00042                                                                                                                                 * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
00043                                                                                                                                 * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
00044                                         
00045                                         Eigen::Translation3d v(x,y,z);
00046                                         T = v*m;
00047                                 }
00051                                 void getRPY(double &roll, double &pitch, double &yaw){
00052                                                 Eigen::Vector3d rot = T.rotation().eulerAngles(0,1,2);
00053                                                 roll=rot[0];
00054                                                 pitch=rot[1];
00055                                                 yaw = rot[2];
00056                                 }
00057                                 
00061                                 void getXYZ(double &x, double &y, double &z){
00062                                         Eigen::Vector3d tr = T.translation();
00063                                         x = tr[0];
00064                                         y = tr[1];
00065                                         z = tr[2];
00066                                 }
00067                                 
00068                                 
00069                                 
00070                         
00071                                 
00072 };
00073 
00074 #endif


ndt_mcl
Author(s): jari
autogenerated on Mon Jan 6 2014 11:32:07