ndt_mcl: ndt_mcl.cpp Source File

Go to the documentation of this file.
00001 #include <ndt_mcl/ndt_mcl.h>
00002 
00003 void NDTMCL::updateAndPredict(Eigen::Affine3d Tmotion, pcl::PointCloud<pcl::PointXYZ> &cloud){
00004     Eigen::Vector3d tr = Tmotion.translation();
00005     Eigen::Vector3d rot = Tmotion.rotation().eulerAngles(0,1,2);
00006 
00007 
00017     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));
00018 
00019     lslgeneric::NDTMap local_map(new lslgeneric::LazyGrid(resolution));
00020 
00021     /*
00022        pcl::PointCloud<PointT> cl_f;
00023        pcl::PointCloud<PointT> cl_z;
00024        for(int i=0;i<cloud.size();i++){
00025        if(cloud.points[i].z > 1.95 && cloud.points[i].z <2.05 ) cl_z.push_back(cloud.points[i]);
00026        if(cloud.points[i].z > zfilt_min ) cl_f.push_back(cloud.points[i]);
00027        }
00028 
00029 
00030        fprintf(stderr,"2D scan = %d Points\n",cl_z.size());
00031 
00032      */
00033     //fprintf(stderr,"Could = %d Points\n",cloud.size());
00034     local_map.addPointCloudSimple(cloud);
00035     //local_map.computeNDTCells();
00036     local_map.computeNDTCells(CELL_UPDATE_MODE_STUDENT_T);
00037     int Nn = 0;
00038     //                  #pragma omp parallel for
00039     for(int i=0;i<pf.NumOfParticles;i++){
00040         Eigen::Affine3d T = getAsAffine(i);
00041 
00042         std::vector<lslgeneric::NDTCell*> ndts;
00043         ndts = local_map.pseudoTransformNDT(T);
00044         double score=1;
00045 
00046         if(ndts.size()==0) fprintf(stderr,"ERROR no gaussians in measurement!!!\n");
00047         Nn = ndts.size();
00048         for(int n=0;n<ndts.size();n++){
00049             Eigen::Vector3d m = ndts[n]->getMean();     
00050             if(m[2]<zfilt_min) continue;
00051 
00052             lslgeneric::NDTCell *cell;
00053             pcl::PointXYZ p;
00054             p.x = m[0];p.y=m[1];p.z=m[2];
00055 
00056             //if(map.getCellAtPoint(p,cell)){
00057             if(map.getCellForPoint(p,cell)){
00058                 if(cell == NULL) continue;
00059                 if(cell->hasGaussian_){
00060                     Eigen::Matrix3d covCombined = cell->getCov() + ndts[n]->getCov();
00061                     Eigen::Matrix3d icov;
00062                     bool exists;
00063                     double det = 0;
00064                     covCombined.computeInverseAndDetWithCheck(icov,det,exists);
00065                     if(!exists) continue;
00066                     double l = (cell->getMean() - m).dot(icov*(cell->getMean() - m));
00067                     if(l*0 != 0) continue;
00068                     //if(l > 120) continue;
00069                     score += 0.1 + 0.9 * exp(-0.05*l/2.0);
00070                 }else{
00071                 }
00072             }
00073         }
00074 
00075         /*  -lfd1*(exp(-lfd2*l/2));*/
00076 
00077         pf.Particles[i].lik = score;
00078         for(unsigned int j=0;j<ndts.size();j++){
00079             delete ndts[j];
00080         }
00081 
00082         }
00083 
00084         pf.normalize();
00085 
00086 
00087         if(forceSIR){
00088 
00089             fprintf(stderr, "forceSIR(%d) ",forceSIR);
00090             pf.SIRUpdate();
00091         }else{
00092 
00093             double varP=0;
00094             for(int i = 0; i<pf.NumOfParticles;i++){
00095                 varP += (pf.Particles[i].p - 1.0/pf.NumOfParticles)*(pf.Particles[i].p - 1.0/pf.NumOfParticles);
00096             }
00097             varP /= pf.NumOfParticles;
00098             varP = sqrt(varP); 
00099             fprintf(stderr,"Var P=%lf (Npf=%d, Nm=%d)",varP,pf.NumOfParticles, Nn);
00100             if(varP > 0.006 || sinceSIR >25){
00101                 fprintf(stderr,"-SIR- ");
00102                 sinceSIR = 0;
00103                 pf.SIRUpdate();
00104             }else{
00105                 sinceSIR++;
00106             }
00107 
00108         }
00109     }