mcl_visualization.hpp
Go to the documentation of this file.
00001 mrpt::gui::CDisplayWindow3D  win3D("2D NDT-MCL Visualization",800,600);
00002 mrpt::opengl::CGridPlaneXYPtr plane = mrpt::opengl::CGridPlaneXY::Create();
00003 mrpt::opengl::CPointCloudColouredPtr gl_points = mrpt::opengl::CPointCloudColoured::Create();
00004 mrpt::opengl::CPointCloudColouredPtr gl_particles = mrpt::opengl::CPointCloudColoured::Create();
00005 
00006 
00007 
00008 void initializeScene(){
00009         win3D.setCameraAzimuthDeg(00);
00010         win3D.setCameraElevationDeg(00);
00011         win3D.setCameraZoom(1.0);
00012         win3D.setCameraPointingToPoint(0,0,0);
00013         mrpt::opengl::COpenGLScenePtr &scene = win3D.get3DSceneAndLock();
00014         gl_points->setPointSize(10.5);
00015         plane->setPlaneLimits (-250, 250, -250, 250);
00016         scene->insert( plane);
00017         scene->insert( gl_points );
00018         scene->insert( gl_particles );
00019         win3D.unlockAccess3DScene();
00020         win3D.repaint();
00021 }
00022 
00026 void addMap2Scene(lslgeneric::NDTMap<pcl::PointXYZ> &map, Eigen::Vector3d &origin, mrpt::opengl::COpenGLScenePtr scene){
00028         std::vector<lslgeneric::NDTCell<pcl::PointXYZ>*> ndts;
00029         ndts = map.getAllCells();
00030         
00031         for(unsigned int i=0;i<ndts.size();i++){
00032                 mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00033                 Eigen::Matrix3d cov = ndts[i]->getCov();
00034                 Eigen::Vector3d m = ndts[i]->getMean();
00035                 
00036                 if((m-origin).norm()>20.0) continue;
00037                 
00038                 mrpt::math::CMatrixDouble M = cov;
00039                 objEllip->setCovMatrix(M);
00040                 objEllip->setLocation(m[0], m[1], m[2]);
00041                 float g=0,b=0;
00042                 if(ndts[i]->getOccupancy()<0) objEllip->setColor(1.0,g,b,0.4);
00043                 else objEllip->setColor(1.0,1.0,b,1.0);
00044                 objEllip->enableDrawSolid3D(true);
00045                 scene->insert( objEllip );
00046         }
00047         
00048         for(unsigned int i=0;i<ndts.size();i++){
00049                 delete ndts[i];
00050         }
00051         
00052 }
00053 
00054 void addPoseCovariance(double x, double y, Eigen::Matrix3d cov, mrpt::opengl::COpenGLScenePtr scene){
00055         Eigen::Matrix2d c2d;
00056         c2d << cov(0,0), cov(0,1),
00057                                  cov(1,0), cov(1,1);
00058         mrpt::opengl::CMyEllipsoidPtr objEllip = mrpt::opengl::CMyEllipsoid::Create();
00059         mrpt::math::CMatrixDouble M = c2d;
00060         objEllip->setCovMatrix(M);
00061         objEllip->setLocation(x, y, 0.01);
00062         objEllip->setColor(1.0,0,0,0.4);
00063         scene->insert( objEllip );
00064 }
00065 
00066 
00070 void addScanToScene(mrpt::opengl::COpenGLScenePtr scene, Eigen::Vector3d orig, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){
00071         
00072         mrpt::opengl::CSetOfLinesPtr obj = mrpt::opengl::CSetOfLines::Create();
00073         
00074         for(unsigned int i=0;i<cloud->points.size();i+=2){
00075                         obj->appendLine(orig(0),orig(1),orig(2), cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
00076         }
00077         scene->insert(obj);
00078 }
00079 
00080 void addPointCloud2Scene(mrpt::opengl::COpenGLScenePtr scene, pcl::PointCloud<pcl::PointXYZ>::Ptr points){
00081         gl_points->clear();
00082         fprintf(stderr,"Conflict Points Size = %d\n",points->size());
00083         for(unsigned int i=0;i<points->size();i++){
00084                 gl_points->push_back(points->points[i].x, points->points[i].y, points->points[i].z,0,1.0,1.0) ;
00085         }
00086         scene->insert(gl_points);
00087 
00088 }
00089 
00093 void addParticlesToWorld(mcl::CParticleFilter &pf, Eigen::Vector3d gt,Eigen::Vector3d dist_mean, Eigen::Vector3d odometry){
00094         gl_particles->clear();
00095         gl_particles->setPointSize(2.0);
00096         for(int i=0;i<pf.NumOfParticles;i++){
00097                 gl_particles->push_back(pf.Particles[i].x, pf.Particles[i].y,0, 0,1,0);
00098         }
00099         gl_points->push_back(gt[0], gt[1],0.02, 1.0 ,0,0);
00100         //gl_points->push_back(odo[0], odo[1],0.02, 0 ,0,1.0);
00101         
00102         gl_points->push_back(dist_mean[0], dist_mean[1],0.02, 1.0 ,0,1.0);
00103         
00104         gl_points->push_back(odometry[0], odometry[1],0.02, 1.0 ,1.0,1.0);
00105 
00106 }


ndt_mcl
Author(s): jari
autogenerated on Mon Jan 6 2014 11:32:07