ndt_mcl: 3d_ndt_mcl.hpp Source File

Go to the documentation of this file.
00001 #ifndef NDT_MCL_3D_HPP_
00002 #define NDT_MCL_3D_HPP_
00003 #include <mrpt/utils/CTicTac.h> 
00004 #include <pcl/ros/conversions.h>
00005 #include <pcl/point_cloud.h>
00006 #include <pcl/io/pcd_io.h>
00007 #include <pcl/point_types.h>
00008 #include <cstdio>
00009 #include <Eigen/Eigen>
00010 #include <Eigen/Geometry>
00011 #include <fstream>
00012 #include <ndt_map/ndt_map.h>
00013 #include <ndt_map/ndt_cell.h>
00014 #include <pointcloud_vrml/pointcloud_utils.h>
00015 #include "ndt_mcl/ParticleFilter3D.h"
00016 
00020 template <typename PointT>
00021 class NDTMCL3D{
00022         public:
00023                 lslgeneric::NDTMap<PointT> map;                 
00024                 ParticleFilter3D pf;                                            
00025                 double resolution;
00026                 double resolution_sensor;
00027                 int counter;
00028                 double zfilt_min;
00029                 bool forceSIR;
00033                 NDTMCL3D(double map_resolution, lslgeneric::NDTMap<PointT> &nd_map, double zfilter):
00034                         map(new lslgeneric::LazyGrid<PointT>(map_resolution))
00035                 {
00036                                 isInit = false;
00037                                 forceSIR = false;
00038                                 resolution=map_resolution;
00039                                 resolution_sensor = resolution;
00040                                 counter = 0;
00041                                 zfilt_min = zfilter;
00042                                 sinceSIR = 0;
00045                                 double cx,cy,cz;
00046                                 if(!nd_map.getCentroid(cx, cy, cz)){
00047                                         fprintf(stderr,"Centroid NOT Given-abort!\n");
00048                                         exit(1);
00049                                 }else{
00050                                         fprintf(stderr,"Centroid(%lf,%lf,%lf)\n",cx,cy,cz);
00051                                 }
00052                                 
00053                                 double wx,wy,wz;
00054                                 
00055                                 if(!nd_map.getGridSizeInMeters(wx, wy, wz)){
00056                                         fprintf(stderr,"Grid size NOT Given-abort!\n");
00057                                         exit(1);
00058                                 }else{
00059                                         fprintf(stderr,"GridSize(%lf,%lf,%lf)\n",wx,wy,wz);
00060                                 }
00061                                 
00062                                 map.initialize(cx,cy,cz,wx,wy,wz);
00063                                 
00064                                 std::vector<lslgeneric::NDTCell<PointT>*> ndts;
00065                                 ndts = nd_map.getAllCells();
00066                                 fprintf(stderr,"NDT MAP with %d components",ndts.size());
00067                                 for(unsigned int i=0;i<ndts.size();i++){
00068                                         Eigen::Vector3d m = ndts[i]->getMean(); 
00069                                         if(m[2]>zfilter){
00070                                                 Eigen::Matrix3d cov = ndts[i]->getCov();
00071                                                 unsigned int nump = ndts[i]->getN();
00072                                                 //fprintf(stderr,"-%d-",nump);
00073                                                 map.addDistributionToCell(cov, m,nump);
00074                                         }
00075                                 }
00076                 }
00077                 
00082                 void initializeFilter(double x, double y, double z, double roll, double pitch, double yaw,
00083                                                                                                         double x_e, double y_e, double z_e, double roll_e, double pitch_e, double yaw_e, 
00084                                                                                                         unsigned int numParticles)
00085                 {
00086                         pf.initializeNormalRandom(numParticles, x,y,z,roll,pitch, yaw, x_e,y_e,z_e,roll_e,pitch_e,yaw_e);
00087                 }
00094                 
00095                 void updateAndPredict(Eigen::Affine3d Tmotion, pcl::PointCloud<PointT> &cloud){
00096                         Eigen::Vector3d tr = Tmotion.translation();
00097                         Eigen::Vector3d rot = Tmotion.rotation().eulerAngles(0,1,2);
00098                         
00099                         mrpt::utils::CTicTac    tictac;
00100                         tictac.Tic();   
00101                         
00102                         pf.predict(Tmotion, tr[0]*0.1, tr[1]*0.1, tr[2]*0.1, rot[0]*0.1, rot[1]*0.1, rot[2]*0.1);
00103                         double t_pred = tictac.Tac();   
00104                         
00105                         
00106                         //pf.predict(mcl::pose(tr[0],tr[1],rot[2]), mcl::pose(tr[0]*0.1 + 0.005,tr[1]*0.1+ 0.005,rot[2]*0.1+0.001));
00107 
00108                         lslgeneric::NDTMap<PointT> local_map(new lslgeneric::LazyGrid<PointT>(resolution_sensor));
00109                         
00110                         local_map.addPointCloudSimple(cloud);
00111                         local_map.computeNDTCells();
00112                         //local_map.computeNDTCells(CELL_UPDATE_MODE_STUDENT_T);
00113                         int Nn = 0;
00114 //              #pragma omp parallel for
00115                         double t_pseudo = 0;
00116 
00117                         for(int i=0;i<pf.size();i++){
00118                                 Eigen::Affine3d T = pf.pcloud[i].T;
00119                                 
00120                                 std::vector<lslgeneric::NDTCell<PointT>*> ndts;
00121                                 tictac.Tic();   
00122                                 ndts = local_map.pseudoTransformNDT(T);
00123                                 t_pseudo += tictac.Tac();
00124                                 double score=1;
00125                                 
00126                                 if(ndts.size()==0) fprintf(stderr,"ERROR no gaussians in measurement!!!\n");
00127                                 Nn = ndts.size();
00128                                 
00129                                 for(int n=0;n<ndts.size();n++){
00130                                         Eigen::Vector3d m = ndts[n]->getMean(); 
00131                                         if(m[2]<zfilt_min) continue;
00132                                 
00133                                         lslgeneric::NDTCell<PointT> *cell;
00134                                         PointT p;
00135                                         p.x = m[0];p.y=m[1];p.z=m[2];
00136                                         
00137                                         if(map.getCellAtPoint(p,cell)){
00138                                         //if(map.getCellForPoint(p,cell)){
00139                                                 if(cell == NULL) continue;
00140                                                 if(cell->hasGaussian_){
00141                                                         Eigen::Matrix3d covCombined = cell->getCov() + ndts[n]->getCov();
00142                                                         Eigen::Matrix3d icov;
00143                                                         bool exists;
00144                                                         double det = 0;
00145                                                         covCombined.computeInverseAndDetWithCheck(icov,det,exists);
00146                                                         if(!exists) continue;
00147                                                         double l = (cell->getMean() - m).dot(icov*(cell->getMean() - m));
00148                                                         if(l*0 != 0) continue;
00149                                                         score += 0.1 + 0.9 * exp(-0.05*l/2.0);
00150                                                 }else{
00151                                                 }
00152                                         }
00153                                 }
00154                                 
00155                                 pf.pcloud[i].lik = score;
00156                                 for(unsigned int j=0;j<ndts.size();j++){
00157                                         delete ndts[j];
00158                                 }
00159                                 
00160                         }
00161                         
00162                         pf.normalize();
00163                         
00164 
00165                         if(forceSIR){   
00166                                 fprintf(stderr, "forceSIR(%d) ",forceSIR);
00167                                 pf.SIRUpdate();
00168                         }else{
00169                         
00170                                 double varP=0;
00171                                 for(int i = 0; i<pf.size();i++){
00172                                         varP += (pf.pcloud[i].p - 1.0/pf.size())*(pf.pcloud[i].p - 1.0/pf.size());
00173                                 }
00174                                 varP /= pf.size();
00175                                 varP = sqrt(varP); 
00176                                 fprintf(stderr,"Var P=%lf (Npf=%d, Nm=%d) (t_pred = %.3lf t_pseudo=%.3lf)",varP,pf.size(), Nn, t_pred,t_pseudo);
00177                                 if(varP > 0.006 || sinceSIR >25){
00178                                         fprintf(stderr,"-SIR- ");
00179                                         sinceSIR = 0;
00180                                         pf.SIRUpdate();
00181                                 }else{
00182                                         sinceSIR++;
00183                                 }
00184                                 
00185                         }
00186                 }
00187                 
00194                         
00195                 void updateAndPredictEff(Eigen::Affine3d Tmotion, pcl::PointCloud<PointT> &cloud){
00196                         Eigen::Vector3d tr = Tmotion.translation();
00197                         Eigen::Vector3d rot = Tmotion.rotation().eulerAngles(0,1,2);
00198                         
00199                         mrpt::utils::CTicTac    tictac;
00200                         tictac.Tic();   
00201                         
00202                         //pf.predict(Tmotion, tr[0]*0.1, tr[1]*0.1, tr[2]*0.1, rot[0]*0.1, rot[1]*0.1, rot[2]*0.1);
00203                         if(rot[2]<(0.5 * M_PI/180.0) && tr[0]>=0){ 
00204                                 pf.predict(Tmotion, tr[0]*0.2 + 0.005, tr[1]*0.1+ 0.005, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.2+0.001);
00205                         }else if(tr[0]>=0){
00206                                 pf.predict(Tmotion,tr[0]*0.5 + 0.005, tr[1]*0.1+ 0.005, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.4+0.001);
00207                         }else{
00208                                 pf.predict(Tmotion, tr[0]*0.2 + 0.005, tr[1]*0.1+ 0.005, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.2+0.001);
00209                         }
00210                         
00211                         
00212                         double t_pred = tictac.Tac();   
00213         
00214                         lslgeneric::NDTMap<PointT> local_map(new lslgeneric::LazyGrid<PointT>(resolution_sensor));              
00215                         local_map.addPointCloudSimple(cloud);
00216                         //local_map.computeNDTCells();
00217                         local_map.computeNDTCellsSimple();
00218                         
00219                         std::vector<lslgeneric::NDTCell<PointT>*> ndts = local_map.getAllCells();
00220                         
00221                         
00222                         
00223                         int Nn = 0;
00224 //              #pragma omp parallel for
00225                         double t_pseudo = 0;
00226                 #pragma omp parallel num_threads(4)
00227     {
00228      #pragma omp for
00229                         for(int i=0;i<pf.size();i++){
00230                                 Eigen::Affine3d T = pf.pcloud[i].T;
00231                                 
00232                                 
00233                                 //tictac.Tic(); 
00234                                 //ndts = local_map.pseudoTransformNDT(T);
00235                                 //t_pseudo += tictac.Tac();
00236                                 double score=1;
00237                                 
00238                                 if(ndts.size()==0) fprintf(stderr,"ERROR no gaussians in measurement!!!\n");
00239                                 Nn = ndts.size();
00240                                 
00241                                 for(int n=0;n<ndts.size();n++){
00242                                         Eigen::Vector3d m = T*ndts[n]->getMean();       
00243                                         
00244                                         if(m[2]<zfilt_min) continue;
00245                                 
00246                                         lslgeneric::NDTCell<PointT> *cell;
00247                                         PointT p;
00248                                         p.x = m[0];p.y=m[1];p.z=m[2];
00249                                         
00250                                         if(map.getCellAtPoint(p,cell)){
00251                                         //if(map.getCellForPoint(p,cell)){
00252                                                 if(cell == NULL) continue;
00253                                                 if(cell->hasGaussian_){
00254                                                         Eigen::Matrix3d covCombined = cell->getCov() + T.rotation()*ndts[n]->getCov() *T.rotation().transpose();
00255                                                         Eigen::Matrix3d icov;
00256                                                         bool exists;
00257                                                         double det = 0;
00258                                                         covCombined.computeInverseAndDetWithCheck(icov,det,exists);
00259                                                         if(!exists) continue;
00260                                                         double l = (cell->getMean() - m).dot(icov*(cell->getMean() - m));
00261                                                         if(l*0 != 0) continue;
00262                                                         score += 0.1 + 0.9 * exp(-0.05*l/2.0);
00263                                                 }else{
00264                                                 }
00265                                         }
00266                                 }
00267                                 
00268                                 pf.pcloud[i].lik = score;
00269                                 
00270                                 
00271                         }
00272                 }
00273                         for(unsigned int j=0;j<ndts.size();j++){
00274                                         delete ndts[j];
00275                         }
00276                         
00277                         
00278                         pf.normalize();
00279                         
00280 
00281                         if(forceSIR){   
00282                                 fprintf(stderr, "forceSIR(%d) ",forceSIR);
00283                                 pf.SIRUpdate();
00284                         }else{
00285                         
00286                                 double varP=0;
00287                                 for(int i = 0; i<pf.size();i++){
00288                                         varP += (pf.pcloud[i].p - 1.0/pf.size())*(pf.pcloud[i].p - 1.0/pf.size());
00289                                 }
00290                                 varP /= pf.size();
00291                                 varP = sqrt(varP); 
00292                                 fprintf(stderr,"Var P=%lf (Npf=%d, Nm=%d) (t_pred = %.3lf t_pseudo=%.3lf)",varP,pf.size(), Nn, t_pred,t_pseudo);
00293                                 if(varP > 0.006 || sinceSIR >25){
00294                                         fprintf(stderr,"-SIR- ");
00295                                         sinceSIR = 0;
00296                                         pf.SIRUpdate();
00297                                 }else{
00298                                         sinceSIR++;
00299                                 }
00300                                 
00301                         }
00302                 }
00303                 
00310                 Eigen::Vector3d getMeanVector(){
00311                         return (pf.getMean().translation());
00312                         //mcl::pose m = pf.getDistributionMean(true);
00313                         //Eigen::Vector3d mm; mm<<m.x,m.y,m.a;
00314                         //return mm;
00315                 }
00316                 Eigen::Affine3d getMean(){
00317                         return (pf.getMean());
00318                         //mcl::pose m = pf.getDistributionMean(true);
00319                         //Eigen::Vector3d mm; mm<<m.x,m.y,m.a;
00320                         //return mm;
00321                 }
00322                 
00323                 
00324         private:
00325                 bool isInit;
00326                 int sinceSIR;
00327                 /*
00328                 Eigen::Affine3d getAsAffine(int i){
00329                         Eigen::Matrix3d m;
00330                         m = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
00331                                         * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
00332                                         * Eigen::AngleAxisd(pf.Particles[i].a, Eigen::Vector3d::UnitZ());
00333                         Eigen::Translation3d v(pf.Particles[i].x,pf.Particles[i].y,0);
00334                         Eigen::Affine3d T = v*m;
00335                         return T;
00336                 }*/
00337                 
00338 };
00339 
00340 #endif
00341