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 = %lu\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
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 }