ndt_mcl: 3d_ndt_mcl.cpp Source File

Go to the documentation of this file.
00001 #include <ndt_mcl/3d_ndt_mcl.h>
00002 
00003 void NDTMCL3D::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     double t_start = getDoubleTime();
00008     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);
00009     double t_pred = getDoubleTime() - t_start;  
00010 
00011     //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));
00012 
00013     lslgeneric::NDTMap local_map(new lslgeneric::LazyGrid(resolution_sensor));
00014     std::cerr<<"cloud points "<<cloud.points.size()<<std::endl;
00015     local_map.addPointCloudSimple(cloud);
00016     local_map.computeNDTCells();
00017     //local_map.computeNDTCells(CELL_UPDATE_MODE_STUDENT_T);
00018     int Nn = 0;
00019     //          #pragma omp parallel for
00020     double t_pseudo = 0;
00021 
00022     for(int i=0;i<pf.size();i++){
00023         Eigen::Affine3d T = pf.pcloud[i].T;
00024 
00025         std::vector<lslgeneric::NDTCell*> ndts;
00026         double tictime = getDoubleTime();
00027         ndts = local_map.pseudoTransformNDT(T);
00028         t_pseudo += getDoubleTime()-tictime;
00029         double score=1;
00030 
00031         if(ndts.size()==0) fprintf(stderr,"ERROR no gaussians in measurement!!!\n");
00032         Nn = ndts.size();
00033 
00034         for(int n=0;n<ndts.size();n++){
00035             Eigen::Vector3d m = ndts[n]->getMean();     
00036             if(m[2]<zfilt_min) continue;
00037 
00038             lslgeneric::NDTCell *cell;
00039             pcl::PointXYZ p;
00040             p.x = m[0];p.y=m[1];p.z=m[2];
00041 
00042             if(map.getCellAtPoint(p,cell)){
00043                 //if(map.getCellForPoint(p,cell)){
00044                 if(cell == NULL) continue;
00045                 if(cell->hasGaussian_){
00046                     Eigen::Matrix3d covCombined = cell->getCov() + ndts[n]->getCov();
00047                     Eigen::Matrix3d icov;
00048                     bool exists;
00049                     double det = 0;
00050                     covCombined.computeInverseAndDetWithCheck(icov,det,exists);
00051                     if(!exists) continue;
00052                     double l = (cell->getMean() - m).dot(icov*(cell->getMean() - m));
00053                     if(l*0 != 0) continue;
00054                     score += 0.1 + 0.9 * exp(-0.05*l/2.0);
00055                 }else{
00056                 }
00057             }
00058             }
00059 
00060             pf.pcloud[i].lik = score;
00061             for(unsigned int j=0;j<ndts.size();j++){
00062                 delete ndts[j];
00063             }
00064 
00065         }
00066 
00067         pf.normalize();
00068 
00069 
00070         if(forceSIR){   
00071             fprintf(stderr, "forceSIR(%d) ",forceSIR);
00072             pf.SIRUpdate();
00073         }else{
00074 
00075             double varP=0;
00076             for(int i = 0; i<pf.size();i++){
00077                 varP += (pf.pcloud[i].p - 1.0/pf.size())*(pf.pcloud[i].p - 1.0/pf.size());
00078             }
00079             varP /= pf.size();
00080             varP = sqrt(varP); 
00081             fprintf(stderr,"Var P=%lf (Npf=%d, Nm=%d) (t_pred = %.3lf t_pseudo=%.3lf)",varP,pf.size(), Nn, t_pred,t_pseudo);
00082             if(varP > 0.006 || sinceSIR >25){
00083                 fprintf(stderr,"-SIR- ");
00084                 sinceSIR = 0;
00085                 pf.SIRUpdate();
00086             }else{
00087                 sinceSIR++;
00088             }
00089 
00090         }
00091     }
00092 
00093 
00094 void NDTMCL3D::updateAndPredictEff(Eigen::Affine3d Tmotion, pcl::PointCloud<pcl::PointXYZ> &cloud, double subsample_level){
00095     if(subsample_level < 0 || subsample_level > 1) subsample_level = 1;
00096 
00097     Eigen::Vector3d tr = Tmotion.translation();
00098     Eigen::Vector3d rot = Tmotion.rotation().eulerAngles(0,1,2);
00099 
00100     double time_start = getDoubleTime();
00101     //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);
00102     if(rot[2]<(0.5 * M_PI/180.0) && tr[0]>=0){ 
00103         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);
00104     }else if(tr[0]>=0){
00105         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);
00106     }else{
00107         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);
00108     }
00109 
00110 
00111     double t_pred = getDoubleTime() - time_start;       
00112 
00113     std::cerr<<"cloud points "<<cloud.points.size()<<" res :"<<resolution<<" sres: "<<resolution_sensor<<std::endl;
00114     lslgeneric::NDTMap local_map(new lslgeneric::LazyGrid(resolution));
00115     //local_map.guessSize(0,0,0,30,30,10); //sensor_range,sensor_range,map_size_z);
00116     local_map.loadPointCloud(cloud);//,30); //sensor_range);
00117     local_map.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
00118 
00119     /*lslgeneric::NDTMap<PointT> local_map(new lslgeneric::LazyGrid<PointT>(resolution_sensor));                
00120       local_map.addPointCloudSimple(cloud);
00121     //local_map.computeNDTCells();
00122     local_map.computeNDTCellsSimple();
00123      */
00124     std::vector<lslgeneric::NDTCell*> ndts0 = local_map.getAllCells();
00125     std::vector<lslgeneric::NDTCell*> ndts;
00126     std::cerr<<"ndts: "<<ndts0.size()<<std::endl;
00127 
00128     if(subsample_level != 1) {
00129         srand((int)(t_pred*10000));
00130         for(int i=0; i<ndts0.size(); ++i) {
00131             double p = ((double)rand())/RAND_MAX;
00132             if(p < subsample_level) {
00133                 ndts.push_back(ndts0[i]);
00134             } else {
00135                 delete ndts0[i];
00136             }
00137         }       
00138     } else {
00139         ndts = ndts0;
00140     }
00141     std::cerr<<"resampled ndts: "<<ndts.size()<<std::endl;
00142 
00143     int Nn = 0;
00144     //          #pragma omp parallel for
00145     double t_pseudo = 0;
00146 #pragma omp parallel num_threads(4)
00147     {
00148 #pragma omp for
00149         for(int i=0;i<pf.size();i++){
00150             Eigen::Affine3d T = pf.pcloud[i].T;
00151 
00152 
00153             //ndts = local_map.pseudoTransformNDT(T);
00154             double score=1;
00155 
00156             if(ndts.size()==0) fprintf(stderr,"ERROR no gaussians in measurement!!!\n");
00157             Nn = ndts.size();
00158 
00159             for(int n=0;n<ndts.size();n++){
00160                 Eigen::Vector3d m = T*ndts[n]->getMean();       
00161 
00162                 if(m[2]<zfilt_min) continue;
00163 
00164                 lslgeneric::NDTCell *cell;
00165                 pcl::PointXYZ p;
00166                 p.x = m[0];p.y=m[1];p.z=m[2];
00167 
00168                 if(map.getCellAtPoint(p,cell)){
00169                     //if(map.getCellForPoint(p,cell)){
00170                     if(cell == NULL) continue;
00171                     if(cell->hasGaussian_){
00172                         Eigen::Matrix3d covCombined = cell->getCov() + T.rotation()*ndts[n]->getCov() *T.rotation().transpose();
00173                         Eigen::Matrix3d icov;
00174                         bool exists;
00175                         double det = 0;
00176                         covCombined.computeInverseAndDetWithCheck(icov,det,exists);
00177                         if(!exists) continue;
00178                         double l = (cell->getMean() - m).dot(icov*(cell->getMean() - m));
00179                         if(l*0 != 0) continue;
00180                         score += 0.1 + 0.9 * exp(-0.05*l/2.0);
00181                     }else{
00182                     }
00183                 }
00184                 }
00185 
00186                 pf.pcloud[i].lik = score;
00187 
00188 
00189             }
00190         }
00191         for(unsigned int j=0;j<ndts.size();j++){
00192             delete ndts[j];
00193         }
00194 
00195 
00196         pf.normalize();
00197 
00198 
00199         if(forceSIR){   
00200             fprintf(stderr, "forceSIR(%d) ",forceSIR);
00201             pf.SIRUpdate();
00202         }else{
00203 
00204             double varP=0;
00205             for(int i = 0; i<pf.size();i++){
00206                 varP += (pf.pcloud[i].p - 1.0/pf.size())*(pf.pcloud[i].p - 1.0/pf.size());
00207             }
00208             varP /= pf.size();
00209             varP = sqrt(varP); 
00210             fprintf(stderr,"Var P=%lf (Npf=%d, Nm=%d) (t_pred = %.3lf t_pseudo=%.3lf)",varP,pf.size(), Nn, t_pred,t_pseudo);
00211             if(varP > 0.006 || sinceSIR >25){
00212                 fprintf(stderr,"-SIR- ");
00213                 sinceSIR = 0;
00214                 pf.SIRUpdate();
00215             }else{
00216                 sinceSIR++;
00217             }
00218 
00219         }
00220     }
00221