ndt_viz.h
Go to the documentation of this file.
00001 #ifndef NDT_VIZ_HH
00002 #define NDT_VIZ_HH
00003 
00004 #include <mrpt/gui.h>
00005 #include <mrpt/base.h>
00006 #include <mrpt/opengl.h>
00007 
00008 #include <ndt_map/ndt_map.h>
00009 #include <ndt_visualisation/CMyEllipsoid.h>
00010 
00011 #warning "ALWAYS PLACE THE ndt_viz.h BEFORE THE ROS HEADERS!!!"
00012 
00013 template <typename PointT>
00014 class NDTViz {
00015 
00016     public:
00017         mrpt::gui::CDisplayWindow3D *win3D;
00018         mrpt::opengl::CPointCloudColouredPtr gl_points;
00019         mrpt::opengl::CPointCloudColouredPtr gl_particles;
00020 
00021         NDTViz(bool allocate_new_window=true)
00022 
00023         {
00024             if(allocate_new_window) 
00025             {
00026                 win3D = new mrpt::gui::CDisplayWindow3D("NDT Viz",800,600);
00027             }
00028             else 
00029             {
00030                 win3D = NULL;
00031             }
00032 
00033             gl_points = mrpt::opengl::CPointCloudColoured::Create();
00034             gl_particles = mrpt::opengl::CPointCloudColoured::Create();
00035             gl_points->setPointSize(4.5);
00036             gl_particles->setPointSize(2.5);
00037 
00038         }
00039         void repaint(){
00040             win3D->repaint();
00041         }
00042 
00043         void clear(){
00044             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00045             scene->clear();
00046             win3D->unlockAccess3DScene();
00047         }
00048 
00049         void clearTrajectoryPoints(){
00050             gl_points->clear();
00051         }
00052 
00053         void addTrajectoryPoint(float x, float y, float z, float R=1.0, float G = 1.0, float B = 1.0){
00054             win3D->get3DSceneAndLock();
00055             gl_points->push_back(x, y, z, R ,G,B);
00056             win3D->unlockAccess3DScene();
00057         }
00058         void displayTrajectory(){
00059             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00060             scene->insert(gl_points);
00061             win3D->unlockAccess3DScene();
00062         }
00063 
00064         void clearParticles(){ gl_particles->clear();}
00065         void addParticle(float x, float y, float z, float R=1.0, float G = 1.0, float B = 1.0){
00066             win3D->get3DSceneAndLock();
00067             gl_particles->push_back(x, y, z, R ,G,B);
00068             win3D->unlockAccess3DScene();
00069         }
00070         void displayParticles(){
00071             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00072             scene->insert(gl_particles);
00073             win3D->unlockAccess3DScene();
00074         }
00075         void setCameraPointing(double x, double y, double z) {
00076             win3D->get3DSceneAndLock();
00077             win3D->setCameraPointingToPoint(x,y,z);
00078             win3D->unlockAccess3DScene();
00079         }
00080 
00084         void addScan(Eigen::Vector3d orig, pcl::PointCloud<PointT> &cloud, double R=1.0,double G=1.0,double B=1.0){
00085 
00086             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00087             mrpt::opengl::CSetOfLinesPtr obj = mrpt::opengl::CSetOfLines::Create();
00088             for(unsigned int i=0;i<cloud.points.size();i+=2){
00089                 obj->appendLine(orig(0),orig(1),orig(2), cloud.points[i].x, cloud.points[i].y, cloud.points[i].z);
00090             }
00091             obj->setColor(R,G,B);
00092             scene->insert(obj);
00093             win3D->unlockAccess3DScene();
00094         }
00095 
00096 
00097 
00098         void plotNDTMap(lslgeneric::NDTMap<PointT> *map, double R=1.0,double G=1.0,double B=1.0, bool heightCoding=false, bool setCameraPos = true ){
00099             if(win3D == NULL) return;
00100             std::vector<lslgeneric::NDTCell<PointT>*> global_ndts;
00101             global_ndts = map->getAllCells();
00102 
00103             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00104             unsigned int accepted_ndts=1;
00105             double x = 0,y=0,s=0;
00106             fprintf(stderr,"-NDT:%lu-",global_ndts.size());
00107             for(unsigned int i=0;i<global_ndts.size();i++){
00108                 Eigen::Vector3d m = global_ndts[i]->getMean();
00109                 if(!global_ndts[i]->hasGaussian_) continue;
00110                 x+=m[0];
00111                 y+=m[1];
00112                 s+=1;
00113 
00114                 accepted_ndts++;
00115                 mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00116                 Eigen::Matrix3d cov = global_ndts[i]->getCov();
00117                 mrpt::math::CMatrixDouble M = 0.5*cov;
00118                 objEllip->setCovMatrix(M);
00119                 objEllip->setLocation(m[0], m[1], m[2]);
00120                 double hval=1.0;
00121                 if(heightCoding){
00122                     hval = fabs(m[2]+1.5)/6.0;
00123                 }
00124                 if(global_ndts[i]->getOccupancy() > 0){
00125                     objEllip->setColor(R/hval,G/hval,B/hval,1.0);
00126                 }else{
00127                     objEllip->setColor(1.0,0,0,1.0);
00128                 }
00129 
00130                 objEllip->enableDrawSolid3D(true);
00131                 scene->insert( objEllip );
00132 
00133             }
00134             if(setCameraPos) win3D->setCameraPointingToPoint(x/s,y/s,3.0);
00135             win3D->unlockAccess3DScene();
00136             win3D->repaint();
00137             //fprintf(stderr,"(%lf %lf) s=%lf\n",x/s,y/s,s);
00138             for(unsigned int i=0;i<global_ndts.size();i++) delete global_ndts[i];
00139 
00140         }
00141 
00142         void plotNDTSAccordingToCost(float occupancy, double MAX_COST, lslgeneric::NDTMap<PointT> *map){
00143 
00144             if(win3D == NULL) return;
00145             std::vector<lslgeneric::NDTCell<PointT>*> global_ndts;
00146             global_ndts = map->getAllCells();
00147             fprintf(stderr," NUM NDT: %lu ", global_ndts.size());
00148 
00149             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00150             scene->clear();
00151             unsigned int accepted_ndts=1;
00152             double x = 0,y=0,s=0;
00153             for(unsigned int i=0;i<global_ndts.size();i++){
00154                 Eigen::Vector3d m = global_ndts[i]->getMean();
00155                 if(!global_ndts[i]->hasGaussian_) continue;
00156                 x+=m[0];
00157                 y+=m[1];
00158                 s+=1;
00159                 if(global_ndts[i]->getOccupancy()>occupancy){
00160                     accepted_ndts++;
00161                     mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00162                     Eigen::Matrix3d cov = global_ndts[i]->getCov();
00163                     mrpt::math::CMatrixDouble M = cov;
00164                     objEllip->setCovMatrix(M);
00165                     objEllip->setLocation(m[0], m[1], m[2]);
00166                     double c = global_ndts[i]->cost;
00167                     if(c > MAX_COST) c=MAX_COST;
00168                     c = 1 - c/MAX_COST; 
00169                     objEllip->setColor(c,c,c,0.6);
00170 
00171                     objEllip->enableDrawSolid3D(true);
00172                     scene->insert( objEllip );
00173                 }else if(global_ndts[i]->getOccupancy()<-0){
00174                     /*
00175                        mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00176                        Eigen::Matrix3d cov = global_ndts[i]->getCov();
00177                        cov = cov;
00178                     //Eigen::Vector3d m = global_ndts[i]->getMean();
00179 
00180                     mrpt::math::CMatrixDouble M = cov;
00181                     objEllip->setCovMatrix(M);
00182                     objEllip->setLocation(m[0], m[1], m[2]);
00183                     objEllip->setColor(1.0,0,0,1.0);
00184                     objEllip->enableDrawSolid3D(true);
00185                     scene->insert( objEllip );*/
00186                 }
00187             }
00188             //win3D->setCameraPointingToPoint(x/s,y/s,3.0);
00189             win3D->unlockAccess3DScene();
00190             win3D->repaint();
00191             fprintf(stderr,"(%lf %lf) s=%lf\n",x/s,y/s,s);
00192             for(unsigned int i=0;i<global_ndts.size();i++) delete global_ndts[i];
00193 
00194         }
00196         void plotNDTSAccordingToClass(float occupancy, lslgeneric::NDTMap<PointT> *map){
00197 
00198             if(win3D == NULL) return;
00199             std::vector<lslgeneric::NDTCell<PointT>*> global_ndts;
00200             global_ndts = map->getAllCells();
00201             fprintf(stderr," NUM NDT: %lu ", global_ndts.size());
00202 
00203             Eigen::Vector3d cFlat, cInclined, cRough, cVert, cUnknown, color;
00204             cFlat<<0,0.9,0;
00205             cInclined<<0.1,0.2,0.5;
00206             cRough<<0.9,0,0;
00207             cVert<<0,0,0.9;
00208             cUnknown<<0,0,0;
00209 
00210             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00211             scene->clear();
00212             unsigned int accepted_ndts=1;
00213             double x = 0,y=0,s=0;
00214             for(unsigned int i=0;i<global_ndts.size();i++){
00215                 Eigen::Vector3d m = global_ndts[i]->getMean();
00216                 if(!global_ndts[i]->hasGaussian_) continue;
00217                 x+=m[0];
00218                 y+=m[1];
00219                 s+=1;
00220                 if(global_ndts[i]->getOccupancy()>occupancy){
00221                     accepted_ndts++;
00222                     mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00223                     Eigen::Matrix3d cov = global_ndts[i]->getCov();
00224                     mrpt::math::CMatrixDouble M = cov;
00225                     objEllip->setCovMatrix(M);
00226                     objEllip->setLocation(m[0], m[1], m[2]);
00227                     switch(global_ndts[i]->getClass()) {
00228                         case lslgeneric::NDTCell<PointT>::HORIZONTAL : 
00229                             color = cFlat;
00230                             break;
00231                         case lslgeneric::NDTCell<PointT>::VERTICAL :
00232                             color = cVert;
00233                             break;
00234                         case lslgeneric::NDTCell<PointT>::INCLINED :
00235                             color = cInclined;
00236                             break;
00237                         case lslgeneric::NDTCell<PointT>::ROUGH :
00238                             color = cRough;
00239                             break;
00240                         default:
00241                             color = cUnknown;
00242                     }
00243                     objEllip->setColor(color(0),color(1),color(2),0.6);
00244 
00245                     objEllip->enableDrawSolid3D(true);
00246                     scene->insert( objEllip );
00247                 }else if(global_ndts[i]->getOccupancy()<-0){
00248                     /*
00249                        mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00250                        Eigen::Matrix3d cov = global_ndts[i]->getCov();
00251                        cov = cov;
00252                     //Eigen::Vector3d m = global_ndts[i]->getMean();
00253 
00254                     mrpt::math::CMatrixDouble M = cov;
00255                     objEllip->setCovMatrix(M);
00256                     objEllip->setLocation(m[0], m[1], m[2]);
00257                     objEllip->setColor(1.0,0,0,1.0);
00258                     objEllip->enableDrawSolid3D(true);
00259                     scene->insert( objEllip );*/
00260                 }
00261             }
00262             //win3D->setCameraPointingToPoint(x/s,y/s,3.0);
00263             win3D->unlockAccess3DScene();
00264             win3D->repaint();
00265             fprintf(stderr,"(%lf %lf) s=%lf\n",x/s,y/s,s);
00266             for(unsigned int i=0;i<global_ndts.size();i++) delete global_ndts[i];
00267 
00268         }
00269 
00270         void plotNDTSAccordingToOccupancy(float occupancy, lslgeneric::NDTMap<PointT> *map){
00271             if(win3D == NULL) return;
00272             std::vector<lslgeneric::NDTCell<PointT>*> global_ndts;
00273             global_ndts = map->getAllCells();
00274             fprintf(stderr," NUM NDT: %lu ", global_ndts.size());
00275 
00276             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00277             scene->clear();
00278             unsigned int accepted_ndts=1;
00279             double x = 0,y=0,s=0;
00280             for(unsigned int i=0;i<global_ndts.size();i++){
00281                 Eigen::Vector3d m = global_ndts[i]->getMean();
00282                 if(!global_ndts[i]->hasGaussian_) continue;
00283                 x+=m[0];
00284                 y+=m[1];
00285                 s+=1;
00286                 if(global_ndts[i]->getOccupancy()>occupancy){
00287                     accepted_ndts++;
00288                     mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00289                     Eigen::Matrix3d cov = global_ndts[i]->getCov();
00290                     mrpt::math::CMatrixDouble M = cov;
00291                     objEllip->setCovMatrix(M);
00292                     objEllip->setLocation(m[0], m[1], m[2]);
00293 
00294                     objEllip->setColor((m[2]+2.0)/10.0,0,0,0.6);
00295 
00296                     objEllip->enableDrawSolid3D(true);
00297                     scene->insert( objEllip );
00298                 }else if(global_ndts[i]->getOccupancy()<-0){
00299                     /*
00300                        mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00301                        Eigen::Matrix3d cov = global_ndts[i]->getCov();
00302                        cov = cov;
00303                     //Eigen::Vector3d m = global_ndts[i]->getMean();
00304 
00305                     mrpt::math::CMatrixDouble M = cov;
00306                     objEllip->setCovMatrix(M);
00307                     objEllip->setLocation(m[0], m[1], m[2]);
00308                     objEllip->setColor(1.0,0,0,1.0);
00309                     objEllip->enableDrawSolid3D(true);
00310                     scene->insert( objEllip );*/
00311                 }
00312             }
00313             //win3D->setCameraPointingToPoint(x/s,y/s,3.0);
00314             win3D->unlockAccess3DScene();
00315             win3D->repaint();
00316             fprintf(stderr,"(%lf %lf) s=%lf\n",x/s,y/s,s);
00317             for(unsigned int i=0;i<global_ndts.size();i++) delete global_ndts[i];
00318 
00319         }
00320 
00321         void plotLocalNDTMap(pcl::PointCloud<PointT> &cloud, double resolution, double R=0, double G=1, double B=0, bool heightCoding=true){
00322             if(win3D == NULL) return;
00323 
00324             lslgeneric::NDTMap<PointT> ndlocal(new lslgeneric::LazyGrid<PointT>(resolution));
00325             ndlocal.addPointCloudSimple(cloud);
00326             ndlocal.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
00327 
00328             std::vector<lslgeneric::NDTCell<PointT>*> ndts;
00329             ndts = ndlocal.getAllCells();
00330             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00331             std::cout<<"LOCAL: "<<ndts.size()<<std::endl;
00332 
00333             for(unsigned int i=0;i<ndts.size();i++){
00334                 Eigen::Vector3d m = ndts[i]->getMean();
00335                 //if(m[2]>3.0) continue;
00336 
00337                 mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00338                 Eigen::Matrix3d cov = ndts[i]->getCov();
00339                 cov = cov;
00340 
00341                 mrpt::math::CMatrixDouble M = 0.5*cov;
00342                 objEllip->setCovMatrix(M);
00343                 objEllip->setLocation(m[0], m[1], m[2]);
00344                 double hval=1.0;
00345                 if(heightCoding){
00346                     hval = fabs(m[2]+1.5)/20.0;
00347                 }
00348                 objEllip->setColor(R/hval,G/hval,B/hval,0.4);
00349                 objEllip->enableDrawSolid3D(true);
00350                 scene->insert( objEllip );
00351             }
00352             win3D->unlockAccess3DScene();
00353             for(unsigned int i=0;i<ndts.size();i++){
00354                 delete ndts[i];
00355             }
00356 
00357         }
00358 
00359         void plotLocalConflictNDTMap(lslgeneric::NDTMap<PointT> *map, pcl::PointCloud<PointT> &cloud, 
00360                 double resolution, double R=1, double G=0, double B=0, bool heightCoding=false, double maxz=0){
00361             if(win3D == NULL) return;
00362 
00363             lslgeneric::NDTMap<PointT> ndlocal(new lslgeneric::LazyGrid<PointT>(resolution));
00364 
00365             ndlocal.addPointCloudSimple(cloud);
00366             ndlocal.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
00367 
00368             std::vector<lslgeneric::NDTCell<PointT>*> ndts;
00369             ndts = ndlocal.getAllCells();
00370 
00371             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00372             std::cout<<"LOCAL: "<<ndts.size()<<std::endl;
00373 
00374             for(unsigned int i=0;i<ndts.size();i++){
00375 
00376 
00377                 Eigen::Vector3d m = ndts[i]->getMean();
00378                 if(m[2]>maxz) continue;
00379                 PointT p;
00380                 p.x = m[0]; p.y=m[1]; p.z = m[2];
00381                 lslgeneric::NDTCell<PointT> *map_cell=NULL;
00382                 map->getCellAtPoint(p, map_cell);
00383                 if(map_cell == NULL) continue;
00384 
00385                 if(map_cell->getOccupancy()>0.5) continue;
00386 
00387                 //      if(m[2]>3.0) continue;
00388 
00389                 mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00390                 Eigen::Matrix3d cov = ndts[i]->getCov();
00391                 cov = cov;
00392 
00393                 mrpt::math::CMatrixDouble M = 0.5*cov;
00394                 objEllip->setCovMatrix(M);
00395                 objEllip->setLocation(m[0], m[1], m[2]);
00396                 double hval=1.0;
00397                 if(heightCoding){
00398                     hval = fabs(m[2]+1.5)/20.0;
00399                 }
00400                 objEllip->setColor(R/hval,G/hval,B/hval,0.6);
00401                 objEllip->enableDrawSolid3D(true);
00402                 scene->insert( objEllip );
00403             }
00404             win3D->unlockAccess3DScene();
00405             for(unsigned int i=0;i<ndts.size();i++){
00406                 delete ndts[i];
00407             }
00408 
00409         }
00410 
00411 
00412         void plotNDTTraversabilityMap(lslgeneric::NDTMap<PointT> *map){
00413             if(win3D == NULL) return;
00414             std::vector<lslgeneric::NDTCell<PointT>*> global_ndts;
00415             global_ndts = map->getAllCells();
00416 
00417             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00418 
00419             for(unsigned int i=0;i<global_ndts.size();i++){
00420                 Eigen::Vector3d m = global_ndts[i]->getMean();
00421                 if(!global_ndts[i]->hasGaussian_) continue;
00422 
00423                 mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00424                 Eigen::Matrix3d cov = global_ndts[i]->getCov();
00425                 mrpt::math::CMatrixDouble M = 0.5*cov;
00426                 objEllip->setCovMatrix(M);
00427                 objEllip->setLocation(m[0], m[1], m[2]);
00428 
00429                 //lslgeneric::CellClass CC;
00430                 //lslgeneric::NDTCell<PointT>::CellClass CC;
00431 
00432                 //CC = global_ndts[i]->getClass();
00433                 // {HORIZONTAL=0, VERTICAL, INCLINED, ROUGH, UNKNOWN};
00434                 if(global_ndts[i]->getClass() == lslgeneric::NDTCell<PointT>::HORIZONTAL){
00435                     objEllip->setColor(0,1.0,0,1.0);
00436                 }else if(global_ndts[i]->getClass() == lslgeneric::NDTCell<PointT>::VERTICAL){
00437                     objEllip->setColor(1.0,0,0,1.0);
00438                 }else if(global_ndts[i]->getClass() == lslgeneric::NDTCell<PointT>::INCLINED){
00439                     objEllip->setColor(1.0,1.0,0,1.0);
00440                 }else if(global_ndts[i]->getClass() == lslgeneric::NDTCell<PointT>::ROUGH){
00441                     objEllip->setColor(0,0,1.0,1.0);
00442                 }else{
00443                     objEllip->setColor(1.0,1.0,1.0,1.0);
00444                 }
00445 
00446                 objEllip->enableDrawSolid3D(true);
00447                 scene->insert( objEllip );
00448 
00449             }
00450 
00451             win3D->unlockAccess3DScene();
00452             win3D->repaint();
00453             for(unsigned int i=0;i<global_ndts.size();i++) delete global_ndts[i];
00454 
00455         }
00460 
00464         void ndtCoolCam(lslgeneric::NDTMap<PointT> *map ,const Eigen::Affine3d &spos, double maxDist=70.0, 
00465                 unsigned int Nx=800, unsigned int Ny=600, double fx=800, double fy=600){
00466             Eigen::Matrix3d K;
00467             K << fx,0,(double)Nx/2.0,
00468               0, fy, (double)Ny/2.0,
00469               0,0,1;
00470 
00471             Eigen::Matrix3d invK;
00472             double det=0;
00473             bool exists=false;
00474             K.computeInverseAndDetWithCheck(invK,det,exists);
00475 
00476             if(!exists){
00477                 fprintf(stderr,"ndtCoolCam::ERROR: NO INVERSE!!!!\n");
00478                 return;
00479             }
00480 
00481             Eigen::Matrix3d m1,m2; 
00482             m1 = Eigen::AngleAxisd(-M_PI/2.0, Eigen::Vector3d::UnitX())
00483                 * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
00484                 * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
00485 
00486             m2 = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
00487                 * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
00488                 * Eigen::AngleAxisd(-M_PI/2.0, Eigen::Vector3d::UnitZ());
00489 
00490             mrpt::opengl::CTexturedPlanePtr gl_img              =  mrpt::opengl::CTexturedPlane::Create(0.5,-0.5,-0.5,0.5);
00491             mrpt::opengl::COpenGLViewportPtr viewInt;
00492             //mrpt::utils::CImage  img(Nx,Ny,CH_GRAY);
00493             mrpt::utils::CImage  img(Nx,Ny,CH_RGB);
00494 
00495 
00496             //mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();        
00497 
00498             //mrpt::opengl::CSetOfLinesPtr obj = mrpt::opengl::CSetOfLines::Create();           
00499             for(unsigned int i=0; i<Nx*Ny;i++){
00500                 Eigen::Vector3d pix(i%Nx,(int)i/Nx,1);
00501                 Eigen::Vector3d dirv = invK*pix; 
00502                 dirv.normalize();
00503 
00504                 Eigen::Vector3d dirv_w = spos.rotation()*(m2*(m1*dirv)); 
00505                 //double max_depth = 20;
00506                 double depth = map->getDepth(spos.translation(), dirv_w, maxDist);
00507 
00508                 if(depth>maxDist) img.setPixel(i%Nx,i/Nx,0);
00509                 else{
00510                     float x = (depth/maxDist);
00511                     float r = ((x >= 3.0/8.0) & (x < 5.0/8.0))*(4. * x - 3./2.)+((x >= 5./8.) & (x < 7./8.))+(x >= 7./8.) * (-4. * x + 9./2.);
00512                     float g = ((x >= 1./8.) & (x < 3./8.))*(4. * x - 1./2.)+((x >= 3./8.) & (x < 5./8.))+((x >= 5./8.) & (x < 7./8.))*(-4. * x + 7./2.);
00513                     float b = (x < 1./8.)*(4. * x + 1./2.)+((x >= 1./8.) & (x < 3./8.))+((x >= 3./8.) & (x < 5./8.))*(-4. * x + 5./2.);
00514                     size_t color=((unsigned char)(r*255))*255*255 + ((unsigned char)(g*255))*255 + ((unsigned char)(b*255));
00515 
00516                     img.setPixel(i%Nx,i/Nx,color);
00517                     //fprintf(stderr,"(%.2lf) ",depth);
00518                 }
00519                 /*
00520                    Eigen::Vector3d ray_endpos=spos.translation() + dirv_w * max_depth;
00521                    obj->appendLine(spos.translation()(0),spos.translation()(1),spos.translation()(2),
00522                    ray_endpos(0), ray_endpos(1), ray_endpos(2));*/
00523 
00524             }
00525             //scene->insert(obj);
00526 
00527             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();  
00528             scene->clear();
00529 
00530             viewInt = scene->createViewport("view2d");
00531             scene->insert( gl_img, "view2d");
00532 
00533             viewInt->setViewportPosition(0, 0, -1.0, -1.0);
00534 
00535             viewInt->setTransparent(false);
00536 
00537             viewInt->getCamera().setOrthogonal(true);
00538             viewInt->getCamera().setAzimuthDegrees(90);
00539             viewInt->getCamera().setElevationDegrees(90);
00540             viewInt->getCamera().setZoomDistance(1.0);
00541 
00542             gl_img->assignImage_fast(img);
00543 
00544             win3D->unlockAccess3DScene();
00545             win3D->repaint();
00546         }       
00551 
00552 
00553 
00554 };
00555 
00556 #endif

ndt_visualisation
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:19:48