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.h>
00009 #include <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(3.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         
00076 
00077 
00078 void plotNDTMap(lslgeneric::NDTMap<PointT> *map, double R=1.0,double G=1.0,double B=1.0, bool heightCoding=false, bool setCameraPos = true ){
00079             if(win3D == NULL) return;
00080             std::vector<lslgeneric::NDTCell<PointT>*> global_ndts;
00081             global_ndts = map->getAllCells();
00082             
00083             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00084             unsigned int accepted_ndts=1;
00085             double x = 0,y=0,s=0;
00086             fprintf(stderr,"-NDT:%d-",global_ndts.size());
00087             for(unsigned int i=0;i<global_ndts.size();i++){
00088                                 Eigen::Vector3d m = global_ndts[i]->getMean();
00089                                 if(!global_ndts[i]->hasGaussian_) continue;
00090                                 x+=m[0];
00091                                 y+=m[1];
00092                                 s+=1;
00093         
00094                                 accepted_ndts++;
00095                                 mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00096                                 Eigen::Matrix3d cov = global_ndts[i]->getCov();
00097                                 mrpt::math::CMatrixDouble M = 0.5*cov;
00098                                 objEllip->setCovMatrix(M);
00099                                 objEllip->setLocation(m[0], m[1], m[2]);
00100                                 double hval=1.0;
00101                                 if(heightCoding){
00102                                         hval = fabs(m[2]+1.5)/6.0;
00103                                 }
00104                                 if(global_ndts[i]->getOccupancy() > 0){
00105                                         objEllip->setColor(R/hval,G/hval,B/hval,1.0);
00106                                 }else{
00107                                         objEllip->setColor(1.0,0,0,1.0);
00108                                 }
00109 
00110                                 objEllip->enableDrawSolid3D(true);
00111                                 scene->insert( objEllip );
00112                                 
00113                         }
00114                         if(setCameraPos) win3D->setCameraPointingToPoint(x/s,y/s,3.0);
00115                         win3D->unlockAccess3DScene();
00116                         win3D->repaint();
00117                         //fprintf(stderr,"(%lf %lf) s=%lf\n",x/s,y/s,s);
00118                         for(unsigned int i=0;i<global_ndts.size();i++) delete global_ndts[i];
00119 
00120 }
00121 
00122         void plotNDTSAccordingToCost(float occupancy, double MAX_COST, lslgeneric::NDTMap<PointT> *map){
00123 
00124             if(win3D == NULL) return;
00125             std::vector<lslgeneric::NDTCell<PointT>*> global_ndts;
00126             global_ndts = map->getAllCells();
00127             fprintf(stderr," NUM NDT: %d ", global_ndts.size());
00128 
00129             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00130             scene->clear();
00131             unsigned int accepted_ndts=1;
00132             double x = 0,y=0,s=0;
00133             for(unsigned int i=0;i<global_ndts.size();i++){
00134                 Eigen::Vector3d m = global_ndts[i]->getMean();
00135                 if(!global_ndts[i]->hasGaussian_) continue;
00136                 x+=m[0];
00137                 y+=m[1];
00138                 s+=1;
00139                 if(global_ndts[i]->getOccupancy()>occupancy){
00140                     accepted_ndts++;
00141                     mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00142                     Eigen::Matrix3d cov = global_ndts[i]->getCov();
00143                     mrpt::math::CMatrixDouble M = cov;
00144                     objEllip->setCovMatrix(M);
00145                     objEllip->setLocation(m[0], m[1], m[2]);
00146                     double c = global_ndts[i]->cost;
00147                     if(c > MAX_COST) c=MAX_COST;
00148                     c = 1 - c/MAX_COST; 
00149                     objEllip->setColor(c,c,c,0.6);
00150 
00151                     objEllip->enableDrawSolid3D(true);
00152                     scene->insert( objEllip );
00153                 }else if(global_ndts[i]->getOccupancy()<-0){
00154                     /*
00155                     mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00156                     Eigen::Matrix3d cov = global_ndts[i]->getCov();
00157                     cov = cov;
00158                     //Eigen::Vector3d m = global_ndts[i]->getMean();
00159 
00160                     mrpt::math::CMatrixDouble M = cov;
00161                     objEllip->setCovMatrix(M);
00162                     objEllip->setLocation(m[0], m[1], m[2]);
00163                     objEllip->setColor(1.0,0,0,1.0);
00164                     objEllip->enableDrawSolid3D(true);
00165                     scene->insert( objEllip );*/
00166                 }
00167             }
00168             //win3D->setCameraPointingToPoint(x/s,y/s,3.0);
00169             win3D->unlockAccess3DScene();
00170             win3D->repaint();
00171             fprintf(stderr,"(%lf %lf) s=%lf\n",x/s,y/s,s);
00172             for(unsigned int i=0;i<global_ndts.size();i++) delete global_ndts[i];
00173 
00174         }
00176         void plotNDTSAccordingToClass(float occupancy, lslgeneric::NDTMap<PointT> *map){
00177 
00178             if(win3D == NULL) return;
00179             std::vector<lslgeneric::NDTCell<PointT>*> global_ndts;
00180             global_ndts = map->getAllCells();
00181             fprintf(stderr," NUM NDT: %d ", global_ndts.size());
00182 
00183             Eigen::Vector3d cFlat, cInclined, cRough, cVert, cUnknown, color;
00184             cFlat<<0,0.9,0;
00185             cInclined<<0.1,0.2,0.5;
00186             cRough<<0.9,0,0;
00187             cVert<<0,0,0.9;
00188             cUnknown<<0,0,0;
00189 
00190             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00191             scene->clear();
00192             unsigned int accepted_ndts=1;
00193             double x = 0,y=0,s=0;
00194             for(unsigned int i=0;i<global_ndts.size();i++){
00195                 Eigen::Vector3d m = global_ndts[i]->getMean();
00196                 if(!global_ndts[i]->hasGaussian_) continue;
00197                 x+=m[0];
00198                 y+=m[1];
00199                 s+=1;
00200                 if(global_ndts[i]->getOccupancy()>occupancy){
00201                     accepted_ndts++;
00202                     mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00203                     Eigen::Matrix3d cov = global_ndts[i]->getCov();
00204                     mrpt::math::CMatrixDouble M = cov;
00205                     objEllip->setCovMatrix(M);
00206                     objEllip->setLocation(m[0], m[1], m[2]);
00207                     switch(global_ndts[i]->getClass()) {
00208                         case lslgeneric::NDTCell<PointT>::HORIZONTAL : 
00209                             color = cFlat;
00210                             break;
00211                         case lslgeneric::NDTCell<PointT>::VERTICAL :
00212                             color = cVert;
00213                             break;
00214                         case lslgeneric::NDTCell<PointT>::INCLINED :
00215                             color = cInclined;
00216                             break;
00217                         case lslgeneric::NDTCell<PointT>::ROUGH :
00218                             color = cRough;
00219                             break;
00220                         default:
00221                             color = cUnknown;
00222                     }
00223                     objEllip->setColor(color(0),color(1),color(2),0.6);
00224 
00225                     objEllip->enableDrawSolid3D(true);
00226                     scene->insert( objEllip );
00227                 }else if(global_ndts[i]->getOccupancy()<-0){
00228                     /*
00229                     mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00230                     Eigen::Matrix3d cov = global_ndts[i]->getCov();
00231                     cov = cov;
00232                     //Eigen::Vector3d m = global_ndts[i]->getMean();
00233 
00234                     mrpt::math::CMatrixDouble M = cov;
00235                     objEllip->setCovMatrix(M);
00236                     objEllip->setLocation(m[0], m[1], m[2]);
00237                     objEllip->setColor(1.0,0,0,1.0);
00238                     objEllip->enableDrawSolid3D(true);
00239                     scene->insert( objEllip );*/
00240                 }
00241             }
00242             //win3D->setCameraPointingToPoint(x/s,y/s,3.0);
00243             win3D->unlockAccess3DScene();
00244             win3D->repaint();
00245             fprintf(stderr,"(%lf %lf) s=%lf\n",x/s,y/s,s);
00246             for(unsigned int i=0;i<global_ndts.size();i++) delete global_ndts[i];
00247 
00248         }
00249 
00250         void plotNDTSAccordingToOccupancy(float occupancy, lslgeneric::NDTMap<PointT> *map){
00251             if(win3D == NULL) return;
00252             std::vector<lslgeneric::NDTCell<PointT>*> global_ndts;
00253             global_ndts = map->getAllCells();
00254             fprintf(stderr," NUM NDT: %d ", global_ndts.size());
00255 
00256             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00257             scene->clear();
00258             unsigned int accepted_ndts=1;
00259             double x = 0,y=0,s=0;
00260             for(unsigned int i=0;i<global_ndts.size();i++){
00261                 Eigen::Vector3d m = global_ndts[i]->getMean();
00262                 if(!global_ndts[i]->hasGaussian_) continue;
00263                 x+=m[0];
00264                 y+=m[1];
00265                 s+=1;
00266                 if(global_ndts[i]->getOccupancy()>occupancy){
00267                     accepted_ndts++;
00268                     mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00269                     Eigen::Matrix3d cov = global_ndts[i]->getCov();
00270                     mrpt::math::CMatrixDouble M = cov;
00271                     objEllip->setCovMatrix(M);
00272                     objEllip->setLocation(m[0], m[1], m[2]);
00273 
00274                     objEllip->setColor((m[2]+2.0)/10.0,0,0,0.6);
00275 
00276                     objEllip->enableDrawSolid3D(true);
00277                     scene->insert( objEllip );
00278                 }else if(global_ndts[i]->getOccupancy()<-0){
00279                     /*
00280                     mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00281                     Eigen::Matrix3d cov = global_ndts[i]->getCov();
00282                     cov = cov;
00283                     //Eigen::Vector3d m = global_ndts[i]->getMean();
00284 
00285                     mrpt::math::CMatrixDouble M = cov;
00286                     objEllip->setCovMatrix(M);
00287                     objEllip->setLocation(m[0], m[1], m[2]);
00288                     objEllip->setColor(1.0,0,0,1.0);
00289                     objEllip->enableDrawSolid3D(true);
00290                     scene->insert( objEllip );*/
00291                 }
00292             }
00293             win3D->setCameraPointingToPoint(x/s,y/s,3.0);
00294             win3D->unlockAccess3DScene();
00295             win3D->repaint();
00296             fprintf(stderr,"(%lf %lf) s=%lf\n",x/s,y/s,s);
00297             for(unsigned int i=0;i<global_ndts.size();i++) delete global_ndts[i];
00298 
00299         }
00300 
00301         void plotLocalNDTMap(pcl::PointCloud<PointT> &cloud, double resolution, double R=0, double G=1, double B=0, bool heightCoding=true){
00302             if(win3D == NULL) return;
00303 
00304             lslgeneric::NDTMap<PointT> ndlocal(new lslgeneric::LazyGrid<PointT>(resolution));
00305             ndlocal.addPointCloudSimple(cloud);
00306             ndlocal.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
00307 
00308             std::vector<lslgeneric::NDTCell<PointT>*> ndts;
00309             ndts = ndlocal.getAllCells();
00310             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00311             std::cout<<"LOCAL: "<<ndts.size()<<std::endl;
00312 
00313             for(unsigned int i=0;i<ndts.size();i++){
00314                 Eigen::Vector3d m = ndts[i]->getMean();
00315                 //if(m[2]>3.0) continue;
00316 
00317                 mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00318                 Eigen::Matrix3d cov = ndts[i]->getCov();
00319                 cov = cov;
00320 
00321                 mrpt::math::CMatrixDouble M = 0.5*cov;
00322                 objEllip->setCovMatrix(M);
00323                 objEllip->setLocation(m[0], m[1], m[2]);
00324                 double hval=1.0;
00325                 if(heightCoding){
00326                                 hval = fabs(m[2]+1.5)/20.0;
00327                 }
00328                 objEllip->setColor(R/hval,G/hval,B/hval,0.4);
00329                 objEllip->enableDrawSolid3D(true);
00330                 scene->insert( objEllip );
00331             }
00332             win3D->unlockAccess3DScene();
00333             for(unsigned int i=0;i<ndts.size();i++){
00334                 delete ndts[i];
00335             }
00336 
00337         }
00338         
00339         void plotLocalConflictNDTMap(lslgeneric::NDTMap<PointT> *map, pcl::PointCloud<PointT> &cloud, 
00340                                                                                         double resolution, double R=1, double G=0, double B=0, bool heightCoding=false, double maxz=0){
00341             if(win3D == NULL) return;
00342 
00343             lslgeneric::NDTMap<PointT> ndlocal(new lslgeneric::LazyGrid<PointT>(resolution));
00344             
00345             ndlocal.addPointCloudSimple(cloud);
00346             ndlocal.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
00347 
00348             std::vector<lslgeneric::NDTCell<PointT>*> ndts;
00349             ndts = ndlocal.getAllCells();
00350             
00351             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00352             std::cout<<"LOCAL: "<<ndts.size()<<std::endl;
00353 
00354            for(unsigned int i=0;i<ndts.size();i++){
00355                         
00356                         
00357                         Eigen::Vector3d m = ndts[i]->getMean();
00358                         if(m[2]>maxz) continue;
00359                         PointT p;
00360                         p.x = m[0]; p.y=m[1]; p.z = m[2];
00361                         lslgeneric::NDTCell<PointT> *map_cell=NULL;
00362                         map->getCellAtPoint(p, map_cell);
00363                         if(map_cell == NULL) continue;
00364                         
00365                         if(map_cell->getOccupancy()>0.5) continue;
00366                 
00367                 //      if(m[2]>3.0) continue;
00368 
00369                 mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00370                 Eigen::Matrix3d cov = ndts[i]->getCov();
00371                 cov = cov;
00372 
00373                 mrpt::math::CMatrixDouble M = 0.5*cov;
00374                 objEllip->setCovMatrix(M);
00375                 objEllip->setLocation(m[0], m[1], m[2]);
00376                 double hval=1.0;
00377                 if(heightCoding){
00378                                 hval = fabs(m[2]+1.5)/20.0;
00379                 }
00380                 objEllip->setColor(R/hval,G/hval,B/hval,0.6);
00381                 objEllip->enableDrawSolid3D(true);
00382                 scene->insert( objEllip );
00383             }
00384             win3D->unlockAccess3DScene();
00385             for(unsigned int i=0;i<ndts.size();i++){
00386                 delete ndts[i];
00387             }
00388 
00389         }
00390         
00391         
00392         void plotNDTTraversabilityMap(lslgeneric::NDTMap<PointT> *map){
00393             if(win3D == NULL) return;
00394             std::vector<lslgeneric::NDTCell<PointT>*> global_ndts;
00395             global_ndts = map->getAllCells();
00396             
00397             mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();
00398            
00399             for(unsigned int i=0;i<global_ndts.size();i++){
00400                                 Eigen::Vector3d m = global_ndts[i]->getMean();
00401                                 if(!global_ndts[i]->hasGaussian_) continue;
00402                                 
00403                                 mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00404                                 Eigen::Matrix3d cov = global_ndts[i]->getCov();
00405                                 mrpt::math::CMatrixDouble M = 0.5*cov;
00406                                 objEllip->setCovMatrix(M);
00407                                 objEllip->setLocation(m[0], m[1], m[2]);
00408                                 
00409                                 //lslgeneric::CellClass CC;
00410                                 //lslgeneric::NDTCell<PointT>::CellClass CC;
00411                                 
00412                                 //CC = global_ndts[i]->getClass();
00413                                 // {HORIZONTAL=0, VERTICAL, INCLINED, ROUGH, UNKNOWN};
00414                                 if(global_ndts[i]->getClass() == lslgeneric::NDTCell<PointT>::HORIZONTAL){
00415                                         objEllip->setColor(0,1.0,0,1.0);
00416                                 }else if(global_ndts[i]->getClass() == lslgeneric::NDTCell<PointT>::VERTICAL){
00417                                         objEllip->setColor(1.0,0,0,1.0);
00418                                 }else if(global_ndts[i]->getClass() == lslgeneric::NDTCell<PointT>::INCLINED){
00419                                         objEllip->setColor(1.0,1.0,0,1.0);
00420                                 }else if(global_ndts[i]->getClass() == lslgeneric::NDTCell<PointT>::ROUGH){
00421                                         objEllip->setColor(0,0,1.0,1.0);
00422                                 }else{
00423                                         objEllip->setColor(1.0,1.0,1.0,1.0);
00424                                 }
00425                                 
00426                                 objEllip->enableDrawSolid3D(true);
00427                                 scene->insert( objEllip );
00428                                 
00429                         }
00430                         
00431                         win3D->unlockAccess3DScene();
00432                         win3D->repaint();
00433                         for(unsigned int i=0;i<global_ndts.size();i++) delete global_ndts[i];
00434 
00435 }
00440 
00444         void ndtCoolCam(lslgeneric::NDTMap<PointT> *map ,const Eigen::Affine3d &spos, double maxDist=70.0, 
00445                                                                 unsigned int Nx=800, unsigned int Ny=600, double fx=800, double fy=600){
00446                 Eigen::Matrix3d K;
00447                 K << fx,0,(double)Nx/2.0,
00448                                  0, fy, (double)Ny/2.0,
00449                                  0,0,1;
00450                 
00451                 Eigen::Matrix3d invK;
00452                 double det=0;
00453                 bool exists=false;
00454                 K.computeInverseAndDetWithCheck(invK,det,exists);
00455                 
00456                 if(!exists){
00457                         fprintf(stderr,"ndtCoolCam::ERROR: NO INVERSE!!!!\n");
00458                         return;
00459                 }
00460                 
00461                 Eigen::Matrix3d m1,m2; 
00462                         m1 = Eigen::AngleAxisd(-M_PI/2.0, Eigen::Vector3d::UnitX())
00463                                         * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
00464                                         * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
00465                 
00466                         m2 = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
00467                                         * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
00468                                         * Eigen::AngleAxisd(-M_PI/2.0, Eigen::Vector3d::UnitZ());
00469                 
00470                 mrpt::opengl::CTexturedPlanePtr gl_img          =  mrpt::opengl::CTexturedPlane::Create(0.5,-0.5,-0.5,0.5);
00471                 mrpt::opengl::COpenGLViewportPtr viewInt;
00472                 //mrpt::utils::CImage  img(Nx,Ny,CH_GRAY);
00473                 mrpt::utils::CImage  img(Nx,Ny,CH_RGB);
00474                 
00475                 
00476                 //mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();    
00477                 
00478                 //mrpt::opengl::CSetOfLinesPtr obj = mrpt::opengl::CSetOfLines::Create();               
00479                 for(unsigned int i=0; i<Nx*Ny;i++){
00480                         Eigen::Vector3d pix(i%Nx,(int)i/Nx,1);
00481                         Eigen::Vector3d dirv = invK*pix; 
00482                         dirv.normalize();
00483                         
00484                         Eigen::Vector3d dirv_w = spos.rotation()*(m2*(m1*dirv)); 
00485                         //double max_depth = 20;
00486                         double depth = map->getDepth(spos.translation(), dirv_w, maxDist);
00487                         
00488                         if(depth>maxDist) img.setPixel(i%Nx,i/Nx,0);
00489                         else{
00490                                 float x = (depth/maxDist);
00491                                 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.);
00492                                 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.);
00493                                 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.);
00494                                 size_t color=((unsigned char)(r*255))*255*255 + ((unsigned char)(g*255))*255 + ((unsigned char)(b*255));
00495                                  
00496                                 img.setPixel(i%Nx,i/Nx,color);
00497                                  //fprintf(stderr,"(%.2lf) ",depth);
00498                          }
00499                         /*
00500                          Eigen::Vector3d ray_endpos=spos.translation() + dirv_w * max_depth;
00501                          obj->appendLine(spos.translation()(0),spos.translation()(1),spos.translation()(2),
00502                                                                                         ray_endpos(0), ray_endpos(1), ray_endpos(2));*/
00503                           
00504                 }
00505                 //scene->insert(obj);
00506                 
00507                 mrpt::opengl::COpenGLScenePtr &scene = win3D->get3DSceneAndLock();      
00508                 scene->clear();
00509                 
00510                 viewInt = scene->createViewport("view2d");
00511                 scene->insert( gl_img, "view2d");
00512                 
00513                 viewInt->setViewportPosition(0, 0, -1.0, -1.0);
00514                 
00515                 viewInt->setTransparent(false);
00516                 
00517                 viewInt->getCamera().setOrthogonal(true);
00518                 viewInt->getCamera().setAzimuthDegrees(90);
00519                 viewInt->getCamera().setElevationDegrees(90);
00520                 viewInt->getCamera().setZoomDistance(1.0);
00521                 
00522                 gl_img->assignImage_fast(img);
00523                  
00524                 win3D->unlockAccess3DScene();
00525                 win3D->repaint();
00526         }       
00531         
00532 
00533 
00534 };
00535 
00536 #endif

ndt_visualisation
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Jan 6 2014 11:32:06