ndt_fuser.h
Go to the documentation of this file.
00001 #ifndef NDT_FUSER_HH
00002 #define NDT_FUSER_HH
00003 
00004 #include <ndt_viz.h>
00005 #include <ndt_map.h>
00006 #include <ndt_matcher_d2d_2d.h>
00007 #include <ndt_matcher_d2d.h>
00008 #include <pointcloud_utils.h>
00009 
00010 #include <Eigen/Eigen>
00011 #include <pcl/point_cloud.h>
00012 
00013 namespace lslgeneric {
00019 template <typename PointT>
00020 class NDTFuser{
00021     public:
00022         Eigen::Affine3d Tnow, Tlast_fuse; 
00023         lslgeneric::NDTMap<PointT> *map;  
00024         bool checkConsistency; 
00025         double max_translation_norm, max_rotation_norm;
00026         double sensor_range;
00027         bool be2D;
00028         NDTViz<PointT> *viewer;
00029 
00030         NDTFuser(double map_resolution, double map_size_x_, double map_size_y_, double map_size_z_, double sensor_range_ = 3, bool visualize_=false, bool be2D_=false){
00031             isInit = false;
00032             resolution = map_resolution;
00033             sensor_pose.setIdentity();
00034             checkConsistency = false;
00035             visualize = true;
00036             translation_fuse_delta = 0.05;
00037             rotation_fuse_delta = 0.01;
00038             max_translation_norm = 1.;
00039             max_rotation_norm = M_PI/4;
00040             map_size_x = map_size_x_;
00041             map_size_y = map_size_y_;
00042             map_size_z = map_size_z_;
00043             visualize = visualize_;
00044             be2D = be2D_;
00045             sensor_range = sensor_range_;
00046             viewer = new NDTViz<PointT>(visualize);
00047             std::cout<<"MAP: resolution: "<<resolution<<" size "<<map_size_x<<" "<<map_size_y<<" "<<map_size_z<<std::endl;
00048             map = new lslgeneric::NDTMap<PointT>(new lslgeneric::LazyGrid<PointT>(resolution));
00049         }
00050         ~NDTFuser()
00051         {
00052             delete map;
00053             delete viewer;
00054         }
00055 
00056         double getDoubleTime()
00057         {
00058             struct timeval time;
00059             gettimeofday(&time,NULL);
00060             return time.tv_sec + time.tv_usec * 1e-6;
00061         }
00062         void setSensorPose(Eigen::Affine3d spose){
00063             sensor_pose = spose;
00064         }
00065         
00066         bool wasInit()
00067         {
00068             return isInit;
00069         }
00070 
00074         void initialize(Eigen::Affine3d initPos, pcl::PointCloud<PointT> &cloud){
00075             std::cout<<"Initializing NDT FUSER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n";
00077             lslgeneric::transformPointCloudInPlace(sensor_pose, cloud);
00078             lslgeneric::transformPointCloudInPlace(initPos, cloud);
00079             Tnow = initPos;
00080             map->initialize(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2),map_size_x,map_size_y,map_size_z);
00081             map->addPointCloud(Tnow.translation(),cloud, 0.1, 100.0, 0.1);
00082             map->computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE, 1e5, 255, Tnow.translation(), 0.1);
00083             isInit = true;
00084             Tlast_fuse = Tnow;
00085             if(visualize) 
00086             {
00087                 viewer->plotNDTSAccordingToOccupancy(-1,map); 
00088                 viewer->plotLocalNDTMap(cloud,resolution); 
00089             }
00090         }
00091 
00096         Eigen::Affine3d update(Eigen::Affine3d Tmotion, pcl::PointCloud<PointT> &cloud){
00097             if(!isInit){
00098                 fprintf(stderr,"NDT-Fuser: Call Initialize first!!\n");
00099                 return Tnow;
00100             }
00101             double t1,t2,t3,t4;
00103             lslgeneric::transformPointCloudInPlace(sensor_pose, cloud);
00104             t1 = getDoubleTime();
00106             lslgeneric::NDTMap<PointT> ndlocal(new lslgeneric::LazyGrid<PointT>(resolution));
00107             //ndlocal.guessSize(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2),sensor_range,sensor_range,sensor_range);
00108             ndlocal.addPointCloudSimple(cloud);
00109             ndlocal.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
00110             t2 = getDoubleTime();
00111             t3 = t4 = t2;
00112             //Tmotion.setIdentity();        
00113             Eigen::Affine3d Tinit = Tnow * Tmotion;
00114             if(be2D) {
00115                 if(matcher2D.match( *map, ndlocal,Tinit,true)){
00116                     Eigen::Affine3d diff = (Tnow * Tmotion).inverse() * Tinit;
00117                     if((diff.translation().norm() > max_translation_norm || 
00118                                 diff.rotation().eulerAngles(0,1,2).norm() > max_rotation_norm) && checkConsistency){
00119                         fprintf(stderr,"****  NDTFuser -- ALMOST DEFINATELY A REGISTRATION FAILURE *****\n");
00120                         Tnow = Tnow * Tmotion;
00121                     }else{
00122                         Tnow = Tinit;
00123                         lslgeneric::transformPointCloudInPlace(Tnow, cloud);
00124                         Eigen::Affine3d spose = Tnow*sensor_pose;
00125                         Eigen::Affine3d diff_fuse = Tlast_fuse.inverse()*Tnow;
00126 
00127                         if(diff_fuse.translation().norm() > translation_fuse_delta ||
00128                                 diff_fuse.rotation().eulerAngles(0,1,2).norm() > rotation_fuse_delta)
00129                         {
00130                             t3 = getDoubleTime();
00131                             map->addPointCloud(spose.translation(),cloud, 0.06, 2.5);
00132                             t4 = getDoubleTime();
00133                             map->computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE, 1e5, 255, spose.translation(), 0.1);
00134                             Tlast_fuse = Tnow;
00135                             if(visualize) 
00136                             {
00137                                 viewer->plotNDTSAccordingToOccupancy(-1,map); 
00138                                 viewer->plotLocalNDTMap(cloud,resolution); 
00139                             }
00140                         }
00141                     }
00142                     /*
00143                     std::cout<<"load: "<<t2-t1<<" match: "<<t3-t2<<" fuse: "<<t4-t3<<" total: "<<t4-t1<<std::endl;
00144                     FILE *ftmp = fopen("tmp.txt","a");
00145                     fprintf(ftmp,"%lf, ",t4-t3);
00146                     fclose(ftmp);*/
00147                 }else{
00148                     Tnow = Tnow * Tmotion;
00149                 }
00150 
00151             }
00152             else
00153             {
00154                 if(matcher.match( *map, ndlocal,Tinit,true)){
00155                     Eigen::Affine3d diff = (Tnow * Tmotion).inverse() * Tinit;
00156                     if((diff.translation().norm() > max_translation_norm || 
00157                                 diff.rotation().eulerAngles(0,1,2).norm() > max_rotation_norm) && checkConsistency){
00158                         fprintf(stderr,"****  NDTFuser -- ALMOST DEFINATELY A REGISTRATION FAILURE *****\n");
00159                         Tnow = Tnow * Tmotion;
00160                         //save offending map:
00161                         //map->writeToJFF("map.jff");
00162                         //ndlocal.writeToJFF("local.jff");
00163                     }else{
00164                         Tnow = Tinit;
00165                         lslgeneric::transformPointCloudInPlace(Tnow, cloud);
00166                         std::cout<<"Tnow :" << Tnow.matrix()<<std::endl;
00167                         Eigen::Affine3d spose = Tnow*sensor_pose;
00168                         Eigen::Affine3d diff_fuse = Tlast_fuse.inverse()*Tnow;
00169 
00170                         if(diff_fuse.translation().norm() > translation_fuse_delta ||
00171                                 diff_fuse.rotation().eulerAngles(0,1,2).norm() > rotation_fuse_delta)
00172                         {
00173                             t3 = getDoubleTime();
00174                             map->addPointCloud(spose.translation(),cloud, 0.06, 2.5);
00175                             t4 = getDoubleTime();
00176                             map->computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE, 1e5, 255, spose.translation(), 0.1);
00177                             Tlast_fuse = Tnow;
00178                             if(visualize) 
00179                             {
00180                                 viewer->plotNDTSAccordingToOccupancy(-1,map); 
00181                                 viewer->plotLocalNDTMap(cloud,resolution); 
00182                             }
00183 
00184                         }
00185                     }
00186                     std::cout<<"load: "<<t2-t1<<" match: "<<t3-t2<<" fuse: "<<t4-t3<<" total: "<<t4-t1<<std::endl;
00187                     /*FILE *ftmp = fopen("tmp.txt","a");
00188                     fprintf(ftmp,"%lf, ",t4-t3);
00189                     fclose(ftmp);*/
00190                 }else{
00191                     Tnow = Tnow * Tmotion;
00192                 }
00193             }
00194             
00195             return Tnow;
00196         }
00197 
00198     private:
00199         bool isInit;
00200 
00201         double resolution; 
00202         double map_size;
00203         
00204         double translation_fuse_delta, rotation_fuse_delta;
00205         double map_size_x;
00206         double map_size_y;
00207         double map_size_z;
00208         bool visualize;
00209 
00210         Eigen::Affine3d sensor_pose;
00211         lslgeneric::NDTMatcherD2D<PointT,PointT> matcher;
00212         lslgeneric::NDTMatcherD2D_2D<PointT,PointT> matcher2D;
00213 
00214     public:
00215         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00216 
00217 };
00218 }
00219 #endif


ndt_fuser
Author(s): Todor Stoyanov and Jari Saarinen
autogenerated on Mon Jan 6 2014 11:35:58