osgPCDLoader.h
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2013 University of Jaume-I.
00003  * All rights reserved. This program and the accompanying materials
00004  * are made available under the terms of the GNU Public License v3.0
00005  * which accompanies this distribution, and is available at
00006  * http://www.gnu.org/licenses/gpl.html
00007  * 
00008  * Contributors:
00009  *     Mario Prats
00010  *     Javier Perez
00011  */
00012 
00013 #ifndef OSGPCDLOADER_H_
00014 #define OSGPCDLOADER_H_
00015 
00016 #include "SimulatorConfig.h"
00017 
00018 #include <pcl/io/pcd_io.h>
00019 #include <pcl/point_types.h>
00020 
00021 #include <osg/Geometry>
00022 #include <osg/Geode>
00023 #include <osg/Point>
00024 
00025 template<class T>
00026   class osgPCDLoader
00027   {
00028   private:
00029     osg::ref_ptr<osg::Geometry> geometry;
00030     osg::ref_ptr<osg::Vec3Array> vertices;
00031     osg::ref_ptr<osg::Vec4Array> colors;
00032 
00033   public:
00034     pcl::PointCloud<T> cloud;
00035     osg::ref_ptr<osg::Geode> geode;
00036 
00037     osgPCDLoader(std::string pcd_file)
00038     {
00039       if (pcl::io::loadPCDFile < T > (pcd_file, cloud) == -1) //* load the file
00040       {
00041         std::cerr << "Couldn't read file " << pcd_file << std::endl;
00042       }
00043       else
00044       {
00045         ROS_INFO_STREAM("Loaded " << cloud.width * cloud.height << " data points from " << pcd_file);
00046         createGeode(cloud);
00047       }
00048     }
00049 
00050     osgPCDLoader(const pcl::PointCloud<T> &c)
00051     {
00052       createGeode(c);
00053     }
00054 
00055     void createGeode(const pcl::PointCloud<T> &cloud);
00056 
00057     osg::Geode *getGeode()
00058     {
00059       return geode.get();
00060     }
00061 
00062     ~osgPCDLoader()
00063     {
00064     }
00065 
00066   };
00067 
00068 template<>
00069   void osgPCDLoader<pcl::PointXYZRGB>::createGeode(const pcl::PointCloud<pcl::PointXYZRGB> &cloud)
00070   {
00071     geode = osg::ref_ptr < osg::Geode > (new osg::Geode());
00072     geometry = osg::ref_ptr < osg::Geometry > (new osg::Geometry());
00073     vertices = osg::ref_ptr < osg::Vec3Array > (new osg::Vec3Array());
00074     colors = osg::ref_ptr < osg::Vec4Array > (new osg::Vec4Array());
00075 
00076     //Read vertex and color info from PCD
00077     for (int i = 0; i < cloud.points.size(); i++)
00078     {
00079       vertices->push_back(osg::Vec3(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z));
00080 
00081       uint32_t rgb_val_;
00082       memcpy(&rgb_val_, &(cloud.points[i].rgb), sizeof(uint32_t));
00083 
00084       uint32_t red, green, blue;
00085       blue = rgb_val_ & 0x000000ff;
00086       rgb_val_ = rgb_val_ >> 8;
00087       green = rgb_val_ & 0x000000ff;
00088       rgb_val_ = rgb_val_ >> 8;
00089       red = rgb_val_ & 0x000000ff;
00090 
00091       colors->push_back(osg::Vec4f((float)red / 255.0f, (float)green / 255.0f, (float)blue / 255.0f, 1.0f));
00092     }
00093 
00094     //Set OSG Geometry vertex and colors
00095     geometry->setVertexArray(vertices.get());
00096     geometry->setColorArray(colors.get());
00097     geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
00098 
00099     geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS, 0, vertices->size()));
00100 
00101     geode->addDrawable(geometry.get());
00102     osg::StateSet* state = geometry->getOrCreateStateSet();
00103     state->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
00104 
00105     osg::Point *point = new osg::Point;
00106     point->setSize(2);
00107     state->setAttribute(point);
00108   }
00109 
00110 template<>
00111   void osgPCDLoader<pcl::PointXYZ>::createGeode(const pcl::PointCloud<pcl::PointXYZ> &cloud)
00112   {
00113     geode = osg::ref_ptr < osg::Geode > (new osg::Geode());
00114     geometry = osg::ref_ptr < osg::Geometry > (new osg::Geometry());
00115     vertices = osg::ref_ptr < osg::Vec3Array > (new osg::Vec3Array());
00116     colors = osg::ref_ptr < osg::Vec4Array > (new osg::Vec4Array());
00117 
00118     //Read vertex and color info from PCD
00119     for (int i = 0; i < cloud.points.size(); i++)
00120     {
00121       vertices->push_back(osg::Vec3(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z));
00122       colors->push_back(osg::Vec4f(1.0f, 1.0f, 1.0f, 1.0f));
00123     }
00124 
00125     //Set OSG Geometry vertex and colors
00126     geometry->setVertexArray(vertices.get());
00127     geometry->setColorArray(colors.get());
00128     geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
00129     geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS, 0, vertices->size()));
00130 
00131     geode->addDrawable(geometry);
00132     osg::StateSet* state = geometry->getOrCreateStateSet();
00133     state->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
00134 
00135     osg::Point *point = new osg::Point;
00136     point->setSize(2);
00137     state->setAttribute(point);
00138   }
00139 
00140 #endif
00141 


uwsim
Author(s): Mario Prats , Javier Perez
autogenerated on Fri Aug 28 2015 13:28:58