ndt_fuser: ndt_fuser_hmt.cpp Source File

Go to the documentation of this file.
00001 #include <ndt_fuser/ndt_fuser_hmt.h>
00002 
00003 namespace lslgeneric {
00004     void NDTFuserHMT::initialize(Eigen::Affine3d initPos, pcl::PointCloud<pcl::PointXYZ> &cloud, bool preLoad)
00005     {
00007         lslgeneric::transformPointCloudInPlace(sensor_pose, cloud);
00008         lslgeneric::transformPointCloudInPlace(initPos, cloud);
00009         Tnow = initPos;
00010         //#ifdef BASELINE
00011         //#else
00012         if(beHMT) {
00013             map = new lslgeneric::NDTMapHMT(resolution,
00014                     Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2),
00015                     map_size_x,map_size_y,map_size_z,sensor_range,hmt_map_dir,true);
00016             if(preLoad) {
00017                 lslgeneric::NDTMapHMT *map_hmt = dynamic_cast<lslgeneric::NDTMapHMT*> (map);
00018                 std::cout<<"Trying to pre-load maps at "<<initPos.translation()<<std::endl;
00019                 map_hmt->tryLoadPosition(initPos.translation());
00020             }
00021         } else {
00022             map = new lslgeneric::NDTMap(new lslgeneric::LazyGrid(resolution));
00023             if(preLoad) {
00024                 char fname[1000];
00025                 snprintf(fname,999,"%s/%s_map.jff",hmt_map_dir.c_str(),prefix.c_str());
00026                 std::cerr<<"Loading "<<fname<<std::endl;
00027                 map->loadFromJFF(fname);
00028             } else {
00029                 map = new lslgeneric::NDTMap(new lslgeneric::LazyGrid(resolution));
00030                 map->initialize(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2),map_size_x,map_size_y,map_size_z);
00031             }
00032         }
00033         //#endif
00034         map->addPointCloud(Tnow.translation(),cloud, 0.1, 100.0, 0.1);
00035         //map->addPointCloudMeanUpdate(Tnow.translation(),cloud,localMapSize, 1e5, 1250, map_size_z/2, 0.06);
00036         //map->addPointCloudMeanUpdate(Tnow.translation(),cloud,localMapSize, 0.1, 100.0, 0.1);
00037         map->computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE, 1e5, 255, Tnow.translation(), 0.1);
00038         isInit = true;
00039         Tlast_fuse = Tnow;
00040         Todom = Tnow;
00041         if(visualize) 
00042         {
00043 #ifndef NO_NDT_VIZ
00044       //      # error compiling with visualization
00045             viewer->plotNDTSAccordingToOccupancy(-1,map); 
00046             //viewer->plotLocalNDTMap(cloud,resolution);
00047             viewer->addTrajectoryPoint(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+0.5,1,0,0);
00048             viewer->addTrajectoryPoint(Todom.translation()(0),Todom.translation()(1),Todom.translation()(2)+0.5,0,1,0);
00049             viewer->displayTrajectory();
00050             viewer->setCameraPointing(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+3);
00051             viewer->repaint();  
00052 #endif
00053         }
00054     }
00055 
00060     Eigen::Affine3d NDTFuserHMT::update(Eigen::Affine3d Tmotion, pcl::PointCloud<pcl::PointXYZ> &cloud)
00061     {
00062         if(!isInit){
00063             fprintf(stderr,"NDT-FuserHMT: Call Initialize first!!\n");
00064             return Tnow;
00065         }
00066         Todom = Todom * Tmotion; //we track this only for display purposes!
00067         double t0=0,t1=0,t2=0,t3=0,t4=0,t5=0,t6=0;
00069         lslgeneric::transformPointCloudInPlace(sensor_pose, cloud);
00070         t0 = getDoubleTime();
00072         lslgeneric::NDTMap ndlocal(new lslgeneric::LazyGrid(resolution));
00073         ndlocal.guessSize(0,0,0,sensor_range,sensor_range,map_size_z);
00074         ndlocal.loadPointCloud(cloud,sensor_range);
00075         ndlocal.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
00076         //pass through ndlocal and set all cells with vertically pointing normals to non-gaussian :-O
00077         /*SpatialIndex *index = ndlocal.getMyIndex();
00078           typename SpatialIndexctorItr it = index->begin();
00079           while (it != index->end())
00080           {
00081           NDTCell *cell = dynamic_cast<NDTCell*> (*it);
00082           if(cell!=NULL)
00083           {
00084           if(cell->hasGaussian_)
00085           {
00086           if(cell->getClass() == NDTCell::HORIZONTAL) {
00087           cell->hasGaussian_ = false;
00088           }
00089           }
00090           }
00091           it++;
00092           }*/
00093 
00094         t1 = getDoubleTime();
00095         Eigen::Affine3d Tinit = Tnow * Tmotion;
00096         if(disableRegistration) {
00097             Tnow = Tinit;
00098             lslgeneric::transformPointCloudInPlace(Tnow, cloud);
00099             Eigen::Affine3d spose = Tnow*sensor_pose;
00100             map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 25, 2*map_size_z, 0.06);
00101             if(visualize) //&&ctr%20==0) 
00102             {
00103 #ifndef NO_NDT_VIZ
00104                 if(ctr%50==0) {
00105 
00106                     viewer->plotNDTSAccordingToOccupancy(-1,map); 
00107                     //viewer->plotLocalNDTMap(cloud,resolution); 
00108                 }
00109                 viewer->addTrajectoryPoint(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+0.2,0,1,0);
00110                 viewer->addTrajectoryPoint(Todom.translation()(0),Todom.translation()(1),Todom.translation()(2)+0.2,0.5,0,0.5);
00111                 viewer->displayTrajectory();
00112                 viewer->setCameraPointing(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+3);
00113                 viewer->repaint();      
00114 #endif
00115             }
00116             ctr++;
00117             return Tnow;
00118         }
00119 
00120         if(doMultires) {
00121             //create two ndt maps with resolution = 3*resolution (or 5?)
00122             lslgeneric::NDTMap ndlocalLow(new lslgeneric::LazyGrid(3*resolution));
00123             ndlocalLow.guessSize(0,0,0,sensor_range,sensor_range,map_size_z);
00124             ndlocalLow.loadPointCloud(cloud,sensor_range);
00125             ndlocalLow.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
00126 
00127             lslgeneric::NDTMap mapLow(new lslgeneric::LazyGrid(3*resolution));
00128             //add distros
00129             double cx,cy,cz;
00130             if(!map->getCentroid(cx, cy, cz)){
00131                 fprintf(stderr,"Centroid NOT Given-abort!\n");
00132             }
00133             mapLow.initialize(cx,cy,cz,3*map_size_x,3*map_size_y,map_size_z);
00134 
00135             std::vector<lslgeneric::NDTCell*> ndts;
00136             ndts = map->getAllCells(); //this copies cells?
00137 
00138             for(int i=0; i<ndts.size(); i++)    
00139             {
00140                 NDTCell *cell = ndts[i];
00141                 if(cell!=NULL)
00142                 {
00143                     if(cell->hasGaussian_)
00144                     {
00145                         Eigen::Vector3d m = cell->getMean();    
00146                         Eigen::Matrix3d cov = cell->getCov();
00147                         unsigned int nump = cell->getN();
00148                         mapLow.addDistributionToCell(cov, m,nump);
00149                     }
00150                 }
00151                 delete cell;
00152             }
00153             //do match
00154             if(matcher2D.match( mapLow, ndlocalLow,Tinit,true)){
00155                 //if success, set Tmotion to result
00156                 t2 = getDoubleTime();
00157                 //std::cout<<"success: new initial guess! t= "<<t2-t1<<std::endl;
00158             } else {
00159                 Tinit = Tnow * Tmotion;
00160             }       
00161         }
00162 
00163         if(be2D) {
00164             t2 = getDoubleTime();
00165             if(matcher2D.match( *map, ndlocal,Tinit,true) || fuseIncomplete){
00166                 t3 = getDoubleTime();
00167                 Eigen::Affine3d diff = (Tnow * Tmotion).inverse() * Tinit;
00168                 if((diff.translation().norm() > max_translation_norm || 
00169                             diff.rotation().eulerAngles(0,1,2).norm() > max_rotation_norm) && checkConsistency){
00170                     fprintf(stderr,"****  NDTFuserHMT -- ALMOST DEFINATELY A REGISTRATION FAILURE *****\n");
00171                     Tnow = Tnow * Tmotion;
00172                 }else{
00173                     Tnow = Tinit;
00174                     lslgeneric::transformPointCloudInPlace(Tnow, cloud);
00175                     Eigen::Affine3d spose = Tnow*sensor_pose;
00176                     Eigen::Affine3d diff_fuse = Tlast_fuse.inverse()*Tnow;
00177                     if(diff_fuse.translation().norm() > translation_fuse_delta ||
00178                             diff_fuse.rotation().eulerAngles(0,1,2).norm() > rotation_fuse_delta)
00179                     {
00180                         //std::cout<<"F: "<<spose.translation().transpose()<<" "<<spose.rotation().eulerAngles(0,1,2).transpose()<<std::endl;
00181                         t4 = getDoubleTime();
00182                         //TSV: originally this!
00183                         //map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 1250, map_size_z/2, 0.06);
00184                         map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 25, 2*map_size_z, 0.06);
00185                         t5 = getDoubleTime();
00186                         //map->addPointCloud(spose.translation(),cloud, 0.06, 25);
00187                         //map->computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE, 1e5, 255, spose.translation(), 0.1);
00188                         //t4 = getDoubleTime();
00189                         //std::cout<<"match: "<<t3-t2<<" addPointCloud: "<<t5-t4<<" ndlocal "<<t1-t0<<" total: "<<t5-t0<<std::endl;
00190                         Tlast_fuse = Tnow;
00191                         if(visualize) //&&ctr%20==0) 
00192                         {
00193 #ifndef NO_NDT_VIZ
00194                             if(ctr%30==0) {
00195                                 viewer->plotNDTSAccordingToOccupancy(-1,map); 
00196                                 //viewer->plotLocalNDTMap(cloud,resolution); 
00197                             }
00198                             viewer->addTrajectoryPoint(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+0.2,0,1,0);
00199                             viewer->addTrajectoryPoint(Todom.translation()(0),Todom.translation()(1),Todom.translation()(2)+0.2,0.5,0,0.5);
00200                             viewer->displayTrajectory();
00201                             viewer->setCameraPointing(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+3);
00202                             viewer->repaint();
00203                             //viewer->win3D->process_events();
00204 #endif
00205             }
00206 
00207                         ctr++;
00208                     }
00209                 }
00210             }else{
00211                 t3 = getDoubleTime();
00212                 Tnow = Tnow * Tmotion;
00213             }
00214 
00215         }
00216         else
00217         {
00218 
00219             t2 = getDoubleTime();
00220             if(matcher.match( *map, ndlocal,Tinit,true) || fuseIncomplete){
00221                 t3 = getDoubleTime();
00222                 Eigen::Affine3d diff = (Tnow * Tmotion).inverse() * Tinit;
00223 
00224                 if((diff.translation().norm() > max_translation_norm || 
00225                             diff.rotation().eulerAngles(0,1,2).norm() > max_rotation_norm) && checkConsistency){
00226                     fprintf(stderr,"****  NDTFuserHMT -- ALMOST DEFINATELY A REGISTRATION FAILURE *****\n");
00227                     Tnow = Tnow * Tmotion;
00228                     //save offending map:
00229                     //map->writeToJFF("map.jff");
00230                     //ndlocal.writeToJFF("local.jff");
00231                 }else{
00232                     Tnow = Tinit;
00233                     //Tnow = Tnow * Tmotion;
00234                     lslgeneric::transformPointCloudInPlace(Tnow, cloud);
00235                     Eigen::Affine3d spose = Tnow*sensor_pose;
00236                     Eigen::Affine3d diff_fuse = Tlast_fuse.inverse()*Tnow;
00237                     if(diff_fuse.translation().norm() > translation_fuse_delta ||
00238                             diff_fuse.rotation().eulerAngles(0,1,2).norm() > rotation_fuse_delta)
00239                     {
00240                         //std::cout<<"F: "<<spose.translation().transpose()<<" "<<spose.rotation().eulerAngles(0,1,2).transpose()<<std::endl;
00241                         t4 = getDoubleTime();
00242                         map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 1250, map_size_z/2, 0.06);
00243                         t5 = getDoubleTime();
00244                         //map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 25, 2*map_size_z, 0.06);
00245                         //map->addPointCloud(spose.translation(),cloud, 0.06, 25);
00246                         //map->computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE, 1e5, 255, spose.translation(), 0.1);
00247                         //t4 = getDoubleTime();
00248                         //std::cout<<"match: "<<t3-t2<<" addPointCloud: "<<t5-t4<<" ndlocal "<<t1-t0<<" total: "<<t5-t0<<std::endl;
00249                         Tlast_fuse = Tnow;
00250                         if(visualize) //&&ctr%20==0) 
00251                         {
00252 #ifndef NO_NDT_VIZ
00253                             if(ctr%2==0) {
00254                                 viewer->plotNDTSAccordingToOccupancy(-1,map); 
00255                                 //viewer->plotLocalNDTMap(cloud,resolution); 
00256                             }
00257                             viewer->addTrajectoryPoint(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+0.2,0,1,0);
00258                             viewer->addTrajectoryPoint(Todom.translation()(0),Todom.translation()(1),Todom.translation()(2)+0.2,0.5,0,0.5);
00259                             viewer->displayTrajectory();
00260                             viewer->setCameraPointing(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+3);
00261                             viewer->repaint();
00262                 viewer->win3D->process_events();
00263 #endif
00264             }
00265                         ctr++;
00266                     }
00267                 }
00268             }else{
00269                 t3 = getDoubleTime();
00270                 Tnow = Tnow * Tmotion;
00271             }
00272         }
00273 
00274         t6 = getDoubleTime();
00275         if(fAddTimes!=NULL) {
00276             fprintf(fAddTimes,"%lf %lf %lf\n",t3-t2,t5-t4,t6-t0);
00277             fflush(fAddTimes);
00278         }
00279 
00280         return Tnow;
00281     }
00282 }