├── manifest.xml
├── cmake
    ├── FindLIBLAS.cmake
    ├── FindE57RefImpl.cmake
    ├── FindPCL_IO_EXTRA.cmake
    └── FindXerces.cmake
├── README
├── include
    └── pcl
    │   ├── double_utils.h
    │   ├── extra_point_type.h
    │   └── io
    │       ├── cloud_collection.h
    │       ├── impl
    │           └── cloud_collection.hpp
    │       ├── pts_io.h
    │       ├── xyz_io.h
    │       ├── vtx_io.h
    │       ├── xya_io.h
    │       ├── las_io.h
    │       ├── ptx_io.h
    │       ├── e57_io.h
    │       ├── cloud_io.h
    │       ├── filereaderd.h
    │       └── points_to_rosmsg.h
├── tools
    ├── cvt2xyz.cpp
    ├── cvt2ptx.cpp
    └── cvt2pcd.cpp
├── CMakeLists.txt
└── src
    ├── cloud_io.cpp
    ├── las_io.cpp
    ├── double_utils.cpp
    ├── xyz_io.cpp
    ├── xya_io.cpp
    ├── pts_io.cpp
    ├── cloud_collection.cpp
    ├── vtx_io.cpp
    ├── e57_io.cpp
    └── ptx_io.cpp


/manifest.xml:
--------------------------------------------------------------------------------
 1 | <package>
 2 |   <description brief="pcl_io_extra">
 3 | 
 4 |      pcl_io_extra
 5 | 
 6 |   </description>
 7 |   <author>Adam Stambler</author>
 8 |   <license>BSD</license>
 9 |   <review status="unreviewed" notes=""/>
10 |   <url>http://github.com/adasta/pcl_io_extra</url>
11 | 
12 |   <!-- PCL -->
13 |   <rosdep name="pcl" />
14 |   
15 |   <export>
16 |     <cpp cflags="-I${prefix}/include " 
17 |     lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lpcl_io_extra"/>
18 |   </export>
19 | 
20 | 
21 | </package>
22 | 
23 | 
24 | 


--------------------------------------------------------------------------------
/cmake/FindLIBLAS.cmake:
--------------------------------------------------------------------------------
 1 |   
 2 | include(FindPackageHandleStandardArgs)
 3 | 
 4 | find_program(LIBLAS_CONFIG_PROG
 5 | 			liblas-config
 6 | 			/usr/local/bin
 7 | )
 8 | 
 9 | if (LIBLAS_CONFIG_PROG)
10 | 
11 | exec_program(${LIBLAS_CONFIG_PROG}  ARGS  --libs OUTPUT_VARIABLE
12 |                LIBLAS_LIBRARIES)
13 | 
14 | set(LIBLAS_LIBRARIES ${LIBLAS_LIBRARIES} las)
15 | exec_program(${LIBLAS_CONFIG_PROG} ARGS --includes OUTPUT_VARIABLE
16 |                LIBLAS_INCLUDE_DIRS)
17 | 
18 | exec_program(${LIBLAS_CONFIG_PROG} ARGS --defines OUTPUT_VARIABLE
19 |                LIBLAS_DEFINES)
20 | 
21 | exec_program(${LIBLAS_CONFIG_PROG} ARGS --cxxflags OUTPUT_VARIABLE
22 |                LIBLAS_CXXFLAGS)
23 | 
24 | else()
25 | 	MESSAGE(FATAL_ERROR, "FindLIBAS.cmaek.  liblas-config not found. liblas-config=${LIBLAS_CONFIG_PROG}")
26 | endif(LIBLAS_CONFIG_PROG)
27 | 
28 | 
29 | find_package_handle_standard_args(LIBLAS  DEFAULT_MSG
30 |                                    LIBLAS_INCLUDE_DIRS LIBLAS_LIBRARIES)
31 | 
32 | 


--------------------------------------------------------------------------------
/cmake/FindE57RefImpl.cmake:
--------------------------------------------------------------------------------
 1 | # $Id: FindE57RefImpl.cmake   $
 2 | 
 3 | if (E57RefImpl_FIND_VERSION)
 4 |     message(WARNING "Finding a specific version of E57RefImpl is not supported.")
 5 | endif (E57RefImpl_FIND_VERSION)
 6 | 
 7 | 
 8 | find_package(Xerces)
 9 | 
10 | if(Xerces_FOUND)
11 | 
12 | 	find_path(E57RefImpl_ROOT
13 | 			NAMES E57RefImplConfig.cmake
14 | 			PATHS /usr/local/e57 /usr/E57RefImpl
15 | 		)
16 | 
17 | 
18 | 	if (E57RefImpl_ROOT)
19 | 	set(E57RefImpl_LIBRARY_DIRS "${E57RefImpl_ROOT}/lib" 
20 | 								${Xerces_LIBRARY_DIRS})
21 | 	set(E57RefImpl_INCLUDE_DIRS "${E57RefImpl_ROOT}/include" 
22 | 								${Xerces_INCLUDE_DIRS})
23 | 	set(E57RefImpl_LIBRARIES E57RefImpl ${Xerces_LIBRARIES} ${Xerces_LIBRARY} )
24 | 	endif()
25 | 
26 | 
27 | 	find_package_handle_standard_args(E57RefImpl  DEFAULT_MSG
28 | 									E57RefImpl_LIBRARY_DIRS 
29 | 									E57RefImpl_INCLUDE_DIRS 
30 | 									E57RefImpl_LIBRARIES)
31 | else()
32 | 
33 | message(STATUS "Cannot use E57.  Xerces was not found")
34 | endif()
35 | 


--------------------------------------------------------------------------------
/README:
--------------------------------------------------------------------------------
 1 | pcl_io_extra
 2 | By Adam Stambler of Carnegie Mellon University
 3 | 
 4 | This is a package of additional io modules for PCL. 
 5 | It is meant to make it easier to just take a random point cloud file
 6 | and read it into your PCL program.  
 7 | 
 8 | Reader Support
 9 | PTX
10 | E57
11 | LAS
12 | 
13 | Write Support:
14 | 
15 | 
16 | 
17 | 
18 | --------------Library Requrements-------------------
19 | 
20 | Xerces
21 | Both E57 and LAS use Xerces.  You want Xerces 3.0 or greater.
22 | 
23 | sudo apt-get install libxerces-c-dev
24 | 
25 | 
26 | 
27 | LAS Support requires liblas.  
28 | You can get liblas from : http://liblas.org/index.html
29 | 
30 | git clone git://github.com/libLAS/libLAS.git liblas
31 | 
32 | Do not use the debian install for Ubuntu 
33 | The debian install on 11.10 is too old.  
34 | 
35 | 
36 | E57 
37 | E57 support depends on libe57 ( http://www.libe57.org/)
38 | 
39 | You need to compile your own E57:
40 | 
41 | svn co https://e57-3d-imgfmt.svn.sourceforge.net/svnroot/e57-3d-imgfmt/trunk e57refimpl
42 | 


--------------------------------------------------------------------------------
/include/pcl/double_utils.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * double.h
 3 |  *
 4 |  *  Created on: Aug 8, 2012
 5 |  *      Author: Adam Stambler
 6 |  */
 7 | 
 8 | #ifndef DOUBLE_UTILS_H_
 9 | #define DOUBLE_UTILS_H_
10 | 
11 | #include <Eigen/Core>
12 | #include <pcl/PCLPointCloud2.h>
13 | 
14 | #include <pcl/point_types.h>
15 | #include <pcl/point_cloud.h>
16 | 
17 | namespace pcl{
18 | 
19 | 
20 | bool hasDoublePointXYZ(const pcl::PCLPointCloud2& cloud);
21 | 
22 | 
23 | template<typename PointTA, typename PointTB>
24 | void cvtAndOffset( const pcl::PointCloud<PointTA>& cloudA, const pcl::PointCloud<PointTB>& cloudB, Eigen::Vector3d& offset){
25 |     if ( cloudB.points.size() != cloudA.points.size()){
26 |       cloudB.points.resize(cloudA.size());
27 |     }
28 |     for(int i=0; i<cloudA.points.size(); i++){
29 |       cloudB.points[i].x = cloudA.points[i].x+offset[0];
30 |       cloudB.points[i].y = cloudA.points[i].y+offset[1];
31 |       cloudB.points[i].z = cloudA.points[i].z+offset[2];
32 |     }
33 |     cloudB.header = cloudA.header;
34 |     cloudB.width = cloudA.width;
35 |     cloudB.height=cloudA.height;
36 |   }
37 | 
38 | 
39 | bool cvtToDoubleAndOffset( const pcl::PCLPointCloud2& cloudA, pcl::PCLPointCloud2& cloudB,
40 |     Eigen::Vector3d& offset);
41 | 
42 | }
43 | 
44 | #endif /* DOUBLE_H_ */
45 | 


--------------------------------------------------------------------------------
/cmake/FindPCL_IO_EXTRA.cmake:
--------------------------------------------------------------------------------
 1 |   
 2 | include(FindPackageHandleStandardArgs)
 3 | 
 4 | find_path(PCL_IO_EXTRA_ROOT
 5 | 			include/pcl/io/cloud_io.h
 6 | 			/usr/local/include
 7 | )
 8 | 
 9 | MESSAGE( "IO ROOT IS " ${PCL_IO_EXTRA_ROOT}) 
10 | 
11 | set(PCL_IO_EXTRA_INCLUDE_DIRS ${PCL_IO_EXTRA_ROOT}/include)
12 | set(PCL_IO_EXTRA_LIBRARY_DIRS  ${PCL_IO_EXTRA_ROOT}/lib)
13 | set(PCL_IO_EXTRA_LIBRARIES   pcl_io_extra )
14 | 
15 | FIND_PACKAGE(E57RefImpl)
16 | 
17 | if (E57RefImpl_FOUND)
18 | 	message(STATUS "Adding E57 Support")
19 | 	set(PCL_IO_EXTRA_INCLUDE_DIRS ${PCL_IO_EXTRA_INCLUDE_DIRS} ${E57RefImpl_INCLUDE_DIRS})
20 | 	set( PCL_IO_EXTRA_LIBRARY_DIRS ${PCL_IO_EXTRA_LIBRARY_DIRS} ${E57RefImpl_LIBRARY_DIRS} )
21 | 	set(PCL_IO_EXTRA_LIBRARIES  ${PCL_IO_EXTRA_LIBRARIES} pcl_e57 )
22 | 	add_definitions(-DE57)
23 | endif()
24 | 
25 | 
26 | FIND_PACKAGE(LIBLAS)
27 | if (LIBLAS_FOUND)
28 | 	set(PCL_IO_EXTRA_INCLUDE_DIRS ${PCL_IO_EXTRA_INCLUDE_DIRS} ${LIBLAS_INCLUDE_DIRS} )
29 | 	set( PCL_IO_EXTRA_LIBRARY_DIRS ${PCL_IO_EXTRA_LIBRARY_DIRS} ${LIBLAS_LIBRARIES} )
30 | 	set(PCL_IO_EXTRA_LIBRARIES  ${PCL_IO_EXTRA_LIBRARIES} pcl_las )
31 |  	add_definitions(-DLAS)
32 | endif()
33 | 
34 | find_package_handle_standard_args(PCL_IO_EXTRA  DEFAULT_MSG
35 |                                    PCL_IO_EXTRA_ROOT  
36 |                                    PCL_IO_EXTRA_INCLUDE_DIRS 
37 |                                    PCL_IO_EXTRA_LIBRARY_DIRS)
38 | 
39 | 


--------------------------------------------------------------------------------
/include/pcl/extra_point_type.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * intensity_point_type.h
 3 |  *
 4 |  *  Created on: Jul 20, 2012
 5 |  *      Author: Adam Stambler
 6 |  */
 7 | 
 8 | #ifndef EXTRA_POINT_TYPE_H_
 9 | #define EXTRA_POINT_TYPE_H_
10 | 
11 | #include <pcl/point_types.h>
12 | 
13 | 
14 | namespace pcl{
15 |   /** \brief A point structure representing Euclidean xyz coordinates, and the intensity value.
16 |     * \ingroup common
17 |     */
18 |  // struct  Intensity
19 |  // {
20 |  //       float intensity;
21 |   // };
22 | 
23 |   struct PointXYZD{
24 |       union {
25 |          double data[3]; \
26 |          struct {
27 |            double x;
28 |            double y;
29 |            double z;
30 |          };
31 |        };
32 |        inline Eigen::Map<Eigen::Vector3d> getVector3dMap () { return (Eigen::Vector3d::Map (data)); }
33 |        inline const Eigen::Map<const Eigen::Vector3d> getVector3dMap () const { return (Eigen::Vector3d::Map (data)); }
34 | };
35 | }
36 | 
37 | POINT_CLOUD_REGISTER_POINT_STRUCT ( pcl::PointXYZD,
38 |                                     (double, x, x)
39 |                                     (double, y, y)
40 |                                     (double, z, z)
41 |                                     )
42 | 
43 | 
44 | //POINT_CLOUD_REGISTER_POINT_STRUCT ( pcl::Intensity,
45 |    //                               (float, intensity, intensity)
46 |      //                             )
47 | 
48 | 
49 | 
50 | #endif /* INTENSITY_POINT_TYPE_H_ */
51 | 


--------------------------------------------------------------------------------
/include/pcl/io/cloud_collection.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * CloudCollection.h
 3 |  *
 4 |  *  Created on: Feb 21, 2012
 5 |  *      Author: asher
 6 |  */
 7 | 
 8 | #ifndef CLOUDCOLLECTION_H_
 9 | #define CLOUDCOLLECTION_H_
10 | 
11 | #include <pcl/point_cloud.h>
12 | #include <pcl/point_types.h>
13 | #include <pcl/PCLPointCloud2.h>
14 | 
15 | #include <boost/any.hpp>
16 | #include <map>
17 | #include <string>
18 | 
19 | 
20 | namespace pcl {
21 |   namespace traits{
22 |   template<typename T> struct pointName {static const std::string name;};
23 |   }
24 | }
25 | 
26 | namespace pcl
27 | {
28 |   namespace io{
29 |   class CloudCollection
30 |   {
31 |   public:
32 |     CloudCollection ();
33 |     virtual
34 |     ~CloudCollection ();
35 | 
36 |     typedef boost::shared_ptr<CloudCollection> Ptr;
37 | 
38 |     template<typename PointT> typename pcl::PointCloud<PointT>::Ptr
39 |     getCloud();
40 | 
41 |     template<typename PointT>
42 |     void getCloud(typename pcl::PointCloud<PointT>::Ptr& cloud);
43 | 
44 | 
45 |     template<typename PointT> void
46 |     setCloud(typename pcl::PointCloud<PointT>::Ptr);
47 | 
48 |     void getPointCloud2(pcl::PCLPointCloud2& cloud);
49 | 
50 |     void getCloudList(std::vector<std::string>& clouds);
51 | 
52 |     bool load(const std::string& name);
53 |     void save(const std::string& name);
54 | 
55 |   protected:
56 |     std::map<std::string, boost::any> point_map_;
57 | 
58 |   };
59 |   } // namespace io
60 | } /* namespace pcl */
61 | 
62 | #include "impl/cloud_collection.hpp"
63 | #endif /* CLOUDCOLLECTION_H_ */
64 | 


--------------------------------------------------------------------------------
/tools/cvt2xyz.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | 
 3 | #include <iostream>
 4 | #include <boost/program_options.hpp>
 5 | #include <pcl/io/pcd_io.h>
 6 | #include <pcl/io/ptx_io.h>
 7 | 
 8 | namespace po=boost::program_options;
 9 | 
10 | int main(int argc, char** argv){
11 |   po::options_description desc("./cvt2xyz input.pcd output.xyz\n converts PointXYZI clouds to ptx format\n");
12 |   std::string infile;
13 |   std::string ofile;
14 | 
15 |   desc.add_options()
16 |     ("help", "produce help message")
17 |     ("input,i",po::value<std::string>(&infile)->required(), "input point cloud ")
18 |     ("output,o",po::value<std::string>(&ofile)->required(), "output xyz ")
19 |           ;
20 | 
21 |   po::positional_options_description p;
22 |   p.add("input",1);
23 |   p.add("output",1);
24 | 
25 |   po::variables_map vm;
26 |  try{
27 |   po::store(po::command_line_parser(argc, argv).
28 |   options(desc).positional(p).run(), vm);
29 |   po::notify(vm);
30 |  }
31 |  catch( const std::exception& e)
32 |  {
33 |      std::cerr << e.what() << std::endl;
34 |      std::cout << desc << std::endl;
35 |      return 1;
36 |  }
37 |  if (vm.count("help") ){
38 |    std::cout << desc << std::endl;
39 |    return 0;
40 |  }
41 | 
42 |  pcl::PointCloud<pcl::PointXYZI> cloud;
43 |  if(pcl::io::loadPCDFile(infile, cloud) <0 ){
44 | 	 std::cout << "Failed to read input \n";
45 | 	 return -1;
46 |  }
47 | 
48 | std::ofstream osfile( ofile.c_str());
49 | 
50 | for(int i=0; i< cloud.size(); i++){
51 | 	osfile << cloud[i].x << " " << cloud[i].y << " " << cloud[i].z << "\n";
52 | }
53 | 
54 | return 0;
55 | }
56 | 


--------------------------------------------------------------------------------
/tools/cvt2ptx.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | 
 3 | #include <iostream>
 4 | #include <boost/program_options.hpp>
 5 | #include <pcl/io/pcd_io.h>
 6 | #include <pcl/io/ptx_io.h>
 7 | 
 8 | namespace po=boost::program_options;
 9 | 
10 | int main(int argc, char** argv){
11 |   po::options_description desc("./cvt2ptx input.pcd output.ptx\n converts PointXYZI clouds to ptx format\n");
12 |   std::string infile;
13 |   std::string ofile;
14 | 
15 |   desc.add_options()
16 |     ("help", "produce help message")
17 |     ("input,i",po::value<std::string>(&infile)->required(), "input point cloud ")
18 |     ("output,o",po::value<std::string>(&ofile)->required(), "output ptx ")
19 |           ;
20 | 
21 |   po::positional_options_description p;
22 |   p.add("input",1);
23 |   p.add("output",1);
24 | 
25 |   po::variables_map vm;
26 |  try{
27 |   po::store(po::command_line_parser(argc, argv).
28 |   options(desc).positional(p).run(), vm);
29 |   po::notify(vm);
30 |  }
31 |  catch( const std::exception& e)
32 |  {
33 |      std::cerr << e.what() << std::endl;
34 |      std::cout << desc << std::endl;
35 |      return 1;
36 |  }
37 |  if (vm.count("help") ){
38 |    std::cout << desc << std::endl;
39 |    return 0;
40 |  }
41 | 
42 |  pcl::PointCloud<pcl::PointXYZI> cloud;
43 |  if(pcl::io::loadPCDFile(infile, cloud) <0 ){
44 | 	 std::cout << "Failed to read input \n";
45 | 	 return -1;
46 |  }
47 | cloud.sensor_orientation_ = Eigen::Quaternionf::Identity();
48 | cloud.sensor_origin_ << 0,0,0,1;
49 | 
50 |  std::cout<<"writing to " << ofile<< "\n";
51 |  pcl::PTXWriter writer;
52 |  writer.write(ofile, cloud, true);
53 | return 0;
54 | }
55 | 


--------------------------------------------------------------------------------
/include/pcl/io/impl/cloud_collection.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * cloud_collection.hpp
 3 |  *
 4 |  *  Created on: Feb 21, 2012
 5 |  *      Author: asher
 6 |  */
 7 | 
 8 | #ifndef CLOUD_COLLECTION_HPP_
 9 | #define CLOUD_COLLECTION_HPP_
10 | 
11 | #include "../cloud_collection.h"
12 | #include <pcl/common/io.h>
13 | 
14 | #include <iostream>
15 | 
16 | namespace pcl{
17 |   namespace io {
18 |   template<typename PointT> typename pcl::PointCloud<PointT>::Ptr
19 |   CloudCollection::getCloud(){
20 | 
21 |     typename pcl::PointCloud<PointT>::Ptr cloud;
22 | 
23 |     std::vector<pcl::PCLPointField> fields;
24 |     std::string key ="";
25 |     pcl::getFields<PointT>(fields);
26 | 
27 |     for(int i=0; i< fields.size(); i++){
28 |       char tmp[30];
29 |       sprintf(tmp, "%s-%d-%d_", fields[i].name.c_str(),fields[i].datatype, fields[i].count);
30 |       key = key+tmp;
31 |     }
32 | 
33 |     try
34 |     {
35 |       return boost::any_cast<typename pcl::PointCloud<PointT>::Ptr >(point_map_[key]);
36 |     }
37 |     catch(std::exception & e)
38 |     {
39 |      return cloud;
40 |     }
41 | 
42 |   }
43 | 
44 |   template<typename PointT>
45 |   void CloudCollection::getCloud(typename pcl::PointCloud<PointT>::Ptr& cloud){
46 |   	    std::vector<pcl::PCLPointField> fields;
47 |   	    std::string key ="";
48 |   	    pcl::getFields<PointT>(fields);
49 |   	    cloud.reset();
50 |   	    for(int i=0; i< fields.size(); i++){
51 |   	      char tmp[30];
52 |   	      sprintf(tmp, "%s-%d-%d_", fields[i].name.c_str(),fields[i].datatype, fields[i].count);
53 |   	      key = key+tmp;
54 |   	    }
55 | 
56 |   	    try
57 |   	    {
58 |   	      cloud = boost::any_cast<typename pcl::PointCloud<PointT>::Ptr >(point_map_[key]);
59 |   	    }
60 |   	    catch(std::exception & e)
61 |   	    {
62 |   	    }
63 |   }
64 | 
65 |   template<typename PointT> void
66 |   CloudCollection::setCloud(typename pcl::PointCloud<PointT>::Ptr cloud){
67 |         std::vector<pcl::PCLPointField> fields;
68 |         std::string key ="";
69 |         pcl::getFields<PointT>(fields);
70 | 
71 |         for(int i=0; i< fields.size(); i++){
72 |           char tmp[30];
73 |           sprintf(tmp, "%s-%d-%d_", fields[i].name.c_str(),fields[i].datatype, fields[i].count);
74 |           key = key+tmp;
75 |         }
76 |         point_map_[key] = cloud;
77 |   }
78 | }
79 | }
80 | #endif /* CLOUD_COLLECTION_HPP_ */
81 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required (VERSION 2.6)
 2 | project(pcl_io_extra)
 3 | 
 4 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_LIST_DIR}/cmake /usr/local/E57RefImpl)
 5 | 
 6 | set(CMAKE_BUILD_TYPE "RELEASE")
 7 | 
 8 | #set the default path for built executables to the "bin" directory
 9 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin)
10 | #set the default path for built libraries to the "lib" directory
11 | set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
12 | link_directories(${CMAKE_BINARY_DIR}/lib)
13 | 
14 | find_package(PCL 1.6 REQUIRED COMPONENTS  io )
15 | include_directories(${PCL_INCLUDE_DIRS})
16 | link_directories(${PCL_LIBRARY_DIRS})
17 | add_definitions(${PCL_DEFINITIONS})
18 | 
19 | FIND_PACKAGE( Boost 1.40 COMPONENTS 
20 | 					program_options filesystem thread
21 | 					serialization python  system
22 | 					REQUIRED )
23 | INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIR} )
24 | include_directories( include)
25 | 
26 | 
27 | add_library(pcl_io_extra src/cloud_io.cpp 
28 | 						 src/ptx_io.cpp 
29 | 						 src/pts_io.cpp
30 | 						 src/xyz_io.cpp
31 | 						 src/vtx_io.cpp
32 | 						 src/xya_io.cpp
33 | 						 src/cloud_collection.cpp
34 | 						 src/double_utils.cpp
35 | 						 )
36 | target_link_libraries(pcl_io_extra ${Boost_LIBRARIES}  ${PCL_LIBRARIES})
37 | 
38 | 
39 | 
40 | 
41 | #FIX E57Cmake !!!
42 | #FIND_PACKAGE(E57RefImpl)
43 | if (E57REFIMPL_FOUND)
44 | 	message(STATUS "Adding E57 Support")
45 | 	include_directories(${E57RefImpl_INCLUDE_DIRS})
46 | 	link_directories(${E57RefImpl_LIBRARY_DIRS} )
47 | 	add_library( pcl_e57 src/e57_io.cpp)
48 | 	target_link_libraries(pcl_e57 ${Boost_LIBRARIES}  ${PCL_LIBRARIES} ${E57RefImpl_LIBRARIES} )
49 | 	add_definitions(-DE57)
50 | endif()
51 | 
52 | 
53 | FIND_PACKAGE(LIBLAS)
54 | if (LIBLAS_FOUND)
55 | 	add_library(pcl_las src/las_io.cpp)
56 | 	target_link_libraries(pcl_las ${Boost_LIBRARIES}  ${PCL_LIBRARIES} ${LIBLAS_LIBRARIES})
57 | 	include_directories(${LIBLAS_INCLUDE_DIRS})
58 | 	add_definitions(-DLAS)
59 | endif()
60 | 
61 |  add_executable(cvt2pcd tools/cvt2pcd.cpp)
62 |  target_link_libraries(cvt2pcd pcl_io_extra ${Boost_LIBRARIES}  ${PCL_LIBRARIES})
63 | 
64 | #add_executable(cvt2ptx tools/cvt2ptx.cpp)
65 | #target_link_libraries(cvt2ptx pcl_io_extra ${Boost_LIBRARIES}  ${PCL_LIBRARIES})
66 | 
67 | #add_executable(cvt2xyz tools/cvt2xyz.cpp)
68 | #target_link_libraries(cvt2xyz pcl_io_extra ${Boost_LIBRARIES}  ${PCL_LIBRARIES})
69 | 
70 | 
71 | if (E57REFIMPL_FOUND)
72 | target_link_libraries(cvt2pcd pcl_e57)
73 | INSTALL(TARGETS pcl_e57  
74 |   ARCHIVE DESTINATION lib
75 | )
76 | endif()
77 | 
78 | if (LIBLAS_FOUND)
79 | target_link_libraries(cvt2pcd pcl_las ${LIBLAS_LIBRARIES})
80 | INSTALL(TARGETS pcl_las  
81 |   LIBRARY DESTINATION lib
82 |     ARCHIVE DESTINATION lib
83 | )
84 | endif()
85 | 
86 | 
87 | 
88 | INSTALL(TARGETS pcl_io_extra cvt2pcd
89 |   RUNTIME DESTINATION bin
90 |   LIBRARY DESTINATION lib
91 |   ARCHIVE DESTINATION lib
92 | )
93 | 
94 | INSTALL(DIRECTORY include/pcl  DESTINATION include)
95 | 


--------------------------------------------------------------------------------
/src/cloud_io.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * CloudReader.cpp
 3 |  *
 4 |  *  Created on: May 21, 2012
 5 |  *      Author: asher
 6 |  */
 7 | 
 8 | #include <pcl/io/cloud_io.h>
 9 | 
10 | #include <pcl/io/pcd_io.h>
11 | #include <pcl/io/ptx_io.h>
12 | #include <pcl/io/pts_io.h>
13 | #include <pcl/io/xyz_io.h>
14 | #include <pcl/io/vtx_io.h>
15 | #include <pcl/io/ascii_io.h>
16 | 
17 | #include <algorithm>
18 | 
19 | 
20 | 
21 | namespace pcl
22 | {
23 |     CloudReader::CloudReader ()
24 |     {
25 |       // TODO Auto-generated constructor stub
26 | 
27 |       reader_map_["pcd"] = new pcl::PCDReader;
28 |       reader_map_["ptx"] = new pcl::PTXReader;
29 |       reader_map_["pts"] = new pcl::PTSReader;
30 |       reader_map_["vtx"] = new pcl::VTXReader;
31 |       reader_map_["xyz"] = new pcl::XYZAsciiReader;
32 |       reader_map_["txt"] = new pcl::ASCIIReader;
33 |     }
34 | 
35 |     CloudReader::~CloudReader ()
36 |     {
37 |       // TODO Auto-generated destructor stub
38 |       for(std::map<std::string, FileReader*>::iterator iter= reader_map_.begin();
39 |           iter!= reader_map_.end(); iter++){
40 |         delete iter->second;
41 |       }
42 |     }
43 | 
44 |     bool CloudReader::registerExtension(std::string ext, FileReader *reader)
45 |     {
46 |       //TODO check for . and remove it
47 | 
48 |       reader_map_[ext] = reader;
49 |       return true;
50 |     }
51 | 
52 |       void CloudReader::getSupportedExtensions(std::vector<std::string> extensions)
53 |       {
54 |         for(std::map<std::string, FileReader*>::iterator iter= reader_map_.begin();
55 |           iter!= reader_map_.end(); iter++){
56 |           extensions.push_back(iter->first);
57 |         }
58 |       }
59 | 
60 |       int CloudReader::readHeader(const std::string & file_name, pcl::PCLPointCloud2 & cloud, Eigen::Vector4f & origin, Eigen::Quaternionf & orientation, int & file_version, int & data_type, unsigned int & data_idx, const int offset)
61 |       {
62 |         FileReader* reader = chooseReader(file_name);
63 |         if (reader == NULL) return -1;
64 |         return reader->readHeader(file_name, cloud,origin,orientation,file_version, data_type, data_idx, offset);
65 |       }
66 | 
67 |       int CloudReader::read(const std::string & file_name, pcl::PCLPointCloud2 & cloud, Eigen::Vector4f & origin, Eigen::Quaternionf & orientation, int & file_version, const int offset)
68 |       {
69 |         FileReader* reader = chooseReader(file_name);
70 |         if (reader == NULL) return -1;
71 |         return reader->read(file_name, cloud, origin, orientation, file_version, offset);
72 |       }
73 | 
74 |       FileReader *CloudReader::chooseReader(std::string file_name)
75 |       {
76 |         //figure out the extension
77 |          size_t pos = file_name.rfind('.');
78 |          if (pos == std::string::npos) {
79 |            pcl::console::print_error("[CloudReader] Input file %s does not have extension.\n", file_name.c_str());
80 |            return NULL;
81 |          }
82 |          std::string ext = file_name.substr(pos+1,file_name.size()-pos);
83 |          size_t p2 = ext.find_last_not_of( " \t\n\r" );
84 |          if( std::string::npos != p2 )
85 |            ext.erase( p2+1 );
86 |          else
87 |            ext.clear();
88 | 
89 |          if (reader_map_.count(ext) ==0 ) {
90 |            pcl::console::print_error("[CloudReader] %s extension not supported.\n", ext.c_str());
91 |            return NULL;
92 |          }
93 |          return reader_map_[ext];
94 |       }
95 | } /* namespace pcl */
96 | 


--------------------------------------------------------------------------------
/include/pcl/io/pts_io.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * pts_io.h
 3 |  *
 4 |  *  Created on: Aug 5, 2012
 5 |  *      Author: Adam Stambler
 6 |  */
 7 | 
 8 | #ifndef PTS_IO_H_
 9 | #define PTS_IO_H_
10 | 
11 | 
12 | #include <pcl/io/file_io.h>
13 | 
14 | namespace pcl{
15 | 
16 |   class PTSReader : public FileReader {
17 | 
18 |   public:
19 |     PTSReader();
20 |     virtual ~PTSReader();
21 |     /* Load only the meta information (number of points, their types, etc),
22 |             * and not the points themselves, from a given FILE file. Useful for fast
23 |             * evaluation of the underlying data structure.
24 |             *
25 |             * Returns:
26 |             *  * < 0 (-1) on error
27 |             *  * > 0 on success
28 |             * \param[in] file_name the name of the file to load
29 |             * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
30 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
31 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
32 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
33 |             * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
34 |             * \param[out] data_idx the offset of cloud data within the file
35 |             * \param[in] offset the offset in the file where to expect the true header to begin.
36 |             * One usage example for setting the offset parameter is for reading
37 |             * data from a TAR "archive containing multiple files: TAR files always
38 |             * add a 512 byte header in front of the actual file, so set the offset
39 |             * to the next byte after the header (e.g., 513).
40 |             */
41 |           virtual int
42 |           readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
43 |                       Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
44 |                       int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) ;
45 | 
46 | 
47 |           /** \brief Read a point cloud data from a FILE file and store it into a sensor_msgs/PointCloud2.
48 |             * \param[in] file_name the name of the file containing the actual PointCloud data
49 |             * \param[out] cloud the resultant PointCloud message read from disk
50 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
51 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
52 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
53 |             * \param[in] offset the offset in the file where to expect the true header to begin.
54 |             * One usage example for setting the offset parameter is for reading
55 |             * data from a TAR "archive containing multiple files: TAR files always
56 |             * add a 512 byte header in front of the actual file, so set the offset
57 |             * to the next byte after the header (e.g., 513).
58 |             */
59 |           virtual int
60 |           read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
61 |                 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version,
62 |                 const int offset = 0);
63 | 
64 | 
65 |   };
66 | 
67 | }
68 | #endif /* PTS_IO_H_ */
69 | 


--------------------------------------------------------------------------------
/include/pcl/io/xyz_io.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * pts_io.h
 3 |  *
 4 |  *  Created on: Aug 5, 2012
 5 |  *      Author: Adam Stambler
 6 |  */
 7 | 
 8 | #ifndef XYZ_ASCII_IO_H_
 9 | #define XYZ_ASCII_IO_H_
10 | 
11 | 
12 | #include <pcl/io/file_io.h>
13 | 
14 | namespace pcl{
15 | 
16 |   class XYZAsciiReader : public FileReader {
17 | 
18 |   public:
19 |     XYZAsciiReader();
20 |     virtual ~XYZAsciiReader();
21 |     /* Load only the meta information (number of points, their types, etc),
22 |             * and not the points themselves, from a given FILE file. Useful for fast
23 |             * evaluation of the underlying data structure.
24 |             *
25 |             * Returns:
26 |             *  * < 0 (-1) on error
27 |             *  * > 0 on success
28 |             * \param[in] file_name the name of the file to load
29 |             * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
30 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
31 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
32 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
33 |             * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
34 |             * \param[out] data_idx the offset of cloud data within the file
35 |             * \param[in] offset the offset in the file where to expect the true header to begin.
36 |             * One usage example for setting the offset parameter is for reading
37 |             * data from a TAR "archive containing multiple files: TAR files always
38 |             * add a 512 byte header in front of the actual file, so set the offset
39 |             * to the next byte after the header (e.g., 513).
40 |             */
41 |           virtual int
42 |           readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
43 |                       Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
44 |                       int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) ;
45 | 
46 | 
47 |           /** \brief Read a point cloud data from a FILE file and store it into a sensor_msgs/PointCloud2.
48 |             * \param[in] file_name the name of the file containing the actual PointCloud data
49 |             * \param[out] cloud the resultant PointCloud message read from disk
50 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
51 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
52 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
53 |             * \param[in] offset the offset in the file where to expect the true header to begin.
54 |             * One usage example for setting the offset parameter is for reading
55 |             * data from a TAR "archive containing multiple files: TAR files always
56 |             * add a 512 byte header in front of the actual file, so set the offset
57 |             * to the next byte after the header (e.g., 513).
58 |             */
59 |           virtual int
60 |           read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
61 |                 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version,
62 |                 const int offset = 0);
63 | 
64 | 
65 |   };
66 | 
67 | }
68 | #endif /* PTS_IO_H_ */
69 | 


--------------------------------------------------------------------------------
/tools/cvt2pcd.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * pcl_e572pcd.cpp
  3 |  *
  4 |  *  Created on: May 21, 2012
  5 |  *      Author: asher
  6 |  */
  7 | 
  8 | 
  9 | #include <pcl/io/pcd_io.h>
 10 | #include <pcl/io/cloud_io.h>
 11 | #include <pcl/io/ptx_io.h>
 12 | #include <pcl/point_types.h>
 13 | #include <pcl/common/transforms.h>
 14 | 
 15 | #include <boost/program_options.hpp>
 16 | #include <boost/filesystem.hpp>
 17 | 
 18 | #include <pcl/double_utils.h>
 19 | 
 20 | namespace po=boost::program_options;
 21 | 
 22 | #ifdef E57
 23 | #include <pcl/io/e57_io.h>
 24 | #endif
 25 | #ifdef LAS
 26 | #include <pcl/io/las_io.h>
 27 | #endif
 28 | 
 29 | 
 30 | #include <iostream>
 31 | #include <vector>
 32 | 
 33 | int main(int argc, char** argv){
 34 | 
 35 |   po::options_description desc("./cvt2pcd [options] input_cloud output_pcd");
 36 | 
 37 |   desc.add_options()
 38 |           ("help", "produce help message")
 39 |           ("input,i",po::value<std::string>()->required(), "input point cloud ")
 40 |           ("output,o",po::value<std::string>(), "output pcd ")
 41 |           ("view_offset,v", po::value< std::string>(), "Offset view offset to translate and convert a float64 to float32 XYZ")
 42 |           ("ascii,a", "PCD should be in asci format.  (Default binary compressed)")
 43 |           ;
 44 | 
 45 |   if (argc <3 ){
 46 |            std::cout << "Incorrect number of arguments\n";
 47 |            desc.print(std::cout);
 48 |            return -1;
 49 |    }
 50 |   po::positional_options_description p;
 51 |   p.add("input",1);
 52 |   p.add("output",1);
 53 | 
 54 |   po::variables_map vm;
 55 |  try{
 56 |   po::store(po::command_line_parser(argc, argv).
 57 |   options(desc).positional(p).run(), vm);
 58 |   po::notify(vm);
 59 |  }
 60 |  catch( const std::exception& e)
 61 |  {
 62 |      std::cerr << e.what() << std::endl;
 63 |      std::cout << desc << std::endl;
 64 |      return 1;
 65 |  }
 66 | 
 67 |   pcl::CloudReader reader;
 68 | 
 69 |   std::string ifile, ofile;
 70 |   ifile = vm["input"].as<std::string>();
 71 |   if(vm.count("output")) ofile = vm["output"].as<std::string>();
 72 |   else {
 73 |     ofile = ifile;
 74 |     boost::filesystem::path opath = ofile;
 75 |     opath.replace_extension(".pcd");
 76 |     ofile = opath.string();
 77 |   }
 78 | 
 79 | #ifdef E57
 80 |   std::cout << "Adding E57 Support : \n";
 81 |   reader.registerExtension("e57", new pcl::E57Reader( ));
 82 | #endif
 83 | #ifdef LAS
 84 |   reader.registerExtension("las", new pcl::LASReader );
 85 | #endif
 86 |   pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
 87 | 
 88 |   Eigen::Vector4f origin;
 89 |   Eigen::Quaternionf rot;
 90 | 
 91 |   int fv;
 92 |   std::clog << "Loading " <<  ifile << " \n";
 93 |   int c = reader.read(ifile,*cloud, origin, rot, fv);
 94 |   std::clog << "There were " << c << " points \n";
 95 | 
 96 |   if (pcl::hasDoublePointXYZ(*cloud) ){
 97 |     if (vm.count("view_offset")){
 98 |       Eigen::Vector3d voff;
 99 | 
100 |       if ( 3 == sscanf(vm["view_offset"].as<std::string>().c_str(), "%lf,%lf,%lf", &voff[0], &voff[1], &voff[2]) ){
101 |         std::cout << "Translating by " << voff.transpose() << " \n";
102 |         pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
103 |         pcl::cvtToDoubleAndOffset(*cloud,*cloud2,voff);
104 |         cloud = cloud2;
105 |       }
106 |       else{
107 |         std::cout << "Invalid view offset of : " << vm["view_offset"].as<std::string>() << " \n";
108 |       }
109 |     }
110 |   }
111 | 
112 | 
113 |   pcl::PCDWriter writer;
114 |   writer.write(ofile, cloud,origin, rot, !vm.count("ascii"));
115 |   std::cout << ofile << "\n";
116 | }
117 | 


--------------------------------------------------------------------------------
/include/pcl/io/vtx_io.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * vtx_io.h
 3 |  *
 4 |  *  Created on: Aug 5, 2012
 5 |  *      Author: Adam Stambler
 6 |  */
 7 | 
 8 | #ifndef VTX_IO_H_
 9 | #define VTX_IO_H_
10 | 
11 | 
12 | #include <pcl/io/file_io.h>
13 | 
14 | namespace pcl{
15 | /*
16 |  * Geomagic vtx format.  This reader only handles vtx with xyz ijk
17 |  * If you just have xyz data, use the xyzasciireader
18 |  */
19 |   class VTXReader : public FileReader {
20 | 
21 |   public:
22 |     VTXReader();
23 |     virtual ~VTXReader();
24 |     /* Load only the meta information (number of points, their types, etc),
25 |             * and not the points themselves, from a given FILE file. Useful for fast
26 |             * evaluation of the underlying data structure.
27 |             *
28 |             * Returns:
29 |             *  * < 0 (-1) on error
30 |             *  * > 0 on success
31 |             * \param[in] file_name the name of the file to load
32 |             * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
33 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
34 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
35 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
36 |             * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
37 |             * \param[out] data_idx the offset of cloud data within the file
38 |             * \param[in] offset the offset in the file where to expect the true header to begin.
39 |             * One usage example for setting the offset parameter is for reading
40 |             * data from a TAR "archive containing multiple files: TAR files always
41 |             * add a 512 byte header in front of the actual file, so set the offset
42 |             * to the next byte after the header (e.g., 513).
43 |             */
44 |           virtual int
45 |           readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
46 |                       Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
47 |                       int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) ;
48 | 
49 | 
50 |           /** \brief Read a point cloud data from a FILE file and store it into a sensor_msgs/PointCloud2.
51 |             * \param[in] file_name the name of the file containing the actual PointCloud data
52 |             * \param[out] cloud the resultant PointCloud message read from disk
53 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
54 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
55 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
56 |             * \param[in] offset the offset in the file where to expect the true header to begin.
57 |             * One usage example for setting the offset parameter is for reading
58 |             * data from a TAR "archive containing multiple files: TAR files always
59 |             * add a 512 byte header in front of the actual file, so set the offset
60 |             * to the next byte after the header (e.g., 513).
61 |             */
62 |           virtual int
63 |           read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
64 |                 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version,
65 |                 const int offset = 0);
66 | 
67 | 
68 |   };
69 | 
70 | }
71 | #endif /* PTS_IO_H_ */
72 | 


--------------------------------------------------------------------------------
/include/pcl/io/xya_io.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * vtx_io.h
 3 |  *
 4 |  *  Created on: Aug 5, 2012
 5 |  *      Author: Adam Stambler
 6 |  */
 7 | 
 8 | #ifndef XYA_IO_H_
 9 | #define XYA_IO_H_
10 | 
11 | 
12 | #include <pcl/io/file_io.h>
13 | 
14 | namespace pcl{
15 | /*
16 |  * Geomagic xya format.  This reader only handles xya with xyz intensity
17 |  * If you just have xyz data, use the xyzasciireader
18 |  */
19 |   class XYAReader : public FileReader {
20 | 
21 |   public:
22 |     XYAReader();
23 |     virtual ~XYAReader();
24 |     /* Load only the meta information (number of points, their types, etc),
25 |             * and not the points themselves, from a given FILE file. Useful for fast
26 |             * evaluation of the underlying data structure.
27 |             *
28 |             * Returns:
29 |             *  * < 0 (-1) on error
30 |             *  * > 0 on success
31 |             * \param[in] file_name the name of the file to load
32 |             * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
33 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
34 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
35 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
36 |             * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
37 |             * \param[out] data_idx the offset of cloud data within the file
38 |             * \param[in] offset the offset in the file where to expect the true header to begin.
39 |             * One usage example for setting the offset parameter is for reading
40 |             * data from a TAR "archive containing multiple files: TAR files always
41 |             * add a 512 byte header in front of the actual file, so set the offset
42 |             * to the next byte after the header (e.g., 513).
43 |             */
44 |           virtual int
45 |           readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
46 |                       Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
47 |                       int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) ;
48 | 
49 | 
50 |           /** \brief Read a point cloud data from a FILE file and store it into a sensor_msgs/PointCloud2.
51 |             * \param[in] file_name the name of the file containing the actual PointCloud data
52 |             * \param[out] cloud the resultant PointCloud message read from disk
53 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
54 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
55 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
56 |             * \param[in] offset the offset in the file where to expect the true header to begin.
57 |             * One usage example for setting the offset parameter is for reading
58 |             * data from a TAR "archive containing multiple files: TAR files always
59 |             * add a 512 byte header in front of the actual file, so set the offset
60 |             * to the next byte after the header (e.g., 513).
61 |             */
62 |           virtual int
63 |           read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
64 |                 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version,
65 |                 const int offset = 0);
66 | 
67 | 
68 |   };
69 | 
70 | }
71 | #endif /* PTS_IO_H_ */
72 | 


--------------------------------------------------------------------------------
/src/las_io.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | 
  3 | #include <pcl/io/las_io.h>
  4 | #include <liblas/reader.hpp>
  5 | #include <liblas/writer.hpp>
  6 | 
  7 | #include <pcl/PCLPointCloud2.h>
  8 | 
  9 | #include <fstream>  // std::ifstream
 10 | #include <iostream> // std::cout
 11 | 
 12 | pcl::LASReader::LASReader()
 13 | {
 14 | 
 15 | }
 16 | 
 17 | pcl::LASReader::~LASReader()
 18 | {
 19 | }
 20 | 
 21 | int pcl::LASReader::readHeader(const std::string & file_name, pcl::PCLPointCloud2 & cloud,
 22 |                                Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
 23 |                                int & file_version, int & data_type, unsigned int & data_idx, const int offset)
 24 | {
 25 |   std::ifstream ifs;
 26 |   ifs.open(file_name.c_str(), std::ios::in | std::ios::binary);
 27 | 
 28 |   liblas::Reader reader(ifs);
 29 | 
 30 |   liblas::Header const& header = reader.GetHeader();
 31 | 
 32 |   //todo
 33 | }
 34 | 
 35 | 
 36 | 
 37 | /*
 38 |  *
 39 | Base properties of all points regardless of dataformat_id Name
 40 | x
 41 | y
 42 | z
 43 | intensity
 44 | return_number
 45 | number_of_returns
 46 | scan_direction
 47 | flightline_edge
 48 | classification
 49 | scan_angle
 50 | user_data
 51 | 
 52 | For PCL, I am just going to use x,y,z, itensity and colro
 53 |  */
 54 | 
 55 | int pcl::LASReader::read(const std::string & file_name,
 56 |                          pcl::PCLPointCloud2 & cloud,
 57 |                          Eigen::Vector4f & origin,
 58 |                          Eigen::Quaternionf & orientation,
 59 |                          int & file_version, const int offset)
 60 | {
 61 |   std::ifstream ifs;
 62 |   ifs.open(file_name.c_str(), std::ios::in | std::ios::binary);
 63 | 
 64 |   liblas::Reader reader(ifs);
 65 | 
 66 | 
 67 |   unsigned int idx = 0;
 68 | 
 69 |   unsigned int nr_points;
 70 | 
 71 |   // Setting the is_dense property to true by default
 72 |   cloud.is_dense = true;
 73 | 
 74 |   {
 75 |     pcl::PCLPointField f;
 76 |   f.datatype = pcl::PCLPointField::FLOAT64;
 77 |   f.count= 1;
 78 |   f.name="x";
 79 |   cloud.fields.push_back(f);
 80 |   }
 81 | 
 82 |   {
 83 |   pcl::PCLPointField f;
 84 |   f.datatype = pcl::PCLPointField::FLOAT64;
 85 |   f.count= 1;
 86 |   f.name="y";
 87 |   f.offset =8;
 88 |   cloud.fields.push_back(f);
 89 |   }
 90 |   {
 91 |   pcl::PCLPointField f;
 92 |   f.datatype = pcl::PCLPointField::FLOAT64;
 93 |   f.count= 1;
 94 |   f.name="z";
 95 |   f.offset =16;
 96 |   cloud.fields.push_back(f);
 97 |   }
 98 | 
 99 |   {
100 |   pcl::PCLPointField f;
101 |   f.datatype = pcl::PCLPointField::FLOAT32;
102 |   f.count= 1;
103 |   f.name="intensity";
104 |   f.offset =24;
105 |   cloud.fields.push_back(f);
106 |   }
107 | 
108 |   const int point_size = 28;
109 |   cloud.data.resize( reader.GetHeader().GetPointRecordsCount() * point_size);
110 |   cloud.height =1;
111 |   cloud.width = reader.GetHeader().GetPointRecordsCount();
112 |   cloud.point_step =point_size;
113 | 
114 |   for(uint64_t i=0; reader.ReadNextPoint(); i++)
115 |   {
116 |       liblas::Point const& p = reader.GetPoint();
117 |        *( (double *) ( cloud.data.data() +point_size*i     ) )=  p.GetX();
118 |        *( (double *) ( cloud.data.data() + point_size*i +8 ) )=  p.GetY();
119 |        *( (double *) ( cloud.data.data() + point_size*i +16 ) )=  p.GetZ();
120 |        *( (float *) ( cloud.data.data() + point_size*i +24) ) = p.GetIntensity();
121 |   }
122 |   return cloud.width*cloud.height;
123 | }
124 | 
125 | pcl::LASWriter::LASWriter()
126 | {
127 | }
128 | 
129 | 
130 | pcl::LASWriter::~LASWriter()
131 | {
132 | }
133 | 
134 | 
135 | int pcl::LASWriter::write(const std::string & file_name, const pcl::PCLPointCloud2 & cloud, const Eigen::Vector4f & origin, const Eigen::Quaternionf & orientation, const bool binary)
136 | {
137 | }
138 | 
139 | 


--------------------------------------------------------------------------------
/src/double_utils.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * double_utils.cpp
  3 |  *
  4 |  *  Created on: Aug 8, 2012
  5 |  *      Author: Adam Stambler
  6 |  */
  7 | 
  8 | 
  9 | #include <pcl/double_utils.h>
 10 | 
 11 | bool pcl::hasDoublePointXYZ (const pcl::PCLPointCloud2& cloud)
 12 | {
 13 |   for(int i=0; i<cloud.fields.size(); i++){
 14 |     bool is_coord=false;
 15 |     if ((cloud.fields[i].name== "x") && (cloud.fields[i].datatype == pcl::PCLPointField::FLOAT64) ){
 16 |       return true;
 17 |     }
 18 |     if ((cloud.fields[i].name== "y") && (cloud.fields[i].datatype == pcl::PCLPointField::FLOAT64) ){
 19 |       return true;
 20 |     }
 21 |     if ((cloud.fields[i].name== "z") && (cloud.fields[i].datatype == pcl::PCLPointField::FLOAT64) ){
 22 |       return true;
 23 |     }
 24 |   }
 25 | }
 26 | 
 27 | bool pcl::cvtToDoubleAndOffset( const pcl::PCLPointCloud2& cloudA, pcl::PCLPointCloud2& cloudB, Eigen::Vector3d& offset){
 28 | 
 29 |   cloudB.fields.clear();
 30 | 
 31 |   int xidx = -1,yidx=-1, zidx =-1;
 32 |   int byte_offset=0;
 33 |   std::vector<int> copy_idx;
 34 |   std::vector<int> length;
 35 |   int point_size=0;
 36 |   for(int i=0; i<cloudA.fields.size(); i++){
 37 |     bool is_coord=false;
 38 |     if (cloudA.fields[i].name== "x"){
 39 |       xidx=i;
 40 |       is_coord=true;
 41 |     }
 42 |     else if (cloudA.fields[i].name== "y"){
 43 |       yidx=i;
 44 |       is_coord=true;
 45 |     }
 46 |     else if (cloudA.fields[i].name== "z"){
 47 |       zidx=i;
 48 |       is_coord=true;
 49 |     }
 50 |     if(is_coord){
 51 |       cloudB.fields.push_back(cloudA.fields[i]);
 52 |       cloudB.fields[i].datatype = pcl::PCLPointField::FLOAT32;
 53 |       cloudB.fields[i].count=1;
 54 |       cloudB.fields[i].offset = byte_offset;
 55 |       byte_offset+=4;
 56 |       point_size+=4;
 57 |     }
 58 |     else{
 59 |       cloudB.fields.push_back(cloudA.fields[i]);
 60 |       cloudB.fields[i].offset = byte_offset;
 61 |       copy_idx.push_back(i);
 62 |       switch (cloudB.fields.back().datatype){
 63 |         case pcl::PCLPointField::FLOAT64:
 64 |           byte_offset+=8;
 65 |           point_size+=8;
 66 |           length.push_back(8);
 67 |           break;
 68 |         case pcl::PCLPointField::FLOAT32:
 69 |         case pcl::PCLPointField::INT32:
 70 |         case pcl::PCLPointField::UINT32:
 71 |           byte_offset+=4;
 72 |           point_size+= 4;
 73 |           length.push_back(4);
 74 |           break;
 75 |         case pcl::PCLPointField::UINT16:
 76 |         case pcl::PCLPointField::INT16:
 77 |           byte_offset+=2;
 78 |           point_size+= 2;
 79 |           length.push_back(2);
 80 |           break;
 81 |         case pcl::PCLPointField::INT8:
 82 |         case pcl::PCLPointField::UINT8:
 83 |           byte_offset+=1;
 84 |           byte_offset+=1;
 85 |           length.push_back(1);
 86 |           break;
 87 |       }
 88 |     }
 89 |   }
 90 | 
 91 |   if ( (xidx<0) || (yidx <0) || (zidx<0) ) return false;
 92 | 
 93 |   cloudB.point_step = point_size;
 94 |   cloudB.width =cloudA.width;
 95 |   cloudB.height=cloudA.height;
 96 |   cloudB.header=cloudA.header;
 97 | 
 98 | 
 99 |   int npts = cloudB.width*cloudB.height;
100 |   cloudB.data.resize(npts*point_size);
101 | 
102 |   uint8_t* dataB = cloudB.data.data();
103 |   const uint8_t* dataA = cloudA.data.data();
104 | 
105 |   for(int i=0; i<npts; i++){
106 |    *( (float*) (dataB +cloudB.fields[xidx].offset))= *((double *)(dataA+ cloudA.fields[xidx].offset)) + offset[0];
107 |    *( (float*) (dataB +cloudB.fields[yidx].offset)) =*((double *)(dataA+ cloudA.fields[yidx].offset))  + offset[1];
108 |    *( (float*) (dataB +cloudB.fields[zidx].offset)) =*((double *)(dataA+ cloudA.fields[zidx].offset))  + offset[2];
109 | 
110 |     for(int j=0; j<copy_idx.size(); j++){
111 |       memcpy( dataB +cloudB.fields[copy_idx[j]].offset, dataA+ cloudA.fields[copy_idx[j]].offset, length[j]);
112 |     }
113 |     dataB+= point_size;
114 |     dataA += cloudA.point_step;
115 |   }
116 |   return true;
117 |   }
118 | 


--------------------------------------------------------------------------------
/src/xyz_io.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * pts_io.cpp
  3 |  *
  4 |  *  Created on: May 21, 2012
  5 |  *      Author: asher
  6 |  */
  7 | 
  8 | #include <pcl/io/xyz_io.h>
  9 | #include <istream>
 10 | #include <fstream>
 11 | #include <boost/filesystem.hpp>
 12 | #include <boost/algorithm/string.hpp>
 13 | 
 14 | pcl::XYZAsciiReader::XYZAsciiReader()
 15 | {
 16 | }
 17 | 
 18 | 
 19 | 
 20 | pcl::XYZAsciiReader::~XYZAsciiReader()
 21 | {
 22 | }
 23 | 
 24 | 
 25 | 
 26 | int pcl::XYZAsciiReader::readHeader(const std::string & file_name, pcl::PCLPointCloud2 & cloud,
 27 |                                Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
 28 |                                int & file_version, int & data_type, unsigned int & data_idx,
 29 |                                const int offset)
 30 | {
 31 | 
 32 |   boost::filesystem::path fpath  = file_name;
 33 | 
 34 |   if ( !boost::filesystem::exists(fpath) ){
 35 |     pcl::console::print_error("[XYZAscciReader] File %s does not exist.\n", file_name.c_str());
 36 |     return -1;
 37 |   }
 38 |   if ( fpath.extension().string() != ".xyz"){
 39 |     pcl::console::print_error("[XYZAscciReader] File does not have xyz extension. \n");
 40 |     return -1;
 41 |   }
 42 | 
 43 |   origin = Eigen::Vector4f::Zero();
 44 |   orientation = Eigen::Quaternionf();
 45 | 
 46 |  {
 47 |   pcl::PCLPointField f;
 48 |   f.datatype = pcl::PCLPointField::FLOAT32;
 49 |   f.count= 1;
 50 |   f.name="x";
 51 |   cloud.fields.push_back(f);
 52 |   }
 53 | 
 54 |   {
 55 |   pcl::PCLPointField f;
 56 |   f.datatype = pcl::PCLPointField::FLOAT32;
 57 |   f.count= 1;
 58 |   f.name="y";
 59 |   f.offset =4;
 60 |   cloud.fields.push_back(f);
 61 |   }
 62 |   {
 63 |   pcl::PCLPointField f;
 64 |   f.datatype = pcl::PCLPointField::FLOAT32;
 65 |   f.count= 1;
 66 |   f.name="z";
 67 |   f.offset =8;
 68 |   cloud.fields.push_back(f);
 69 |   }
 70 | 
 71 |  cloud.point_step= 12;
 72 | 
 73 |   std::fstream ifile(file_name.c_str(), std::fstream::in);
 74 |   std::string line;
 75 |   int total=0;
 76 |   while(std::getline(ifile,line)){
 77 |     boost::algorithm::trim(line);
 78 |     if (line.find_first_not_of("#") !=0 ) continue;
 79 |     std::vector<std::string> tokens;
 80 |     boost::algorithm::split(tokens, line,boost::algorithm::is_any_of(", \n\r\t"), boost::algorithm::token_compress_on);
 81 |     if (tokens.size() ==3) total++;
 82 |   }
 83 |   cloud.height=1;
 84 |   cloud.width = total;
 85 |   ifile.close();
 86 | 
 87 |   return cloud.height * cloud.width;
 88 | }
 89 | 
 90 | 
 91 | int pcl::XYZAsciiReader::read(const std::string & file_name, pcl::PCLPointCloud2 & cloud,
 92 |                          Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
 93 |                          int & file_version, const int offset)
 94 | {
 95 | 
 96 |   int  data_type;
 97 |   unsigned int data_idx;
 98 |   if ( this->readHeader(file_name,cloud,origin,orientation,file_version, data_type, data_idx, offset) <0 ) return -1;
 99 |   cloud.data.resize(cloud.height*cloud.width*cloud.point_step);
100 | 
101 |   std::string line;
102 |   std::fstream ifile(file_name.c_str(), std::fstream::in);
103 | 
104 |     int total=0;
105 | 
106 |     uint32_t ptoffset=0;
107 |     while(std::getline(ifile,line)){
108 |       boost::algorithm::trim(line);
109 |       if (line.find_first_not_of("#") !=0 ) continue;
110 |          std::vector<std::string> tokens;
111 |          boost::algorithm::split(tokens, line,boost::algorithm::is_any_of(", \n\r\t"), boost::algorithm::token_compress_on);
112 |          if (tokens.size() !=3) continue;
113 |        std::stringstream ss(line);
114 |        float x=0,y=0,z=0;
115 |        ss >> x ;
116 |        ss >> y ;
117 |        ss >> z ;
118 |        if ((x==0) && (y==0) && (z==0)){
119 |          x=y=z = std::numeric_limits<float>::quiet_NaN();
120 |        }
121 |        float *data = ( (float *) ( cloud.data.data() + ptoffset) )  ;
122 |        *( ( data    ) )  = x;
123 |        *( ( data +1 ) )  =y;
124 |        *( ( data +2 ) )  =z;
125 |        ptoffset+= cloud.point_step;
126 |     }
127 |   return cloud.width*cloud.height;
128 | }
129 | 
130 | 


--------------------------------------------------------------------------------
/src/xya_io.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * pts_io.cpp
  3 |  *
  4 |  *  Created on: May 21, 2012
  5 |  *      Author: asher
  6 |  */
  7 | 
  8 | #include <pcl/io/xya_io.h>
  9 | #include <istream>
 10 | #include <fstream>
 11 | #include <boost/filesystem.hpp>
 12 | #include <boost/algorithm/string.hpp>
 13 | 
 14 | pcl::XYAReader::XYAReader()
 15 | {
 16 | }
 17 | 
 18 | 
 19 | 
 20 | pcl::XYAReader::~XYAReader()
 21 | {
 22 | }
 23 | 
 24 | 
 25 | 
 26 | int pcl::XYAReader::readHeader(const std::string & file_name, pcl::PCLPointCloud2 & cloud,
 27 |                                Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
 28 |                                int & file_version, int & data_type, unsigned int & data_idx,
 29 |                                const int offset)
 30 | {
 31 | 
 32 |   boost::filesystem::path fpath  = file_name;
 33 | 
 34 |   if ( !boost::filesystem::exists(fpath) ){
 35 |     pcl::console::print_error("[XYAReader] File %s does not exist.\n", file_name.c_str());
 36 |     return -1;
 37 |   }
 38 |   if ( fpath.extension().string() != ".xya"){
 39 |     pcl::console::print_error("[XYAReader] File does not have xya extension. \n");
 40 |     return -1;
 41 |   }
 42 | 
 43 |   origin = Eigen::Vector4f::Zero();
 44 |   orientation = Eigen::Quaternionf();
 45 | 
 46 |  {
 47 |   pcl::PCLPointField f;
 48 |   f.datatype = pcl::PCLPointField::FLOAT32;
 49 |   f.count= 1;
 50 |   f.name="x";
 51 |   cloud.fields.push_back(f);
 52 |   }
 53 | 
 54 |   {
 55 |   pcl::PCLPointField f;
 56 |   f.datatype = pcl::PCLPointField::FLOAT32;
 57 |   f.count= 1;
 58 |   f.name="y";
 59 |   f.offset =4;
 60 |   cloud.fields.push_back(f);
 61 |   }
 62 |   {
 63 |   pcl::PCLPointField f;
 64 |   f.datatype = pcl::PCLPointField::FLOAT32;
 65 |   f.count= 1;
 66 |   f.name="z";
 67 |   f.offset =8;
 68 |   cloud.fields.push_back(f);
 69 |   }
 70 | 
 71 |   {
 72 |   pcl::PCLPointField f;
 73 |   f.datatype = pcl::PCLPointField::FLOAT32;
 74 |   f.count= 1;
 75 |   f.name="rgb";
 76 |   f.offset =12;
 77 |   cloud.fields.push_back(f);
 78 |   }
 79 | 
 80 |  cloud.point_step= 16;
 81 | 
 82 |   std::fstream ifile(file_name.c_str(), std::fstream::in);
 83 |   std::string line;
 84 |   int total=0;
 85 |   while(std::getline(ifile,line)){
 86 |   total++;
 87 |   }
 88 |   cloud.height=1;
 89 |   cloud.width = total;
 90 |   ifile.close();
 91 | 
 92 |   origin = Eigen::Vector4f();
 93 |   orientation = Eigen::Quaternionf();
 94 | 
 95 |   return cloud.height * cloud.width;
 96 | }
 97 | 
 98 | 
 99 | int pcl::XYAReader::read(const std::string & file_name, pcl::PCLPointCloud2 & cloud,
100 |                          Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
101 |                          int & file_version, const int offset)
102 | {
103 | 
104 |   int  data_type;
105 |   unsigned int data_idx;
106 |   if ( this->readHeader(file_name,cloud,origin,orientation,file_version, data_type, data_idx, offset) <0 ) return -1;
107 |   cloud.data.resize(cloud.height*cloud.width*cloud.point_step);
108 | 
109 |   std::string line;
110 |   std::fstream ifile(file_name.c_str(), std::fstream::in);
111 | 
112 |     int total=0;
113 | 
114 |     uint32_t ptoffset=0;
115 |     while(std::getline(ifile,line)){
116 |       boost::algorithm::trim(line);
117 |       if (line.find_first_not_of("#") !=0 ) continue;
118 |          std::vector<std::string> tokens;
119 |          boost::algorithm::split(tokens, line,boost::algorithm::is_any_of(", \n\r\t"), boost::algorithm::token_compress_on);
120 |          if (tokens.size() !=6) continue;
121 |          std::stringstream ss(line);
122 |         float x=0,y=0,z=0;
123 |         int r=0, g=0, b=0;
124 |         ss >> y ;
125 |         ss >> z ;
126 |         ss >> r ;
127 |         ss >> g ;
128 |         ss >> b;
129 |         if ((x==0) && (y==0) && (z==0)){
130 |           x=y=z = std::numeric_limits<float>::quiet_NaN();
131 |         }
132 |         float *data = ( (float *) ( cloud.data.data() + ptoffset) )  ;
133 |         *( ( data    ) )  = x;
134 |         *( ( data +1 ) )  =y;
135 |         *( ( data +2 ) )  =z;
136 |         uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g<< 8 | (uint32_t)b);
137 |         *( ( data+ 3) )  =  *reinterpret_cast<float*>(&rgb);
138 |         ptoffset+= cloud.point_step;
139 |     }
140 |   return cloud.width*cloud.height;
141 | }
142 | 
143 | 


--------------------------------------------------------------------------------
/cmake/FindXerces.cmake:
--------------------------------------------------------------------------------
  1 | # $Id: FindXerces.cmake 65 2010-06-28 14:58:29Z roland_schwarz $
  2 | 
  3 | # Copyright 2010 Roland Schwarz, Riegl LMS GmbH
  4 | #
  5 | # Permission is hereby granted, free of charge, to any person or organization
  6 | # obtaining a copy of the software and accompanying documentation covered by
  7 | # this license (the "Software") to use, reproduce, display, distribute,
  8 | # execute, and transmit the Software, and to prepare derivative works of the
  9 | # Software, and to permit third-parties to whom the Software is furnished to
 10 | # do so, all subject to the following:
 11 | #
 12 | # The copyright notices in the Software and this entire statement, including
 13 | # the above license grant, this restriction and the following disclaimer,
 14 | # must be included in all copies of the Software, in whole or in part, and
 15 | # all derivative works of the Software, unless such copies or derivative
 16 | # works are solely in the form of machine-executable object code generated by
 17 | # a source language processor.
 18 | #
 19 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 20 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 21 | # FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
 22 | # SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
 23 | # FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
 24 | # ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 25 | # DEALINGS IN THE SOFTWARE.
 26 | #
 27 | #
 28 | #Modified by Adam Stambler ( 6/15/2012) to add linux and mac support
 29 | 
 30 | if (Xerces_FIND_VERSION)
 31 |     message(WARNING "Finding a specific version of Xerces is not supported.")
 32 | endif (Xerces_FIND_VERSION)
 33 | 
 34 | if( NOT $ENV{XERCES_ROOT} STREQUAL "" )
 35 |     set(XERCES_ROOT $ENV{XERCES_ROOT})
 36 | endif()
 37 | 
 38 | find_path(XERCES_ROOT
 39 |         NAMES include/xercesc/dom/DOM.hpp
 40 |         PATHS /usr/local/ /usr/
 41 |     )
 42 | 
 43 | find_path(Xerces_INCLUDE_DIR
 44 |     xercesc/sax2/SAX2XMLReader.hpp
 45 |     ${XERCES_ROOT}/include
 46 | )
 47 | 
 48 | 
 49 | if (WIN32)
 50 | if (Xerces_USE_STATIC_LIBS)
 51 |     find_library(Xerces_sLIBRARY_DEBUG
 52 |         NAMES xerces-c_static_3D.lib libxerces-c.a
 53 |         PATHS ${XERCES_ROOT}/lib
 54 |     )
 55 | 
 56 |     find_library(Xerces_LIBRARY_RELEASE
 57 |         NAMES xerces-c_static_3.lib libxerces-c.a
 58 |         PATHS ${XERCES_ROOT}/lib
 59 |     )
 60 | else(Xerces_USE_STATIC_LIBS)
 61 |     find_library(Xerces_LIBRARY_DEBUG
 62 |         NAMES xerces-c_3D.lib libxerces-c.dll.a
 63 |         PATHS ${XERCES_ROOT}/lib
 64 |     )
 65 | 
 66 |     find_library(Xerces_LIBRARY_RELEASE
 67 |         NAMES xerces-c_3.lib libxerces-c.dll.a
 68 |         PATHS ${XERCES_ROOT}/lib
 69 |     )
 70 | endif(Xerces_USE_STATIC_LIBS)
 71 | endif(WIN32)
 72 | 
 73 | if (APPLE)
 74 | if (Xerces_USE_STATIC_LIBS)
 75 |     find_library(Xerces_LIBRARY_DEBUG
 76 |         NAMES  libxerces-c.dylib
 77 |         PATHS ${XERCES_ROOT}/lib
 78 |     )
 79 | 
 80 |     find_library(Xerces_LIBRARY_RELEASE
 81 |         NAMES  libxerces-c.dylib
 82 |         PATHS ${XERCES_ROOT}/lib
 83 |     )
 84 | else(Xerces_USE_STATIC_LIBS)
 85 |     find_library(Xerces_LIBRARY_DEBUG
 86 |         NAMES  libxerces-c.a
 87 |         PATHS ${XERCES_ROOT}/lib
 88 |     )
 89 | 
 90 |     find_library(Xerces_LIBRARY_RELEASE
 91 |         NAMES  libxerces-c.a
 92 |         PATHS ${XERCES_ROOT}/lib
 93 |     )
 94 | endif(Xerces_USE_STATIC_LIBS)
 95 | endif(APPLE)
 96 | 
 97 | if(UNIX AND NOT APPLE)
 98 |     set(Xerces_LIBRARY_DIR ${XERCES_ROOT}/lib)
 99 |     set(Xerces_LIBRARY xerces-c)
100 |     set(Xerces_LIBRARY_DEBUG xerces-c)
101 |     set(Xerces_LIBRARY_RELEASE xerces-c)
102 | endif()
103 | 
104 | mark_as_advanced(
105 |     Xerces_INCLUDE_DIR
106 |     Xerces_LIBRARY_DEBUG
107 |     Xerces_LIBRARY_RELEASE
108 | )
109 | 
110 | include(FindPackageHandleStandardArgs)
111 | find_package_handle_standard_args(
112 |     Xerces
113 |     "Unable to find Xerces: \nPlease set the XERCES_ROOT variable to the Xerces directory.\n"
114 |     Xerces_INCLUDE_DIR
115 |     Xerces_LIBRARY_DEBUG
116 |     Xerces_LIBRARY_RELEASE
117 | )
118 | 
119 | set(Xerces_FOUND ${XERCES_FOUND})
120 | 


--------------------------------------------------------------------------------
/include/pcl/io/las_io.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * las_io.h
 3 |  *
 4 |  *  Created on: May 9, 2012
 5 |  *      Author: asher
 6 |  */
 7 | 
 8 | #ifndef LAS_IO_H_
 9 | #define LAS_IO_H_
10 | 
11 | #include <pcl/io/file_io.h>
12 | 
13 | 
14 | namespace pcl{
15 | 
16 |   class LASReader : public FileReader {
17 | 
18 |   public:
19 |     LASReader();
20 |     virtual ~LASReader();
21 |     /* Load only the meta information (number of points, their types, etc),
22 |             * and not the points themselves, from a given FILE file. Useful for fast
23 |             * evaluation of the underlying data structure.
24 |             *
25 |             * Returns:
26 |             *  * < 0 (-1) on error
27 |             *  * > 0 on success
28 |             * \param[in] file_name the name of the file to load
29 |             * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
30 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
31 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
32 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
33 |             * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
34 |             * \param[out] data_idx the offset of cloud data within the file
35 |             * \param[in] offset the offset in the file where to expect the true header to begin.
36 |             * One usage example for setting the offset parameter is for reading
37 |             * data from a TAR "archive containing multiple files: TAR files always
38 |             * add a 512 byte header in front of the actual file, so set the offset
39 |             * to the next byte after the header (e.g., 513).
40 |             */
41 |           virtual int
42 |           readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
43 |                       Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
44 |                       int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) ;
45 | 
46 | 
47 |           /** \brief Read a point cloud data from a FILE file and store it into a sensor_msgs/PointCloud2.
48 |             * \param[in] file_name the name of the file containing the actual PointCloud data
49 |             * \param[out] cloud the resultant PointCloud message read from disk
50 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
51 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
52 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
53 |             * \param[in] offset the offset in the file where to expect the true header to begin.
54 |             * One usage example for setting the offset parameter is for reading
55 |             * data from a TAR "archive containing multiple files: TAR files always
56 |             * add a 512 byte header in front of the actual file, so set the offset
57 |             * to the next byte after the header (e.g., 513).
58 |             */
59 |           virtual int
60 |           read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
61 |                 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version ,
62 |                 const int offset = 0);
63 | 
64 | 
65 |   };
66 | 
67 |   class LASWriter : FileWriter {
68 | 
69 |   public:
70 |     LASWriter();
71 |     virtual ~LASWriter();
72 | 
73 |     /** \brief Save point cloud data to a FILE file containing n-D points
74 |            * \param[in] file_name the output file name
75 |            * \param[in] cloud the point cloud data message
76 |            * \param[in] origin the sensor acquisition origin
77 |            * \param[in] orientation the sensor acquisition orientation
78 |            * \param[in] binary set to true if the file is to be written in a binary
79 |            * FILE format, false (default) for ASCII
80 |            */
81 |          virtual int
82 |          write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
83 |                 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
84 |                 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
85 |                 const bool binary = false);
86 | 
87 |   };
88 | 
89 | }
90 | 
91 | #endif /* LAS_IO_H_ */
92 | 


--------------------------------------------------------------------------------
/include/pcl/io/ptx_io.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * las_io.h
 3 |  *
 4 |  *  Created on: May 9, 2012
 5 |  *      Author: asher
 6 |  */
 7 | 
 8 | #ifndef PCL_PTX_IO_H_
 9 | #define PCL_PTX_IO_H_
10 | 
11 | #include <pcl/io/file_io.h>
12 | 
13 | 
14 | namespace pcl{
15 | 
16 |   class PTXReader : public FileReader {
17 | 
18 |   public:
19 |     PTXReader();
20 |     virtual ~PTXReader();
21 |     /* Load only the meta information (number of points, their types, etc),
22 |             * and not the points themselves, from a given FILE file. Useful for fast
23 |             * evaluation of the underlying data structure.
24 |             *
25 |             * Returns:
26 |             *  * < 0 (-1) on error
27 |             *  * > 0 on success
28 |             * \param[in] file_name the name of the file to load
29 |             * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
30 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
31 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
32 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
33 |             * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
34 |             * \param[out] data_idx the offset of cloud data within the file
35 |             * \param[in] offset the offset in the file where to expect the true header to begin.
36 |             * One usage example for setting the offset parameter is for reading
37 |             * data from a TAR "archive containing multiple files: TAR files always
38 |             * add a 512 byte header in front of the actual file, so set the offset
39 |             * to the next byte after the header (e.g., 513).
40 |             */
41 |           virtual int
42 |           readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
43 |                       Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
44 |                       int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) ;
45 | 
46 | 
47 |           /** \brief Read a point cloud data from a FILE file and store it into a sensor_msgs/PointCloud2.
48 |             * \param[in] file_name the name of the file containing the actual PointCloud data
49 |             * \param[out] cloud the resultant PointCloud message read from disk
50 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
51 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
52 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
53 |             * \param[in] offset the offset in the file where to expect the true header to begin.
54 |             * One usage example for setting the offset parameter is for reading
55 |             * data from a TAR "archive containing multiple files: TAR files always
56 |             * add a 512 byte header in front of the actual file, so set the offset
57 |             * to the next byte after the header (e.g., 513).
58 |             */
59 |           virtual int
60 |           read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
61 |                 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version,
62 |                 const int offset = 0);
63 | 
64 | 
65 |   };
66 | 
67 |   class PTXWriter : FileWriter {
68 | 
69 |   public:
70 |     PTXWriter();
71 |     virtual ~PTXWriter();
72 | 
73 |     /** \brief Save point cloud data to a FILE file containing n-D points
74 |            * \param[in] file_name the output file name
75 |            * \param[in] cloud the point cloud data message
76 |            * \param[in] origin the sensor acquisition origin
77 |            * \param[in] orientation the sensor acquisition orientation
78 |            * \param[in] binary set to true if the file is to be written in a binary
79 |            * FILE format, false (default) for ASCII
80 |            */
81 |          virtual int
82 |          write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
83 |                 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
84 |                 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
85 |                 const bool binary = false);
86 | 
87 |          using FileWriter::write;
88 |   };
89 | 
90 | }
91 | 
92 | #endif /* LAS_IO_H_ */
93 | 


--------------------------------------------------------------------------------
/src/pts_io.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * pts_io.cpp
  3 |  *
  4 |  *  Created on: May 21, 2012
  5 |  *      Author: asher
  6 |  */
  7 | 
  8 | #include <pcl/io/pts_io.h>
  9 | #include <istream>
 10 | #include <fstream>
 11 | #include <boost/filesystem.hpp>
 12 | #include <boost/algorithm/string.hpp>
 13 | 
 14 | pcl::PTSReader::PTSReader()
 15 | {
 16 | }
 17 | 
 18 | 
 19 | 
 20 | pcl::PTSReader::~PTSReader()
 21 | {
 22 | }
 23 | 
 24 | 
 25 | 
 26 | int pcl::PTSReader::readHeader(const std::string & file_name, pcl::PCLPointCloud2 & cloud,
 27 |                                Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
 28 |                                int & file_version, int & data_type, unsigned int & data_idx,
 29 |                                const int offset)
 30 | {
 31 | 
 32 |   boost::filesystem::path fpath  = file_name;
 33 | 
 34 |   if ( !boost::filesystem::exists(fpath) ){
 35 |     pcl::console::print_error("[PTSReader] File %s does not exist.\n", file_name.c_str());
 36 |     return -1;
 37 |   }
 38 |   if ( fpath.extension().string() != ".pts"){
 39 |     pcl::console::print_error("[PTSReader] File does not have pts extension. \n");
 40 |     return -1;
 41 |   }
 42 | 
 43 |   origin = Eigen::Vector4f::Zero();
 44 |   orientation = Eigen::Quaternionf();
 45 | 
 46 |  {
 47 |   pcl::PCLPointField f;
 48 |   f.datatype = pcl::PCLPointField::FLOAT32;
 49 |   f.count= 1;
 50 |   f.name="x";
 51 |   cloud.fields.push_back(f);
 52 |   }
 53 | 
 54 |   {
 55 |   pcl::PCLPointField f;
 56 |   f.datatype = pcl::PCLPointField::FLOAT32;
 57 |   f.count= 1;
 58 |   f.name="y";
 59 |   f.offset =4;
 60 |   cloud.fields.push_back(f);
 61 |   }
 62 |   {
 63 |   pcl::PCLPointField f;
 64 |   f.datatype = pcl::PCLPointField::FLOAT32;
 65 |   f.count= 1;
 66 |   f.name="z";
 67 |   f.offset =8;
 68 |   cloud.fields.push_back(f);
 69 |   }
 70 | 
 71 |   {
 72 |   pcl::PCLPointField f;
 73 |   f.datatype = pcl::PCLPointField::FLOAT32;
 74 |   f.count= 1;
 75 |   f.name="rgb";
 76 |   f.offset =12;
 77 |   cloud.fields.push_back(f);
 78 |   }
 79 |   cloud.point_step = 16;
 80 | 
 81 | 
 82 |   std::fstream ifile(file_name.c_str(), std::fstream::in);
 83 |   std::string line;
 84 |   int total=0;
 85 |   int cdown =0;
 86 |   while(std::getline(ifile,line)){
 87 |     cdown--;
 88 |     if (cdown <=0) {
 89 |       std::vector<std::string> tokens;
 90 |       boost::algorithm::split(tokens, line,boost::algorithm::is_any_of(" \n\r\t"), boost::algorithm::token_compress_on);
 91 |       if (tokens.size() <3){
 92 |         int count=0;
 93 |         if (sscanf(tokens[0].c_str(), "%d", &count) )total+= count;
 94 |         cdown = count;
 95 |       }
 96 |     }
 97 |     }
 98 |   cloud.height=1;
 99 |   cloud.width = total;
100 |   ifile.close();
101 | 
102 |   return cloud.height * cloud.width;
103 | }
104 | 
105 | 
106 | int pcl::PTSReader::read(const std::string & file_name, pcl::PCLPointCloud2 & cloud,
107 |                          Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
108 |                          int & file_version, const int offset)
109 | {
110 | 
111 |   int  data_type;
112 |   unsigned int data_idx;
113 |   if ( this->readHeader(file_name,cloud,origin,orientation,file_version, data_type, data_idx, offset) <0 ) return -1;
114 |   cloud.data.resize(cloud.height*cloud.width*cloud.point_step);
115 | 
116 |   std::string line;
117 |   std::fstream ifile(file_name.c_str(), std::fstream::in);
118 | 
119 |     int total=0;
120 | 
121 |     uint32_t ptoffset=0;
122 |     while(std::getline(ifile,line)){
123 |       std::vector<std::string> tokens;
124 |       boost::algorithm::split(tokens, line,boost::algorithm::is_any_of(" \n\r\t"), boost::algorithm::token_compress_on);
125 |       if (tokens.size() <6 ){
126 |         continue;
127 |       }
128 |         std::stringstream ss(line);
129 |        float x=0,y=0,z=0;
130 |        int r=0, g=0, b=0;
131 |        float pid;
132 |        ss >> x ;
133 |        ss >> y ;
134 |        ss >> z ;
135 |        ss >> pid;
136 |        ss >> r ;
137 |        ss >> g ;
138 |        ss >>b;
139 |        if ((x==0) && (y==0) && (z==0)){
140 |          x=y=z = std::numeric_limits<float>::quiet_NaN();
141 |        }
142 |        float *data = ( (float *) ( cloud.data.data() + ptoffset) )  ;
143 |        *( ( data    ) )  = x;
144 |        *( ( data +1 ) )  =y;
145 |        *( ( data +2 ) )  =z;
146 |        uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g<< 8 | (uint32_t)b);
147 |        *( ( data+ 3) )  =  *reinterpret_cast<float*>(&rgb);
148 |        ptoffset+= cloud.point_step;
149 |     }
150 |   return cloud.width*cloud.height;
151 | }
152 | 
153 | 


--------------------------------------------------------------------------------
/src/cloud_collection.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * CloudCollection.cpp
  3 |  *
  4 |  *  Created on: Feb 21, 2012
  5 |  *      Author: asher
  6 |  */
  7 | 
  8 | #include <pcl/io/cloud_collection.h>
  9 | #include <pcl/io/points_to_rosmsg.h>
 10 | 
 11 | #include <pcl/io/pcd_io.h>
 12 | #include <pcl/PCLPointCloud2.h>
 13 | #include <pcl/point_types.h>
 14 | 
 15 | #include <pcl/extra_point_type.h>
 16 | 
 17 | 
 18 | namespace pcl
 19 | {
 20 |   namespace io{
 21 |   CloudCollection::CloudCollection ()
 22 |   {
 23 |     // TODO Auto-generated constructor stub
 24 | 
 25 |   }
 26 | 
 27 |   CloudCollection::~CloudCollection ()
 28 |   {
 29 |     // TODO Auto-generated destructor stub
 30 |   }
 31 | 
 32 |   void
 33 |   CloudCollection::getCloudList(std::vector<std::string>& clouds){
 34 |     clouds.clear();
 35 |     for(std::map<std::string, boost::any>::iterator iter = point_map_.begin();
 36 |         iter!= point_map_.end(); iter++){
 37 |       clouds.push_back(iter->first);
 38 |     }
 39 |   }
 40 | 
 41 |   bool
 42 |   CloudCollection::load(const std::string& name){
 43 | 
 44 |     pcl::PCLPointCloud2::Ptr cloud;
 45 |     cloud.reset(new pcl::PCLPointCloud2);
 46 |     Eigen::Vector4f origin;
 47 |     Eigen::Quaternionf rot;
 48 |     if ( pcl::io::loadPCDFile (name, *cloud, origin, rot) <0) return false;
 49 | 
 50 |     for (int i=0; i<cloud->fields.size(); i++ ){
 51 |           if ( strcmp(cloud->fields[i].name.c_str(), "x") ==0 ){
 52 |                pcl::PointCloud<pcl::PointXYZ>::Ptr xyz( new pcl::PointCloud<pcl::PointXYZ>);
 53 |                xyz->sensor_origin_ = origin;
 54 | 			   xyz->sensor_orientation_ = rot;
 55 |                setCloud<pcl::PointXYZ>(xyz);
 56 |                pcl::fromPCLPointCloud2(*cloud, *xyz);
 57 |            }
 58 |             if ( strcmp(cloud->fields[i].name.c_str(), "rgb") ==0  || strcmp(cloud->fields[i].name.c_str(), "rgba") ==0 ){
 59 |               pcl::PointCloud<pcl::RGB>::Ptr rgb( new pcl::PointCloud<pcl::RGB>);
 60 |               setCloud<pcl::RGB>(rgb);
 61 |               pcl::fromPCLPointCloud2(*cloud, *rgb);
 62 |             }
 63 |             if ( strcmp(cloud->fields[i].name.c_str(), "normal_x") ==0 ){
 64 |               pcl::PointCloud<pcl::Normal>::Ptr normal( new pcl::PointCloud<pcl::Normal>);
 65 |                 setCloud<pcl::Normal>(normal);
 66 |                pcl::fromPCLPointCloud2(*cloud, *normal);
 67 |             }
 68 |             if ( strcmp(cloud->fields[i].name.c_str(), "label") ==0 ){
 69 |                pcl::PointCloud<pcl::Label>::Ptr label( new pcl::PointCloud<pcl::Label>);
 70 |                setCloud<pcl::Label>(label);
 71 |                pcl::fromPCLPointCloud2(*cloud, *label);
 72 |             }
 73 |             if ( strcmp(cloud->fields[i].name.c_str(), "intensity") ==0 ){
 74 |                pcl::PointCloud<pcl::Intensity>::Ptr label( new pcl::PointCloud<pcl::Intensity>);
 75 |                setCloud<pcl::Intensity>(label);
 76 |                pcl::fromPCLPointCloud2(*cloud, *label);
 77 |             }
 78 | 
 79 |     }
 80 |     return true;
 81 |   }
 82 | 
 83 | 
 84 |   void CloudCollection::save(const std::string& name){
 85 |   pcl::PCLPointCloud2 cloud_blob;
 86 |   getPointCloud2(cloud_blob);
 87 | 
 88 |   pcl::PCDWriter writer;
 89 |   if (cloud_blob.width) writer.writeBinaryCompressed(name, cloud_blob,
 90 |      this->getCloud<pcl::PointXYZ>()->sensor_origin_,
 91 |      this->getCloud<pcl::PointXYZ>()->sensor_orientation_);
 92 | }
 93 | 
 94 |   void  CloudCollection::getPointCloud2(pcl::PCLPointCloud2 & cloud_blob)
 95 |   {
 96 |     pcl::PointCloud<pcl::PointXYZ>::Ptr xyz = getCloud<pcl::PointXYZ>();
 97 |     pcl::toPCLPointCloud2(*xyz, cloud_blob);
 98 | 
 99 |     pcl::PointCloud<pcl::Normal>::Ptr n = this->getCloud<pcl::Normal>();
100 |     pcl::PointCloud<pcl::RGB>::Ptr r = this->getCloud<pcl::RGB>();
101 |     pcl::PointCloud<pcl::Label>::Ptr l = this->getCloud<pcl::Label>();
102 |     pcl::PointCloud<pcl::Intensity>::Ptr ic = this->getCloud<pcl::Intensity>();
103 | 
104 |     if ( n != NULL){
105 |       pcl::PCLPointCloud2 tmp, tmp2;
106 |       tmp2 = cloud_blob;
107 |       pcl::toPCLPointCloud2(*n, tmp);
108 |       pcl::concatenateFields(tmp, tmp2, cloud_blob);
109 |     }
110 |     if ( r != NULL){
111 |         pcl::PCLPointCloud2 tmp, tmp2;
112 |         tmp2 = cloud_blob;
113 |         pcl::toPCLPointCloud2(*r, tmp);
114 |         pcl::concatenateFields(tmp, tmp2, cloud_blob);
115 |       }
116 |     if ( ic != NULL){
117 |       pcl::PCLPointCloud2 tmp, tmp2;
118 |       tmp2 = cloud_blob;
119 |       pcl::toPCLPointCloud2(*ic, tmp);
120 |       pcl::concatenateFields(tmp, tmp2, cloud_blob);
121 |     }
122 |     if ( l != NULL){
123 |       pcl::PCLPointCloud2 tmp, tmp2;
124 |       tmp2 = cloud_blob;
125 |       pcl::toPCLPointCloud2(*l, tmp);
126 |       pcl::concatenateFields(tmp, tmp2, cloud_blob);
127 |     }
128 |   }
129 | 
130 |   }
131 | }
132 |  /* namespace pcl */
133 | 


--------------------------------------------------------------------------------
/src/vtx_io.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * pts_io.cpp
  3 |  *
  4 |  *  Created on: May 21, 2012
  5 |  *      Author: asher
  6 |  */
  7 | 
  8 | #include <pcl/io/vtx_io.h>
  9 | #include <istream>
 10 | #include <fstream>
 11 | #include <boost/filesystem.hpp>
 12 | #include <boost/algorithm/string.hpp>
 13 | 
 14 | pcl::VTXReader::VTXReader()
 15 | {
 16 | }
 17 | 
 18 | 
 19 | 
 20 | pcl::VTXReader::~VTXReader()
 21 | {
 22 | }
 23 | 
 24 | 
 25 | 
 26 | int pcl::VTXReader::readHeader(const std::string & file_name, pcl::PCLPointCloud2 & cloud,
 27 |                                Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
 28 |                                int & file_version, int & data_type, unsigned int & data_idx,
 29 |                                const int offset)
 30 | {
 31 | 
 32 |   boost::filesystem::path fpath  = file_name;
 33 | 
 34 |   if ( !boost::filesystem::exists(fpath) ){
 35 |     pcl::console::print_error("[VTXReader] File %s does not exist.\n", file_name.c_str());
 36 |     return -1;
 37 |   }
 38 |   if ( fpath.extension().string() != ".vtx"){
 39 |     pcl::console::print_error("[VTXReader] File does not have vtx extension. \n");
 40 |     return -1;
 41 |   }
 42 | 
 43 |   origin = Eigen::Vector4f::Zero();
 44 |   orientation = Eigen::Quaternionf();
 45 | 
 46 |  {
 47 |   pcl::PCLPointField f;
 48 |   f.datatype = pcl::PCLPointField::FLOAT32;
 49 |   f.count= 1;
 50 |   f.name="x";
 51 |   cloud.fields.push_back(f);
 52 |   }
 53 | 
 54 |   {
 55 |   pcl::PCLPointField f;
 56 |   f.datatype = pcl::PCLPointField::FLOAT32;
 57 |   f.count= 1;
 58 |   f.name="y";
 59 |   f.offset =4;
 60 |   cloud.fields.push_back(f);
 61 |   }
 62 |   {
 63 |   pcl::PCLPointField f;
 64 |   f.datatype = pcl::PCLPointField::FLOAT32;
 65 |   f.count= 1;
 66 |   f.name="z";
 67 |   f.offset =8;
 68 |   cloud.fields.push_back(f);
 69 |   }
 70 | 
 71 |   {
 72 |   pcl::PCLPointField f;
 73 |   f.datatype = pcl::PCLPointField::FLOAT32;
 74 |   f.count= 1;
 75 |   f.name="normal_x";
 76 |   f.offset =12;
 77 |   cloud.fields.push_back(f);
 78 |   }
 79 | 
 80 |   {
 81 |   pcl::PCLPointField f;
 82 |   f.datatype = pcl::PCLPointField::FLOAT32;
 83 |   f.count= 1;
 84 |   f.name="normal_y";
 85 |   f.offset =16;
 86 |   cloud.fields.push_back(f);
 87 |   }
 88 | 
 89 |   {
 90 |   pcl::PCLPointField f;
 91 |   f.datatype = pcl::PCLPointField::FLOAT32;
 92 |   f.count= 1;
 93 |   f.name="normal_z";
 94 |   f.offset =20;
 95 |   cloud.fields.push_back(f);
 96 |   }
 97 | 
 98 | 
 99 |  cloud.point_step= 24;
100 | 
101 |   std::fstream ifile(file_name.c_str(), std::fstream::in);
102 |   std::string line;
103 |   int total=0;
104 |   while(std::getline(ifile,line)){
105 |     boost::algorithm::trim(line);
106 |     if (line.find_first_not_of("#") !=0 ) continue;
107 |     std::vector<std::string> tokens;
108 |     boost::algorithm::split(tokens, line,boost::algorithm::is_any_of(", \n\r\t"), boost::algorithm::token_compress_on);
109 |     int s = tokens.size();
110 |     if (tokens.size() ==6)
111 |       total++;
112 |   }
113 |   cloud.height=1;
114 |   cloud.width = total;
115 |   ifile.close();
116 | 
117 |   origin = Eigen::Vector4f();
118 |   orientation = Eigen::Quaternionf();
119 | 
120 |   return cloud.height * cloud.width;
121 | }
122 | 
123 | 
124 | int pcl::VTXReader::read(const std::string & file_name, pcl::PCLPointCloud2 & cloud,
125 |                          Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
126 |                          int & file_version, const int offset)
127 | {
128 | 
129 |   int  data_type;
130 |   unsigned int data_idx;
131 |   if ( this->readHeader(file_name,cloud,origin,orientation,file_version, data_type, data_idx, offset) <0 ) return -1;
132 |   cloud.data.resize(cloud.height*cloud.width*cloud.point_step);
133 | 
134 |   std::string line;
135 |   std::fstream ifile(file_name.c_str(), std::fstream::in);
136 | 
137 |     int total=0;
138 | 
139 |     uint32_t ptoffset=0;
140 |     while(std::getline(ifile,line)){
141 |       boost::algorithm::trim(line);
142 |       if (line.find_first_not_of("#") !=0 ) continue;
143 |          std::vector<std::string> tokens;
144 |          boost::algorithm::split(tokens, line,boost::algorithm::is_any_of(", \n\r\t"), boost::algorithm::token_compress_on);
145 |          if (tokens.size() !=6) continue;
146 |          std::stringstream ss(line);
147 |         float x=0,y=0,z=0;
148 |         float nx=0, ny=0, nz=0;
149 |         ss >> y ;
150 |         ss >> z ;
151 |         ss >> nx ;
152 |         ss >> ny ;
153 |         ss >> nz;
154 |         if ((x==0) && (y==0) && (z==0)){
155 |           x=y=z = std::numeric_limits<float>::quiet_NaN();
156 |         }
157 |         float *data = ( (float *) ( cloud.data.data() + ptoffset) )  ;
158 |         *( ( data    ) )  = x;
159 |         *( ( data +1 ) )  =y;
160 |         *( ( data +2 ) )  =z;
161 |         *( ( data  +3  ) )  = nx;
162 |         *( ( data +4 ) )  =ny;
163 |         *( ( data +5 ) )  =nz;
164 | 
165 |         ptoffset+= cloud.point_step;
166 |     }
167 |   return cloud.width*cloud.height;
168 | }
169 | 
170 | 


--------------------------------------------------------------------------------
/include/pcl/io/e57_io.h:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * las_io.h
  3 |  *
  4 |  *  Created on: May 9, 2012
  5 |  *      Author: asher
  6 |  */
  7 | 
  8 | #ifndef E57_IO_H_
  9 | #define E57_IO_H_
 10 | 
 11 | #include <pcl/io/file_io.h>
 12 | #
 13 | 
 14 | namespace pcl{
 15 | 
 16 |   /*
 17 |    * This is an E57 file reader wrapper.  E57 is a considerably more complicated file format than
 18 |    * the pcd format.  It can contain multiple scans and metadata for each point.
 19 |    *
 20 |    * WARNING - PCL only supports floating point numbers for transformations and point clouds.
 21 |    * IT WILL LOOSE PRECISION when you have a point cloud over a large distance will very high precision.
 22 |    * There are not enough significant digits in 32bit floats to handle the data.
 23 |    *
 24 |    *
 25 |    * TODO View Point needs to be transfered. How do you maintain range image?  How do you read row/column?
 26 |    */
 27 | 
 28 |   class E57Reader : public FileReader {
 29 | 
 30 |   public:
 31 |     E57Reader();
 32 |     E57Reader(Eigen::Vector4d offset): read_buffer_size_(100), scan_number_(0), pt_offset_(offset){};
 33 | 
 34 |     virtual ~E57Reader();
 35 |     /* Load only the meta information (number of points, their types, etc),
 36 |             * and not the points themselves, from a given FILE file. Useful for fast
 37 |             * evaluation of the underlying data structure.
 38 |             *
 39 |             * Returns:
 40 |             *  * < 0 (-1) on error
 41 |             *  * > 0 on success
 42 |             * \param[in] file_name the name of the file to load
 43 |             * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
 44 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
 45 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
 46 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
 47 |             * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
 48 |             * \param[out] data_idx the offset of cloud data within the file
 49 |             * \param[in] offset the offset in the file where to expect the true header to begin.
 50 |             * One usage example for setting the offset parameter is for reading
 51 |             * data from a TAR "archive containing multiple files: TAR files always
 52 |             * add a 512 byte header in front of the actual file, so set the offset
 53 |             * to the next byte after the header (e.g., 513).
 54 |             */
 55 |           virtual int
 56 |           readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
 57 |                       Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
 58 |                       int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) ;
 59 | 
 60 | 
 61 |           /** \brief Read a point cloud data from a FILE file and store it into a sensor_msgs/PointCloud2.
 62 |             * \param[in] file_name the name of the file containing the actual PointCloud data
 63 |             * \param[out] cloud the resultant PointCloud message read from disk
 64 |             * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
 65 |             * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
 66 |             * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
 67 |             * \param[in] offset the offset in the file where to expect the true header to begin.
 68 |             * One usage example for setting the offset parameter is for reading
 69 |             * data from a TAR "archive containing multiple files: TAR files always
 70 |             * add a 512 byte header in front of the actual file, so set the offset
 71 |             * to the next byte after the header (e.g., 513).
 72 |             */
 73 |           virtual int
 74 |           read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
 75 |                 Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version,
 76 |                 const int offset = 0);
 77 | 
 78 |           void setPointOffset(Eigen::Vector4d offset){pt_offset_ =offset;}
 79 | 
 80 |   private:
 81 |        //when reading from a multiscan file, return scan_number scan
 82 |        int scan_number_;
 83 |        int read_buffer_size_;
 84 |        Eigen::Vector4d pt_offset_;
 85 | 
 86 |   };
 87 | 
 88 |   class E57Writer : public FileWriter {
 89 | 
 90 |   public:
 91 |     E57Writer(){}
 92 |     virtual ~E57Writer(){}
 93 | 
 94 |     /** \brief Save point cloud data to a FILE file containing n-D points
 95 |            * \param[in] file_name the output file name
 96 |            * \param[in] cloud the point cloud data message
 97 |            * \param[in] origin the sensor acquisition origin
 98 |            * \param[in] orientation the sensor acquisition orientation
 99 |            * \param[in] binary set to true if the file is to be written in a binary
100 |            * FILE format, false (default) for ASCII
101 |            */
102 |          virtual int
103 |          write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
104 |                 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
105 |                 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
106 |                 const bool binary = false);
107 | 
108 |   };
109 | 
110 | }
111 | 
112 | #endif /* LAS_IO_H_ */
113 | 


--------------------------------------------------------------------------------
/include/pcl/io/cloud_io.h:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * CloudReader.h
  3 |  *
  4 |  *  Created on: May 21, 2012
  5 |  *      Author: asher
  6 |  */
  7 | 
  8 | #ifndef CLOUDREADER_H_
  9 | #define CLOUDREADER_H_
 10 | 
 11 | #include <pcl/io/file_io.h>
 12 | #include <pcl/io/filereaderd.h>
 13 | 
 14 | #include <map>
 15 | #include <string>
 16 | 
 17 | namespace pcl
 18 | {
 19 | 
 20 |     class CloudReader : public FileReader
 21 |     {
 22 |     public:
 23 |       CloudReader ();
 24 |       virtual
 25 |       ~CloudReader ();
 26 | 
 27 |       /*
 28 |        * Register a file extension for the reader.  This is meant to be a wrapper around
 29 |        * a bunch of supported pont cloud types.
 30 |        * only pcd is default
 31 |        * CloudReader handles deletion of file reader pointers
 32 |        */
 33 |       bool registerExtension(std::string ext, FileReader* reader);
 34 | 
 35 |       FileReader* getReader(std::string ext){return reader_map_[ext];};
 36 | 
 37 | 
 38 |       void getSupportedExtensions(std::vector<std::string> extensions);
 39 | 
 40 |       /** \brief Read a point cloud data header from a FILE file.
 41 |              *
 42 |              * Load only the meta information (number of points, their types, etc),
 43 |              * and not the points themselves, from a given FILE file. Useful for fast
 44 |              * evaluation of the underlying data structure.
 45 |              *
 46 |              * Returns:
 47 |              *  * < 0 (-1) on error
 48 |              *  * > 0 on success
 49 |              * \param[in] file_name the name of the file to load
 50 |              * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
 51 |              * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
 52 |              * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
 53 |              * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
 54 |              * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
 55 |              * \param[out] data_idx the offset of cloud data within the file
 56 |              * \param[in] offset the offset in the file where to expect the true header to begin.
 57 |              * One usage example for setting the offset parameter is for reading
 58 |              * data from a TAR "archive containing multiple files: TAR files always
 59 |              * add a 512 byte header in front of the actual file, so set the offset
 60 |              * to the next byte after the header (e.g., 513).
 61 |              */
 62 |            virtual int
 63 |            readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
 64 |                        Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
 65 |                        int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0);
 66 | 
 67 |            /** \brief Read a point cloud data from a FILE file and store it into a sensor_msgs/PointCloud2.
 68 |              * \param[in] file_name the name of the file containing the actual PointCloud data
 69 |              * \param[out] cloud the resultant PointCloud message read from disk
 70 |              * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
 71 |              * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
 72 |              * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
 73 |              * \param[in] offset the offset in the file where to expect the true header to begin.
 74 |              * One usage example for setting the offset parameter is for reading
 75 |              * data from a TAR "archive containing multiple files: TAR files always
 76 |              * add a 512 byte header in front of the actual file, so set the offset
 77 |              * to the next byte after the header (e.g., 513).
 78 |              */
 79 |            virtual int
 80 |            read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
 81 |                  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version,
 82 |                  const int offset = 0);
 83 | 
 84 | 
 85 |       /** \brief Read a point cloud data from any FILE file, and convert it to the given template format.
 86 |         * \param[in] file_name the name of the file containing the actual PointCloud data
 87 |         * \param[out] cloud the resultant PointCloud message read from disk
 88 |         * \param[in] offset the offset in the file where to expect the true header to begin.
 89 |         * One usage example for setting the offset parameter is for reading
 90 |         * data from a TAR "archive containing multiple files: TAR files always
 91 |         * add a 512 byte header in front of the actual file, so set the offset
 92 |         * to the next byte after the header (e.g., 513).
 93 |         */
 94 | 
 95 |  template<typename PointT> inline int
 96 |       read (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset  =0){
 97 |    return FileReader::read(file_name, cloud, offset);
 98 |  }
 99 |       
100 |       
101 |     private:
102 |       /*
103 |        * reader_map_ is a map of point cloud file extensions to its associated
104 |        * file reader implementation pointer.
105 |        */
106 |       std::map<std::string, FileReader*> reader_map_;
107 | 
108 |       //Analyzes file name extension to determine correct file reader;
109 |       FileReader* chooseReader(std::string file_name);
110 |     };
111 | 
112 | } /* namespace pcl */
113 | #endif /* CLOUDREADER_H_ */
114 | 


--------------------------------------------------------------------------------
/include/pcl/io/filereaderd.h:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * FileReaderD.h
  3 |  *
  4 |  *  Created on: Jun 6, 2012
  5 |  *      Author: asher
  6 |  */
  7 | 
  8 | #ifndef FILEREADERD_H_
  9 | #define FILEREADERD_H_
 10 | 
 11 | #include <pcl/io/file_io.h>
 12 | 
 13 | /*
 14 |  * This class is a convience class that allows for easy manipulation of common point cloud formats
 15 |  * whose points may need more significant digits than a float number can handle.  This is common
 16 |  * for arial lidar or large scale terrestrial laser scans.  The hack is to allow reading of the
 17 |  * sensor origin to an eigen double.  Additionally, point offset can be set to reduce the significant
 18 |  * digits so they can be manipulated by floats.
 19 |  */
 20 | 
 21 | 
 22 | namespace pcl {
 23 | 
 24 | class FileReaderD  : public FileReader {
 25 | public:
 26 | 	FileReaderD():_minoffset(false), _use_offset(false), _ox(0), _oy(0), _oz(0){}
 27 | 	virtual ~FileReaderD(){}
 28 | 
 29 | 	void setOffset(double ox, double oy, double oz){ _use_offset =true; _ox=ox; _oy=oy; _oz=oz;}
 30 | 	void getOffset(double& x, double& y, double&z){x=_ox; y=_oy; z=_oz;}
 31 | 
 32 | 
 33 | 	void setMinimumOffset(bool enable = true){_minoffset = enable;_use_offset = enable;};
 34 | 
 35 | 
 36 | 	 /** \brief Read a point cloud data header from a FILE file and use double precision.
 37 | 	        *
 38 | 	        * Load only the meta information (number of points, their types, etc),
 39 | 	        * and not the points themselves, from a given FILE file. Useful for fast
 40 | 	        * evaluation of the underlying data structure.
 41 | 	        *
 42 | 	        * Returns:
 43 | 	        *  * < 0 (-1) on error
 44 | 	        *  * > 0 on success
 45 | 	        * \param[in] file_name the name of the file to load
 46 | 	        * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
 47 | 	        * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
 48 | 	        * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
 49 | 	        * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
 50 | 	        * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
 51 | 	        * \param[out] data_idx the offset of cloud data within the file
 52 | 	        * \param[in] offset the offset in the file where to expect the true header to begin.
 53 | 	        * One usage example for setting the offset parameter is for reading
 54 | 	        * data from a TAR "archive containing multiple files: TAR files always
 55 | 	        * add a 512 byte header in front of the actual file, so set the offset
 56 | 	        * to the next byte after the header (e.g., 513).
 57 | 	        */
 58 | 	      virtual int
 59 | 	      readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
 60 | 	                  Eigen::Vector4d &origin, Eigen::Quaterniond &orientation,
 61 | 	                  int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) = 0;
 62 | 
 63 | 
 64 | 	 /** \brief Read a point cloud data header from a FILE file.
 65 | 	        *
 66 | 	        * Load only the meta information (number of points, their types, etc),
 67 | 	        * and not the points themselves, from a given FILE file. Useful for fast
 68 | 	        * evaluation of the underlying data structure.
 69 | 	        *
 70 | 	        * Returns:
 71 | 	        *  * < 0 (-1) on error
 72 | 	        *  * > 0 on success
 73 | 	        * \param[in] file_name the name of the file to load
 74 | 	        * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
 75 | 	        * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
 76 | 	        * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
 77 | 	        * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
 78 | 	        * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
 79 | 	        * \param[out] data_idx the offset of cloud data within the file
 80 | 	        * \param[in] offset the offset in the file where to expect the true header to begin.
 81 | 	        * One usage example for setting the offset parameter is for reading
 82 | 	        * data from a TAR "archive containing multiple files: TAR files always
 83 | 	        * add a 512 byte header in front of the actual file, so set the offset
 84 | 	        * to the next byte after the header (e.g., 513).
 85 | 	        */
 86 | 	      virtual int
 87 | 	      readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
 88 | 	                  Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
 89 | 	                  int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0){
 90 | 	    	  Eigen::Vector4d orig;
 91 | 	    	  Eigen::Quaterniond orien;
 92 | 	    	  int result = readHeader(file_name, cloud, orig, orien, file_version, data_type, data_idx, offset);
 93 | 	    	  origin = orig.cast<float>();
 94 | 	    	  orientation = orien.cast<float>();
 95 | 	    	  return result;
 96 | 	      }
 97 | 
 98 | 	      /** \brief Read a point cloud data from a FILE file and store it into a sensor_msgs/PointCloud2.
 99 | 	        * \param[in] file_name the name of the file containing the actual PointCloud data
100 | 	        * \param[out] cloud the resultant PointCloud message read from disk
101 | 	        * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
102 | 	        * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
103 | 	        * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
104 | 	        * \param[in] offset the offset in the file where to expect the true header to begin.
105 | 	        * One usage example for setting the offset parameter is for reading
106 | 	        * data from a TAR "archive containing multiple files: TAR files always
107 | 	        * add a 512 byte header in front of the actual file, so set the offset
108 | 	        * to the next byte after the header (e.g., 513).
109 | 	        */
110 | 	      virtual int
111 | 	      read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
112 | 	            Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version,
113 | 	            const int offset = 0) = 0;
114 | 
115 | protected:
116 | 	//Offset all points by location by the minimum point
117 | 	bool _minoffset;
118 | 	//Use a point offset specified by o{x,y,z}
119 | 	bool _use_offset;
120 | 
121 | 	//point offsets
122 | 	double _ox, _oy, _oz;
123 | };
124 | 
125 | } /* namespace pcl */
126 | #endif /* FILEREADERD_H_ */
127 | 


--------------------------------------------------------------------------------
/src/e57_io.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #include <pcl/io/e57_io.h>
  3 | #include <e57/E57Foundation.h>
  4 | #include <boost/assign.hpp>
  5 | 
  6 | 
  7 | using namespace e57;
  8 | using namespace std;
  9 | 
 10 | /*
 11 |  * The FieldReader classes are just utility classes meant to match the
 12 |  * input buffer of the 57 reader to the point cloud binary data blob
 13 |  */
 14 | class FieldReaderBase{
 15 |   public:
 16 |   virtual ~FieldReaderBase(){};
 17 |   virtual void copy( int buffer_idx, int cloud_idx) =0;
 18 | 
 19 |   void setup( uint8_t* output, int point_size){
 20 |     odata_ = output;
 21 |     point_size_ = point_size;
 22 |   }
 23 |   protected:
 24 |     int point_size_;
 25 |     uint8_t* odata_;
 26 | };
 27 | 
 28 | template<class PCLT, class BUFFERT>
 29 | class FieldReader : public FieldReaderBase {
 30 | public:
 31 |   FieldReader(){
 32 |     offset =0;
 33 |   }
 34 |   FieldReader(const sensor_msgs::PointField& field, BUFFERT* buffer){
 35 |     field_ = field;
 36 |     buffer_ = buffer;
 37 |   }
 38 |  virtual  ~FieldReader(){}
 39 |   virtual void copy(int buffer_idx, int cloud_idx){
 40 |       *( (PCLT *) ( odata_ +point_size_*cloud_idx +field_.offset   ) )  =  buffer_[buffer_idx];
 41 |   }
 42 | 
 43 |   void setup( uint8_t* output, int point_size){
 44 |     odata_ = output;
 45 |     point_size_ = point_size;
 46 |   }
 47 | 
 48 | private:
 49 |   BUFFERT* buffer_;
 50 |   sensor_msgs::PointField field_;
 51 |   double offset;
 52 | };
 53 | 
 54 | 
 55 | class FieldReaderColor : public FieldReaderBase{
 56 | 
 57 | public:
 58 |   FieldReaderColor(const pcl::PCLPointField& field, uint8_t* r, uint8_t* g, uint8_t* b){
 59 |     r_=r;
 60 |     g_=g;
 61 |     b_=b;
 62 |     field_ = field;
 63 |   }
 64 | 
 65 |   virtual void copy( int buffer_idx, int cloud_idx){
 66 |       // pack r/g/b into rgb
 67 |       uint32_t rgb = ((uint32_t)r_[buffer_idx] << 16 | (uint32_t)g_[buffer_idx] << 8 | (uint32_t)b_[buffer_idx]);
 68 |       *( (float *) ( odata_ +point_size_*cloud_idx +field_.offset   ) )  =  *reinterpret_cast<float*>(&rgb);
 69 |   }
 70 | 
 71 | protected:
 72 |   uint8_t * r_;
 73 |   uint8_t* g_;
 74 |   uint8_t* b_;
 75 |   sensor_msgs::PointField field_;
 76 | };
 77 | 
 78 | 
 79 | 
 80 | void readE57Header(StructureNode& scan,
 81 |                 pcl::toPCLPointCloud2& cloud,
 82 |                 Eigen::Vector4d& origin,
 83 |                 Eigen::Quaterniond& rot){
 84 | 
 85 |   if(scan.isDefined("indexBounds")) {
 86 |    StructureNode indexBounds(scan.get("indexBounds"));
 87 |    if(indexBounds.isDefined("columnMaximum"))
 88 |        cloud.width = IntegerNode(indexBounds.get("columnMaximum")).value() -
 89 |                         IntegerNode(indexBounds.get("columnMinimum")).value() + 1;
 90 | 
 91 |        if(indexBounds.isDefined("rowMaximum"))
 92 |        cloud.height = IntegerNode(indexBounds.get("rowMaximum")).value() -
 93 |                  IntegerNode(indexBounds.get("rowMinimum")).value() + 1;
 94 |        cloud.is_dense=true;
 95 |      }
 96 |     else{
 97 |       CompressedVectorNode points(scan.get("points"));
 98 |       cloud.width = points.childCount();
 99 |       cloud.height =1;
100 |     }
101 | 
102 | 
103 |   // Get pose structure for scan.
104 | 
105 |     if(scan.isDefined("pose"))
106 |     {
107 |     StructureNode pose(scan.get("pose"));
108 |     if(pose.isDefined("rotation"))
109 |     {
110 |             StructureNode rotation(pose.get("rotation"));
111 |             rot = Eigen::Quaterniond(  FloatNode(rotation.get("w")).value(),
112 |                                        FloatNode(rotation.get("x")).value(),
113 |                                        FloatNode(rotation.get("y")).value(),
114 |                                        FloatNode(rotation.get("z")).value());
115 |     }
116 |     if(pose.isDefined("translation"))
117 |     {
118 |       StructureNode translation(pose.get("translation"));
119 |       origin[0]= FloatNode(translation.get("x")).value();
120 |       origin[1] = FloatNode(translation.get("y")).value();
121 |       origin[2] = FloatNode(translation.get("z")).value();
122 |       origin[3] = 1;
123 |     }
124 |     }
125 | 
126 | }
127 | 
128 | 
129 | /*
130 |  * The majority of this code is adapted from the E57 reader example.
131 |  */
132 | 
133 | pcl::E57Reader::E57Reader() : read_buffer_size_(100), scan_number_(0)
134 | {
135 |   pt_offset_ = Eigen::Vector4d::Zero();
136 | }
137 | 
138 | 
139 | pcl::E57Reader::~E57Reader()
140 | {
141 | }
142 | 
143 | 
144 | 
145 | 
146 | 
147 | int pcl::E57Reader::readHeader(const std::string & file_name, sensor_msgs::PointCloud2 & cloud,
148 |                                Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
149 |                                int & file_version, int & data_type, unsigned int & data_idx, const int offset)
150 | {
151 |   try {
152 |          /// Read file from disk
153 |          e57::ImageFile imf(file_name.c_str(), "r");
154 |          StructureNode root = imf.root();
155 | 
156 |          /// Make sure vector of scans is defined and of expected type.
157 |          /// If "/data3D" wasn't defined, the call to root.get below would raise an exception.
158 |          if (!root.isDefined("/data3D")) {
159 |              cerr << "File doesn't contain 3D images" << endl;
160 |              return 0;
161 |          }
162 |          Node n = root.get("/data3D");
163 |          if (n.type() != E57_VECTOR) {
164 |              cerr << "bad file" << endl;
165 |              return 0;
166 |          }
167 |          /// The node is a vector so we can safely get a VectorNode handle to it.
168 |          /// If n was not a VectorNode, this would raise an exception.
169 |          VectorNode data3D(n);
170 | 
171 |          StructureNode scan(data3D.get(scan_number_));
172 | 
173 |          Eigen::Vector4d origind;
174 |          Eigen::Quaterniond quatd;
175 |          readE57Header(scan,cloud,origind, quatd);
176 |          origind += -pt_offset_;
177 |          origin = origind.cast<float>();
178 |          orientation = quatd.cast<float>();
179 | 
180 |          imf.close();
181 |      } catch(E57Exception& ex) {
182 |          ex.report(__FILE__, __LINE__, __FUNCTION__);
183 |          return -1;
184 |      } catch (std::exception& ex) {
185 |          cerr << "Got an std::exception, what=" << ex.what() << endl;
186 |          return -1;
187 |      } catch (...) {
188 |          cerr << "Got an unknown exception" << endl;
189 |          return -1;
190 |      }
191 |      return 1;
192 | }
193 | 
194 | 
195 | 
196 | 
197 | sensor_msgs::PointField newField(std::string name, pcl::uint32_t offset, pcl::uint8_t datatype, pcl::uint32_t count){
198 |   pcl::PCLPointField f;
199 |   f.name = name;
200 |   f.offset = offset;
201 |   f.datatype = datatype;
202 |   f.count = count;
203 |   return f;
204 | }
205 | 
206 | int pcl::E57Reader::read(const std::string & file_name, pcl::toPCLPointCloud2 & cloud,
207 |                          Eigen::Vector4f & origin, Eigen::Quaternionf & orientation, int & file_version,
208 |                          const int offset)
209 | {
210 |   assert(false && "unimplemented");
211 |   return -1;
212 | }
213 | 
214 | 


--------------------------------------------------------------------------------
/src/ptx_io.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * ptx_io.cpp
  3 |  *
  4 |  *  Created on: May 21, 2012
  5 |  *      Author: asher
  6 |  */
  7 | 
  8 | 
  9 | #include <pcl/io/ptx_io.h>
 10 | #include <istream>
 11 | #include <fstream>
 12 | #include <boost/filesystem.hpp>
 13 | 
 14 | /*
 15 |  * The first two lines contain the number of scan lines (n) and the number of points per
 16 | scanline (m), respectively.
 17 | The next 8 lines consist of a 4x3 and followed by a 4x4 matrix. They are generally
 18 | unused but can be used to store a transformation. Ignore these lines.
 19 | 
 20 | Then come the points one per line.
 21 | The points are arranged in a grid: Every m points comprise a single scan line
 22 | and there are n*m points in total.
 23 | 
 24 | If the scan has no colors attached to the points then each line is of the form:
 25 | 
 26 | x y z i
 27 | 
 28 | where (x,y,z) is the points 3D coordinates expressed in meters and i is the intensity
 29 | of the returned laser beam (0 < i < 1).
 30 | 
 31 | If the scan has colors attached to the points then each point is of the form:
 32 | 
 33 | x y z i r g b
 34 | 
 35 | x y z and i are as before and r g b are the red, green, and blue color components (0 <= r,g,b <= 255).
 36 | The r,g,b values have been acquired by color camera attached on the scanner.
 37 | 
 38 | Some of the points have their (x,y,z) coordinates  equal to (0,0,0). These are points for which the scanner was not able to make any measurements
 39 | (for instance highly reflective surfaces, sky, etc.).
 40 | 
 41 | ptx description taken from 3D Urban Challenge (Ioannis Stamos of Hunter College of the City University of New York
 42 |   dataset README.
 43 | 
 44 |  */
 45 | 
 46 | 
 47 | pcl::PTXReader::PTXReader()
 48 | {
 49 | }
 50 | 
 51 | 
 52 | 
 53 | pcl::PTXReader::~PTXReader()
 54 | {
 55 | }
 56 | 
 57 | 
 58 | 
 59 | int pcl::PTXReader::readHeader(const std::string & file_name, pcl::PCLPointCloud2 & cloud,
 60 |                                Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
 61 |                                int & file_version, int & data_type, unsigned int & data_idx,
 62 |                                const int offset)
 63 | {
 64 | 
 65 |   boost::filesystem::path fpath  = file_name;
 66 | 
 67 |   if ( !boost::filesystem::exists(fpath) ){
 68 |     pcl::console::print_error("[PTXReader] File %s does not exist.\n", file_name.c_str());
 69 |     return -1;
 70 |   }
 71 |   if ( fpath.extension().string() != ".ptx"){
 72 |     pcl::console::print_error("[PTXReader] File does not have ptx extension. \n");
 73 |     return -1;
 74 |   }
 75 | 
 76 |   std::fstream ifile(file_name.c_str(), std::fstream::in);
 77 |  int rows, cols;
 78 |  char line[100];
 79 |  ifile.getline(line,100);
 80 | 
 81 |  {std::stringstream ss(line); ss >> cols;}
 82 |  ifile.getline(line,100);
 83 |  {std::stringstream ss(line); ss >> rows;}
 84 | 
 85 |  cloud.width = cols;
 86 |  cloud.height= rows;
 87 |  cloud.is_dense =true;
 88 |  for(int i=0; i<4; i++) ifile.getline(line,100);
 89 | 
 90 |  Eigen::Matrix4f tf;
 91 |  for(int i=0; i<4; i++){
 92 |    ifile.getline(line,100);
 93 |    std::stringstream ss(line);
 94 |    for(int j=0; j< 4;j++) ss>> tf(i,j);
 95 |  }
 96 |  tf.transposeInPlace();
 97 |  orientation = tf.block<3,3>(0,0);
 98 |  origin = tf.block<4,1>(0,3);
 99 | 
100 | 
101 |  {
102 |     pcl::PCLPointField f;
103 |   f.datatype = pcl::PCLPointField::FLOAT32;
104 |   f.count= 1;
105 |   f.name="x";
106 |   cloud.fields.push_back(f);
107 |   }
108 | 
109 |   {
110 |   pcl::PCLPointField f;
111 |   f.datatype = pcl::PCLPointField::FLOAT32;
112 |   f.count= 1;
113 |   f.name="y";
114 |   f.offset =4;
115 |   cloud.fields.push_back(f);
116 |   }
117 |   {
118 |   pcl::PCLPointField f;
119 |   f.datatype = pcl::PCLPointField::FLOAT32;
120 |   f.count= 1;
121 |   f.name="z";
122 |   f.offset =8;
123 |   cloud.fields.push_back(f);
124 |   }
125 | 
126 |   {
127 |   pcl::PCLPointField f;
128 |   f.datatype = pcl::PCLPointField::FLOAT32;
129 |   f.count= 1;
130 |   f.name="intensity";
131 |   f.offset =12;
132 |   cloud.fields.push_back(f);
133 |   }
134 | 
135 | 
136 |   {
137 |   pcl::PCLPointField f;
138 |   f.datatype = pcl::PCLPointField::FLOAT32;
139 |   f.count= 1;
140 |   f.name="rgb";
141 |   f.offset =16;
142 |   cloud.fields.push_back(f);
143 |   }
144 |   cloud.point_step = 20;
145 |   ifile.close();
146 | }
147 | 
148 | 
149 | int pcl::PTXReader::read(const std::string & file_name, pcl::PCLPointCloud2 & cloud,
150 |                          Eigen::Vector4f & origin, Eigen::Quaternionf & orientation,
151 |                          int & file_version, const int offset)
152 | {
153 | 
154 |   int  data_type;
155 |   unsigned int data_idx;
156 |   if ( this->readHeader(file_name,cloud,origin,orientation,file_version, data_type, data_idx, offset) <0 ) return -1;
157 |   cloud.data.resize(cloud.height*cloud.width*cloud.point_step);
158 | 
159 |   char line[100];
160 |   std::fstream ifile(file_name.c_str(), std::fstream::in);
161 | 
162 |   for(int i=0; i<8; i++) ifile.getline(line,100);
163 | 
164 |   for(int c=0, i=0; c<cloud.width; c++){
165 |     for(int ri=cloud.height-1; ri>=0; ri--, i++){
166 |       ifile.getline(line,100);
167 |        std::stringstream ss(line);
168 |        float x=0,y=0,z=0, intensity=0;
169 |        int r=0, g=0, b=0;
170 |        ss >> x >> y >> z >> intensity >>  r >> g >>b;
171 |        if ((x==0) && (y==0) && (z==0)){
172 |          x=y=z = std::numeric_limits<float>::quiet_NaN();
173 |        }
174 |        uint32_t offset = (cloud.width*ri+c)*cloud.point_step;
175 | 
176 |        float *data = ( (float *) ( cloud.data.data() + offset) )  ;
177 |        *( ( data    ) )  = x;
178 |        *( ( data +1 ) )  =y;
179 |        *( ( data +2 ) )  =z;
180 |        *( ( data +3) )  =intensity;
181 |        uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g<< 8 | (uint32_t)b);
182 |        *( ( data+ 4) )  =  *reinterpret_cast<float*>(&rgb);
183 |     }
184 |   }
185 |   return cloud.width*cloud.height;
186 | }
187 | 
188 | 
189 | 
190 | pcl::PTXWriter::PTXWriter()
191 | {
192 |   //TODO implement ptx writer
193 | }
194 | 
195 | 
196 | 
197 | pcl::PTXWriter::~PTXWriter()
198 | {
199 | }
200 | 
201 | int pcl::PTXWriter::write(const std::string& file_name,
202 | 		const pcl::PCLPointCloud2& cloud, const Eigen::Vector4f& origin,
203 | 		const Eigen::Quaternionf& orientation, const bool binary) {
204 | 
205 | std::ofstream ofile( file_name.c_str());
206 | ofile << cloud.width << "\n"<< cloud.height << "\n";
207 | 
208 | Eigen::Matrix4f tf = Eigen::Matrix4f::Identity();
209 | tf.block<3,3>(0,0) = orientation.toRotationMatrix() ;
210 | tf.block<4,1>(0,3) = origin;
211 | 
212 | for( int c=0 ; c<4; c++){
213 | 	for(int r=0; r<2; r++){
214 | 		ofile << tf(r,c) << " ";
215 | 	}
216 | 	ofile << tf(2,c) << "\n";
217 | }
218 | 
219 | tf = Eigen::Matrix4f::Identity();
220 | ofile << tf << "\n";
221 | 
222 | pcl::PointCloud<pcl::PointXYZI> tmp;
223 | pcl::fromPCLPointCloud2(cloud, tmp);
224 | for(int i=0; i< tmp.size(); i++){
225 | 	ofile << tmp[i].x << " " << tmp[i].y << " " << tmp[i].z << " " << tmp[i].intensity << "\n";
226 | }
227 | /*
228 | if (pcl::getFieldIndex(cloud,"rgba")){
229 | 	return -1;
230 | 	assert(false && "not implemented");
231 | }
232 | else if( pcl::getFieldIndex(cloud,"intensity")>=0 ){
233 | }
234 | else{
235 | 	assert(false && "not implemented");
236 | 	return -1;
237 | }
238 | */
239 | 
240 | return cloud.width*cloud.height;
241 | }
242 | 


--------------------------------------------------------------------------------
/include/pcl/io/points_to_rosmsg.h:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * io_util.h
  3 |  *
  4 |  *  Created on: Jan 11, 2012
  5 |  *      Author: asher
  6 |  */
  7 | 
  8 | #ifndef IO_UTIL_H_
  9 | #define IO_UTIL_H_
 10 | 
 11 | #include <Eigen/StdVector>
 12 | #include <Eigen/Geometry>
 13 | #include <cstddef>
 14 | 
 15 | #include <pcl/pcl_macros.h>
 16 | #include <pcl/exceptions.h>
 17 | #include <pcl/point_traits.h>
 18 | #include <pcl/for_each_type.h>
 19 | #include <boost/shared_ptr.hpp>
 20 | #include <map>
 21 | #include <boost/mpl/size.hpp>
 22 | 
 23 | #include <pcl/PCLPointCloud2.h>
 24 | #include <pcl/point_cloud.h>
 25 | #include <pcl/point_traits.h>
 26 | #include <pcl/for_each_type.h>
 27 | #include <pcl/exceptions.h>
 28 | #include <pcl/console/print.h>
 29 | #include <boost/foreach.hpp>
 30 | 
 31 | 
 32 | #include <pcl/point_cloud.h>
 33 | #include <pcl/ros/conversions.h>
 34 | 
 35 | namespace pcl {
 36 |   /** \brief Convert two pcl::PointCloud<T> objects to a PointCloud2 binary data blob.
 37 |     * \param[in] cloud the input pcl::PointCloud<T>
 38 |     * \param[out] msg the resultant PointCloud2 binary blob
 39 |     */
 40 | 
 41 |   template<typename PointA, typename PointB> void
 42 |   pointsToROSMsg (const pcl::PointCloud<PointA>& cloudA, const pcl::PointCloud<PointB>& cloudB,
 43 |                   pcl::PCLPointCloud2& msg)
 44 |   {
 45 |     assert(cloudA.points.size() == cloudB.points.size());
 46 | 
 47 |     // Ease the user's burden on specifying width/height for unorganized datasets
 48 |     if (cloudA.width == 0 && cloudA.height == 0)
 49 |     {
 50 |       msg.width  = (uint32_t) cloudA.points.size ();
 51 |       msg.height = 1;
 52 |     }
 53 |     else
 54 |     {
 55 |       assert (cloudA.points.size () == cloudA.width * cloudA.height);
 56 |       msg.height = cloudA.height;
 57 |       msg.width  = cloudA.width;
 58 |     }
 59 | 
 60 |     // Fill point cloud binary data (padding and all)
 61 |     size_t point_size =sizeof (PointA) + sizeof (PointB);
 62 |     size_t data_size = point_size * cloudA.points.size ();
 63 |     msg.data.resize (data_size);
 64 |     for(int i=0; i< cloudA.points.size(); i++){
 65 |       memcpy (&msg.data[i*point_size], &cloudA.points[i], sizeof (PointA));
 66 |       memcpy (&msg.data[i*point_size+sizeof(PointA)], &cloudB.points[i], sizeof (PointB));
 67 |     }
 68 | 
 69 |     // Fill fields metadata
 70 |     msg.fields.clear ();
 71 |     for_each_type< typename traits::fieldList<PointA>::type > (detail::FieldAdder<PointA>(msg.fields));
 72 |     int aF = msg.fields.size();
 73 |     for_each_type< typename traits::fieldList<PointB>::type > (detail::FieldAdder<PointB>(msg.fields));
 74 |     for(int i=aF; i<msg.fields.size(); i++) msg.fields[i].offset += sizeof(PointA);
 75 | 
 76 |     msg.header     = cloudA.header;
 77 |     msg.point_step = point_size;
 78 |     msg.row_step   = point_size * msg.width;
 79 |     msg.is_dense   = cloudA.is_dense;
 80 |     /// @todo msg.is_bigendian = ?;
 81 |   }
 82 | 
 83 | 
 84 |   template<typename PointA, typename PointB, typename PointC> void
 85 |   pointsToROSMsg (const pcl::PointCloud<PointA>& cloudA, const pcl::PointCloud<PointB>& cloudB,
 86 |                   const pcl::PointCloud<PointC>& cloudC,
 87 |                   pcl::PCLPointCloud2& msg)
 88 |   {
 89 |     assert((cloudA.points.size() == cloudB.points.size())
 90 |            && (cloudA.points.size()== cloudC.points.size() ));
 91 | 
 92 |     // Ease the user's burden on specifying width/height for unorganized datasets
 93 |     if (cloudA.width == 0 && cloudA.height == 0)
 94 |     {
 95 |       msg.width  = (uint32_t) cloudA.points.size ();
 96 |       msg.height = 1;
 97 |     }
 98 |     else
 99 |     {
100 |       assert (cloudA.points.size () == cloudA.width * cloudA.height);
101 |       msg.height = cloudA.height;
102 |       msg.width  = cloudA.width;
103 |     }
104 | 
105 |     // Fill point cloud binary data (padding and all)
106 |     size_t point_size =sizeof (PointA) + sizeof (PointB)+sizeof(PointC);
107 |     size_t data_size = point_size * cloudA.points.size ();
108 |     msg.data.resize (data_size);
109 |     for(int i=0; i< cloudA.points.size(); i++){
110 |       memcpy (&msg.data[i*point_size], &cloudA.points[i], sizeof (PointA));
111 |       memcpy (&msg.data[i*point_size+sizeof(PointA)], &cloudB.points[i], sizeof (PointB));
112 |       memcpy (&msg.data[i*point_size+sizeof(PointA)+sizeof(PointB)], &cloudC.points[i], sizeof (PointC));
113 | 
114 |     }
115 | 
116 |     // Fill fields metadata
117 |     msg.fields.clear ();
118 |     for_each_type< typename traits::fieldList<PointA>::type > (detail::FieldAdder<PointA>(msg.fields));
119 |     int Fs = msg.fields.size();
120 |     for_each_type< typename traits::fieldList<PointB>::type > (detail::FieldAdder<PointB>(msg.fields));
121 |     for(int i=Fs; i<msg.fields.size(); i++) msg.fields[i].offset += sizeof(PointA);
122 |     Fs = msg.fields.size();
123 |     for_each_type< typename traits::fieldList<PointC>::type > (detail::FieldAdder<PointC>(msg.fields));
124 |     for(int i=Fs; i<msg.fields.size(); i++) msg.fields[i].offset += sizeof(PointA) + sizeof(PointB);
125 | 
126 |     msg.header     = cloudA.header;
127 |     msg.point_step = point_size;
128 |     msg.row_step   = point_size * msg.width;
129 |     msg.is_dense   = cloudA.is_dense;
130 |     /// @todo msg.is_bigendian = ?;
131 |   }
132 | 
133 | 
134 |   template<typename PointA, typename PointB, typename PointC, typename PointD> void
135 |   pointsToROSMsg (const pcl::PointCloud<PointA>& cloudA, const pcl::PointCloud<PointB>& cloudB,
136 |                   const pcl::PointCloud<PointC>& cloudC, const pcl::PointCloud<PointD>& cloudD,
137 |                   pcl::PCLPointCloud2& msg)
138 |   {
139 |     assert( (cloudA.points.size() == cloudB.points.size()) &&
140 |             (cloudC.points.size() == cloudD.points.size()) &&
141 |             (cloudB.points.size() == cloudC.points.size()));
142 | 
143 |     // Ease the user's burden on specifying width/height for unorganized datasets
144 |     if (cloudA.width == 0 && cloudA.height == 0)
145 |     {
146 |       msg.width  = (uint32_t) cloudA.points.size ();
147 |       msg.height = 1;
148 |     }
149 |     else
150 |     {
151 |       assert (cloudA.points.size () == cloudA.width * cloudA.height);
152 |       msg.height = cloudA.height;
153 |       msg.width  = cloudA.width;
154 |     }
155 | 
156 |     // Fill point cloud binary data (padding and all)
157 |     size_t point_size =sizeof (PointA) + sizeof (PointB)+sizeof(PointC)+sizeof(PointD);
158 |     size_t data_size = point_size * cloudA.points.size ();
159 |     msg.data.resize (data_size);
160 |     int Coffset = sizeof(PointA)+sizeof(PointB);
161 |     int Doffset = sizeof(PointA)+sizeof(PointB)+sizeof(PointC);
162 |     for(int i=0; i< cloudA.points.size(); i++){
163 |       memcpy (&msg.data[i*point_size], &cloudA.points[i], sizeof (PointA));
164 |       memcpy (&msg.data[i*point_size+sizeof(PointA)], &cloudB.points[i], sizeof (PointB));
165 |       memcpy (&msg.data[i*point_size+Coffset], &cloudC.points[i], sizeof (PointC));
166 |       memcpy (&msg.data[i*point_size+Doffset], &cloudD.points[i], sizeof (PointD));
167 |     }
168 | 
169 |     // Fill fields metadata
170 |     msg.fields.clear ();
171 |     for_each_type< typename traits::fieldList<PointA>::type > (detail::FieldAdder<PointA>(msg.fields));
172 |     int Fs = msg.fields.size();
173 |     for_each_type< typename traits::fieldList<PointB>::type > (detail::FieldAdder<PointB>(msg.fields));
174 |     for(int i=Fs; i<msg.fields.size(); i++) msg.fields[i].offset += sizeof(PointA);
175 |     Fs = msg.fields.size();
176 |     for_each_type< typename traits::fieldList<PointC>::type > (detail::FieldAdder<PointC>(msg.fields));
177 |     for(int i=Fs; i<msg.fields.size(); i++) msg.fields[i].offset += Coffset;
178 |     Fs = msg.fields.size();
179 |     for_each_type< typename traits::fieldList<PointD>::type > (detail::FieldAdder<PointD>(msg.fields));
180 |     for(int i=Fs; i<msg.fields.size(); i++) msg.fields[i].offset += Doffset;
181 | 
182 |     msg.header     = cloudA.header;
183 |     msg.point_step = point_size;
184 |     msg.row_step   = point_size * msg.width;
185 |     msg.is_dense   = cloudA.is_dense;
186 |     /// @todo msg.is_bigendian = ?;
187 |   }
188 | 
189 | 
190 | }
191 | #endif /* IO_UTIL_H_ */
192 | 


--------------------------------------------------------------------------------