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