├── CMakeLists.txt ├── README ├── cmake ├── FindE57RefImpl.cmake ├── FindLIBLAS.cmake ├── FindPCL_IO_EXTRA.cmake └── FindXerces.cmake ├── include └── pcl │ ├── double_utils.h │ ├── extra_point_type.h │ └── io │ ├── cloud_collection.h │ ├── cloud_io.h │ ├── e57_io.h │ ├── filereaderd.h │ ├── impl │ └── cloud_collection.hpp │ ├── las_io.h │ ├── points_to_rosmsg.h │ ├── pts_io.h │ ├── ptx_io.h │ ├── vtx_io.h │ ├── xya_io.h │ └── xyz_io.h ├── manifest.xml ├── src ├── cloud_collection.cpp ├── cloud_io.cpp ├── double_utils.cpp ├── e57_io.cpp ├── las_io.cpp ├── pts_io.cpp ├── ptx_io.cpp ├── vtx_io.cpp ├── xya_io.cpp └── xyz_io.cpp └── tools ├── cvt2pcd.cpp ├── cvt2ptx.cpp └── cvt2xyz.cpp /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/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 | -------------------------------------------------------------------------------- /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/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 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace pcl{ 18 | 19 | 20 | bool hasDoublePointXYZ(const pcl::PCLPointCloud2& cloud); 21 | 22 | 23 | template 24 | void cvtAndOffset( const pcl::PointCloud& cloudA, const pcl::PointCloud& 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 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 getVector3dMap () { return (Eigen::Vector3d::Map (data)); } 33 | inline const Eigen::Map 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 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | namespace pcl { 21 | namespace traits{ 22 | template 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 Ptr; 37 | 38 | template typename pcl::PointCloud::Ptr 39 | getCloud(); 40 | 41 | template 42 | void getCloud(typename pcl::PointCloud::Ptr& cloud); 43 | 44 | 45 | template void 46 | setCloud(typename pcl::PointCloud::Ptr); 47 | 48 | void getPointCloud2(pcl::PCLPointCloud2& cloud); 49 | 50 | void getCloudList(std::vector& clouds); 51 | 52 | bool load(const std::string& name); 53 | void save(const std::string& name); 54 | 55 | protected: 56 | std::map point_map_; 57 | 58 | }; 59 | } // namespace io 60 | } /* namespace pcl */ 61 | 62 | #include "impl/cloud_collection.hpp" 63 | #endif /* CLOUDCOLLECTION_H_ */ 64 | -------------------------------------------------------------------------------- /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 12 | #include 13 | 14 | #include 15 | #include 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 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 inline int 96 | read (const std::string &file_name, pcl::PointCloud &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 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/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 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/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 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(); 94 | orientation = orien.cast(); 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 | -------------------------------------------------------------------------------- /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 13 | 14 | #include 15 | 16 | namespace pcl{ 17 | namespace io { 18 | template typename pcl::PointCloud::Ptr 19 | CloudCollection::getCloud(){ 20 | 21 | typename pcl::PointCloud::Ptr cloud; 22 | 23 | std::vector fields; 24 | std::string key =""; 25 | pcl::getFields(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::Ptr >(point_map_[key]); 36 | } 37 | catch(std::exception & e) 38 | { 39 | return cloud; 40 | } 41 | 42 | } 43 | 44 | template 45 | void CloudCollection::getCloud(typename pcl::PointCloud::Ptr& cloud){ 46 | std::vector fields; 47 | std::string key =""; 48 | pcl::getFields(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::Ptr >(point_map_[key]); 59 | } 60 | catch(std::exception & e) 61 | { 62 | } 63 | } 64 | 65 | template void 66 | CloudCollection::setCloud(typename pcl::PointCloud::Ptr cloud){ 67 | std::vector fields; 68 | std::string key =""; 69 | pcl::getFields(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 | -------------------------------------------------------------------------------- /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 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/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 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | 32 | #include 33 | #include 34 | 35 | namespace pcl { 36 | /** \brief Convert two pcl::PointCloud objects to a PointCloud2 binary data blob. 37 | * \param[in] cloud the input pcl::PointCloud 38 | * \param[out] msg the resultant PointCloud2 binary blob 39 | */ 40 | 41 | template void 42 | pointsToROSMsg (const pcl::PointCloud& cloudA, const pcl::PointCloud& 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::type > (detail::FieldAdder(msg.fields)); 72 | int aF = msg.fields.size(); 73 | for_each_type< typename traits::fieldList::type > (detail::FieldAdder(msg.fields)); 74 | for(int i=aF; i void 85 | pointsToROSMsg (const pcl::PointCloud& cloudA, const pcl::PointCloud& cloudB, 86 | const pcl::PointCloud& 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::type > (detail::FieldAdder(msg.fields)); 119 | int Fs = msg.fields.size(); 120 | for_each_type< typename traits::fieldList::type > (detail::FieldAdder(msg.fields)); 121 | for(int i=Fs; i::type > (detail::FieldAdder(msg.fields)); 124 | for(int i=Fs; i void 135 | pointsToROSMsg (const pcl::PointCloud& cloudA, const pcl::PointCloud& cloudB, 136 | const pcl::PointCloud& cloudC, const pcl::PointCloud& 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::type > (detail::FieldAdder(msg.fields)); 172 | int Fs = msg.fields.size(); 173 | for_each_type< typename traits::fieldList::type > (detail::FieldAdder(msg.fields)); 174 | for(int i=Fs; i::type > (detail::FieldAdder(msg.fields)); 177 | for(int i=Fs; i::type > (detail::FieldAdder(msg.fields)); 180 | for(int i=Fs; i 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/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 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 | -------------------------------------------------------------------------------- /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 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 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 | -------------------------------------------------------------------------------- /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 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 | -------------------------------------------------------------------------------- /manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pcl_io_extra 5 | 6 | 7 | Adam Stambler 8 | BSD 9 | 10 | http://github.com/adasta/pcl_io_extra 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/cloud_collection.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * CloudCollection.cpp 3 | * 4 | * Created on: Feb 21, 2012 5 | * Author: asher 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 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& clouds){ 34 | clouds.clear(); 35 | for(std::map::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; ifields.size(); i++ ){ 51 | if ( strcmp(cloud->fields[i].name.c_str(), "x") ==0 ){ 52 | pcl::PointCloud::Ptr xyz( new pcl::PointCloud); 53 | xyz->sensor_origin_ = origin; 54 | xyz->sensor_orientation_ = rot; 55 | setCloud(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::Ptr rgb( new pcl::PointCloud); 60 | setCloud(rgb); 61 | pcl::fromPCLPointCloud2(*cloud, *rgb); 62 | } 63 | if ( strcmp(cloud->fields[i].name.c_str(), "normal_x") ==0 ){ 64 | pcl::PointCloud::Ptr normal( new pcl::PointCloud); 65 | setCloud(normal); 66 | pcl::fromPCLPointCloud2(*cloud, *normal); 67 | } 68 | if ( strcmp(cloud->fields[i].name.c_str(), "label") ==0 ){ 69 | pcl::PointCloud::Ptr label( new pcl::PointCloud); 70 | setCloud(label); 71 | pcl::fromPCLPointCloud2(*cloud, *label); 72 | } 73 | if ( strcmp(cloud->fields[i].name.c_str(), "intensity") ==0 ){ 74 | pcl::PointCloud::Ptr label( new pcl::PointCloud); 75 | setCloud(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()->sensor_origin_, 91 | this->getCloud()->sensor_orientation_); 92 | } 93 | 94 | void CloudCollection::getPointCloud2(pcl::PCLPointCloud2 & cloud_blob) 95 | { 96 | pcl::PointCloud::Ptr xyz = getCloud(); 97 | pcl::toPCLPointCloud2(*xyz, cloud_blob); 98 | 99 | pcl::PointCloud::Ptr n = this->getCloud(); 100 | pcl::PointCloud::Ptr r = this->getCloud(); 101 | pcl::PointCloud::Ptr l = this->getCloud(); 102 | pcl::PointCloud::Ptr ic = this->getCloud(); 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/cloud_io.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * CloudReader.cpp 3 | * 4 | * Created on: May 21, 2012 5 | * Author: asher 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 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::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 extensions) 53 | { 54 | for(std::map::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 | -------------------------------------------------------------------------------- /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 10 | 11 | bool pcl::hasDoublePointXYZ (const pcl::PCLPointCloud2& cloud) 12 | { 13 | for(int i=0; i copy_idx; 34 | std::vector length; 35 | int point_size=0; 36 | for(int i=0; i 3 | #include 4 | #include 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 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(&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(); 178 | orientation = quatd.cast(); 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/las_io.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include // std::ifstream 10 | #include // 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/pts_io.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * pts_io.cpp 3 | * 4 | * Created on: May 21, 2012 5 | * Author: asher 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 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 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 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::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(&rgb); 148 | ptoffset+= cloud.point_step; 149 | } 150 | return cloud.width*cloud.height; 151 | } 152 | 153 | -------------------------------------------------------------------------------- /src/ptx_io.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ptx_io.cpp 3 | * 4 | * Created on: May 21, 2012 5 | * Author: asher 6 | */ 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | #include 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=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::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(&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 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 | -------------------------------------------------------------------------------- /src/vtx_io.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * pts_io.cpp 3 | * 4 | * Created on: May 21, 2012 5 | * Author: asher 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 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 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 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::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 | -------------------------------------------------------------------------------- /src/xya_io.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * pts_io.cpp 3 | * 4 | * Created on: May 21, 2012 5 | * Author: asher 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 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 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::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(&rgb); 138 | ptoffset+= cloud.point_step; 139 | } 140 | return cloud.width*cloud.height; 141 | } 142 | 143 | -------------------------------------------------------------------------------- /src/xyz_io.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * pts_io.cpp 3 | * 4 | * Created on: May 21, 2012 5 | * Author: asher 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 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 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 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::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 | -------------------------------------------------------------------------------- /tools/cvt2pcd.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * pcl_e572pcd.cpp 3 | * 4 | * Created on: May 21, 2012 5 | * Author: asher 6 | */ 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace po=boost::program_options; 21 | 22 | #ifdef E57 23 | #include 24 | #endif 25 | #ifdef LAS 26 | #include 27 | #endif 28 | 29 | 30 | #include 31 | #include 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()->required(), "input point cloud ") 40 | ("output,o",po::value(), "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(); 71 | if(vm.count("output")) ofile = vm["output"].as(); 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().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() << " \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 | -------------------------------------------------------------------------------- /tools/cvt2ptx.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include 4 | #include 5 | #include 6 | #include 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(&infile)->required(), "input point cloud ") 18 | ("output,o",po::value(&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 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 | -------------------------------------------------------------------------------- /tools/cvt2xyz.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include 4 | #include 5 | #include 6 | #include 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(&infile)->required(), "input point cloud ") 18 | ("output,o",po::value(&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 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 | --------------------------------------------------------------------------------