diff --git a/io/CMakeLists.txt b/io/CMakeLists.txt index 4f4a4b06f45..03ebf2cd7ba 100644 --- a/io/CMakeLists.txt +++ b/io/CMakeLists.txt @@ -129,6 +129,7 @@ if(build) src/lzf.cpp src/lzf_image_io.cpp src/obj_io.cpp + src/ifs_io.cpp src/image_grabber.cpp src/hdl_grabber.cpp src/robot_eye_grabber.cpp @@ -168,6 +169,7 @@ if(build) include/pcl/${SUBSYS_NAME}/tar.h include/pcl/${SUBSYS_NAME}/obj_io.h include/pcl/${SUBSYS_NAME}/ascii_io.h + include/pcl/${SUBSYS_NAME}/ifs_io.h include/pcl/${SUBSYS_NAME}/image_grabber.h include/pcl/${SUBSYS_NAME}/hdl_grabber.h include/pcl/${SUBSYS_NAME}/robot_eye_grabber.h diff --git a/io/include/pcl/io/boost.h b/io/include/pcl/io/boost.h index 329b765f46a..edd24d61ecc 100644 --- a/io/include/pcl/io/boost.h +++ b/io/include/pcl/io/boost.h @@ -71,6 +71,7 @@ #if BOOST_VERSION >= 104900 #include #endif +#include #define BOOST_PARAMETER_MAX_ARITY 7 #include #include diff --git a/io/include/pcl/io/ifs_io.h b/io/include/pcl/io/ifs_io.h new file mode 100644 index 00000000000..abeee6b4de0 --- /dev/null +++ b/io/include/pcl/io/ifs_io.h @@ -0,0 +1,226 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_IO_IFS_IO_H_ +#define PCL_IO_IFS_IO_H_ + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Indexed Face set (IFS) file format reader. This file format is used for + * the Brown Mesh Set for instance. + * \author Nizar Sallem + * \ingroup io + */ + class PCL_EXPORTS IFSReader + { + public: + /** Empty constructor */ + IFSReader () {} + /** Empty destructor */ + ~IFSReader () {} + + /** \brief we support two versions + * 1.0 classic + * 1.1 with texture coordinates addon + */ + enum + { + IFS_V1_0 = 0, + IFS_V1_1 = 1 + }; + + /** \brief Read a point cloud data header from a IFS file. + * + * Load only the meta information (number of points, their types, etc), + * and not the points themselves, from a given IFS file. Useful for fast + * evaluation of the underlying data structure. + * + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant point cloud dataset (only header will be filled) + * \param[out] ifs_version the IFS version of the file (IFS_V1_0 or IFS_V1_1) + * \param[out] data_idx the offset of cloud data within the file + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + int &ifs_version, unsigned int &data_idx); + + /** \brief Read a point cloud data from a IFS file and store it into a pcl/PCLPointCloud2. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[out] ifs_version the IFS version of the file (either IFS_V6 or IFS_V7) + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, int &ifs_version); + + /** \brief Read a point cloud data from any IFS file, and convert it to the + * given template format. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + template int + read (const std::string &file_name, pcl::PointCloud &cloud) + { + pcl::PCLPointCloud2 blob; + int ifs_version; + cloud.sensor_origin_ = Eigen::Vector4f::Zero (); + cloud.sensor_orientation_ = Eigen::Quaternionf::Identity (); + int res = read (file_name, blob, ifs_version); + + // If no error, convert the data + if (res == 0) + pcl::fromPCLPointCloud2 (blob, cloud); + return (res); + } + }; + + /** \brief Point Cloud Data (IFS) file format writer. + * \author Nizar Sallem + * \ingroup io + */ + class PCL_EXPORTS IFSWriter + { + public: + IFSWriter() {} + ~IFSWriter() {} + + /** \brief Save point cloud data to a IFS file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] cloud_name the point cloud name to be stored insude the IFS file. + * \return + * * 0 on success + * * < 0 on error + */ + int + write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const std::string &cloud_name = "cloud"); + + /** \brief Save point cloud data to a IFS file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud + * \param[in] cloud_name the point cloud name to be stored insude the IFS file. + * \return + * * 0 on success + * * < 0 on error + */ + template int + write (const std::string &file_name, const pcl::PointCloud &cloud, + const std::string &cloud_name = "cloud") + { + pcl::PCLPointCloud2 blob; + pcl::fromPCLPointCloud2 (blob, cloud); + return (write (file_name, blob, cloud_name)); + } + }; + + namespace io + { + /** \brief Load a IFS v.6 file into a templated PointCloud type. + * + * Any IFS files > v.6 will generate a warning as a + * pcl/PCLPointCloud2 message cannot hold the sensor origin. + * + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \ingroup io + */ + inline int + loadIFSFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud) + { + pcl::IFSReader p; + int ifs_version; + return (p.read (file_name, cloud, ifs_version)); + } + + /** \brief Load any IFS file into a templated PointCloud type + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \ingroup io + */ + template inline int + loadIFSFile (const std::string &file_name, pcl::PointCloud &cloud) + { + pcl::IFSReader p; + return (p.read (file_name, cloud)); + } + + /** \brief Save point cloud data to a IFS file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * + * \ingroup io + */ + inline int + saveIFSFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud) + { + pcl::IFSWriter w; + return (w.write (file_name, cloud)); + } + + /** \brief Save point cloud data to a IFS file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud + * + * \ingroup io + */ + template int + saveIFSFile (const std::string &file_name, const pcl::PointCloud &cloud) + { + pcl::IFSWriter w; + return (w.write (file_name, cloud)); + } + } +} + +#endif //#ifndef PCL_IO_IFS_IO_H_ diff --git a/io/src/ifs_io.cpp b/io/src/ifs_io.cpp new file mode 100644 index 00000000000..8622b75196d --- /dev/null +++ b/io/src/ifs_io.cpp @@ -0,0 +1,312 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include +#include +#include +#include + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +int +pcl::IFSReader::readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + int &ifs_version, unsigned int &data_idx) +{ + // Default values + data_idx = 0; + ifs_version = IFS_V1_0; + cloud.width = cloud.height = cloud.point_step = cloud.row_step = 0; + cloud.data.clear (); + + // By default, assume that there are _no_ invalid (e.g., NaN) points + //cloud.is_dense = true; + + uint32_t nr_points = 0; + std::ifstream fs; + std::string line; + + int specified_channel_count = 0; + + if (file_name == "" || !boost::filesystem::exists (file_name)) + { + PCL_ERROR ("[pcl::IFSReader::readHeader] Could not find file '%s'.\n", file_name.c_str ()); + return (-1); + } + + fs.open (file_name.c_str (), std::ios::binary); + if (!fs.is_open () || fs.fail ()) + { + PCL_ERROR ("[pcl::IFSReader::readHeader] Could not open file '%s'! Error : %s\n", file_name.c_str (), strerror(errno)); + fs.close (); + return (-1); + } + + //Read the magic + uint32_t length_of_magic; + fs.read ((char*)&length_of_magic, sizeof (uint32_t)); + char *magic = new char [length_of_magic]; + fs.read (magic, sizeof (char) * length_of_magic); + if (strcmp (magic, "IFS")) + { + PCL_ERROR ("[pcl::IFSReader::readHeader] File %s is not an IFS file!\n", file_name.c_str ()); + fs.close (); + return (-1); + } + delete[] magic; + + //Read IFS version + float version; + fs.read ((char*)&version, sizeof (float)); + if (version == 1.0f) + ifs_version = IFS_V1_0; + else + if (version == 1.1f) + ifs_version = IFS_V1_1; + else + { + PCL_ERROR ("[pcl::IFSReader::readHeader] Bad IFS file %f!\n", version); + fs.close (); + return (-1); + } + + //Read the name + uint32_t length_of_name; + fs.read ((char*)&length_of_name, sizeof (uint32_t)); + char *name = new char [length_of_name]; + fs.read (name, sizeof (char) * length_of_name); + delete[] name; + int offset = 0; + + // Read the header and fill it in with wonderful values + try + { + while (!fs.eof ()) + { + //Read the keyword + uint32_t length_of_keyword; + fs.read ((char*)&length_of_keyword, sizeof (uint32_t)); + char *keyword = new char [length_of_keyword]; + fs.read (keyword, sizeof (char) * length_of_keyword); + + if (strcmp (keyword, "VERTICES") == 0) + { + fs.read ((char*)&nr_points, sizeof (uint32_t)); + if ((nr_points == 0) || (nr_points > 10000000)) + { + PCL_ERROR ("[pcl::IFSReader::readHeader] Bad number of vertices %zu!\n", nr_points); + fs.close (); + return (-1); + } + + cloud.fields.resize (3); + cloud.fields[0].name = "x"; + cloud.fields[1].name = "y"; + cloud.fields[2].name = "z"; + + for (int i = 0; i < 3; ++i, offset += 4) + { + cloud.fields[i].offset = offset; + cloud.fields[i].datatype = pcl::PCLPointField::FLOAT32; + cloud.fields[i].count = 1; + } + cloud.point_step = offset; + cloud.data.resize (nr_points * cloud.point_step); + data_idx = fs.tellg (); + break; + } + } + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::IFSReader::readHeader] %s\n", exception); + fs.close (); + return (-1); + } + + // Set width and height + cloud.width = nr_points; + cloud.height = 1; + cloud.row_step = cloud.point_step * cloud.width; + + // Close file + fs.close (); + + return (0); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +int +pcl::IFSReader::read (const std::string &file_name, + pcl::PCLPointCloud2 &cloud, int &ifs_version) +{ + pcl::console::TicToc tt; + tt.tic (); + + unsigned int data_idx; + + int res = readHeader (file_name, cloud, ifs_version, data_idx); + + if (res < 0) + return (res); + + // Get the number of points the cloud should have + unsigned int nr_points = cloud.width * cloud.height; + + // Setting the is_dense property to true by default + cloud.is_dense = true; + + boost::iostreams::mapped_file_source mapped_file; + + size_t data_size = data_idx + cloud.data.size (); + + try + { + mapped_file.open (file_name, data_size, 0); + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::IFSReader::read] Error : %s!\n", file_name.c_str (), exception); + mapped_file.close (); + return (-1); + } + + if(!mapped_file.is_open ()) + { + PCL_ERROR ("[pcl::IFSReader::read] File mapping failure\n"); + mapped_file.close (); + return (-1); + } + + // Copy the data + memcpy (&cloud.data[0], mapped_file.data () + data_idx, cloud.data.size ()); + + mapped_file.close (); + + double total_time = tt.toc (); + PCL_DEBUG ("[pcl::IFSReader::read] Loaded %s as a %s cloud in %g ms with %d points. Available dimensions: %s.\n", + file_name.c_str (), cloud.is_dense ? "dense" : "non-dense", total_time, + cloud.width * cloud.height, pcl::getFieldsList (cloud).c_str ()); + return (0); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +int +pcl::IFSWriter::write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const std::string& cloud_name) +{ + if (cloud.data.empty ()) + { + PCL_ERROR ("[pcl::IFSWriter::write] Input point cloud has no data!\n"); + return (-1); + } + + if (!cloud.is_dense) + { + PCL_ERROR ("[pcl::IFSWriter::write] Non dense cloud are not alowed by IFS format!\n"); + return (-1); + } + + const std::string magic = "IFS"; + const float version = 1.0f; + const std::string vertices = "VERTICES"; + std::vector header (sizeof (uint32_t) + magic.size () + 1 + + sizeof (float) + + sizeof (uint32_t) + cloud_name.size () + 1 + + sizeof (uint32_t) + vertices.size () + 1 + + sizeof (uint32_t)); + char* addr = &(header[0]); + const uint32_t magic_size = static_cast (magic.size ()) + 1; + memcpy (addr, &magic_size, sizeof (uint32_t)); + addr+= sizeof (uint32_t); + memcpy (addr, magic.c_str (), magic_size * sizeof (char)); + addr+= magic_size * sizeof (char); + memcpy (addr, &version, sizeof (float)); + addr+= sizeof (float); + const uint32_t cloud_name_size = static_cast (cloud_name.size ()) + 1; + memcpy (addr, &cloud_name_size, sizeof (uint32_t)); + addr+= sizeof (uint32_t); + memcpy (addr, cloud_name.c_str (), cloud_name_size * sizeof (char)); + addr+= cloud_name_size * sizeof (char); + const uint32_t vertices_size = static_cast (vertices.size ()) + 1; + memcpy (addr, &vertices_size, sizeof (uint32_t)); + addr+= sizeof (uint32_t); + memcpy (addr, vertices.c_str (), vertices_size * sizeof (char)); + addr+= vertices_size * sizeof (char); + const uint32_t nb_vertices = cloud.data.size () / cloud.point_step; + memcpy (addr, &nb_vertices, sizeof (uint32_t)); + addr+= sizeof (uint32_t); + + std::size_t data_idx = header.size (); + + boost::iostreams::mapped_file_sink sink; + boost::iostreams::mapped_file_params params; + params.path = file_name; + params.flags = boost::iostreams::mapped_file_base::readwrite; + params.offset = 0; + params.new_file_size = data_idx + cloud.data.size (); + params.length = data_idx + cloud.data.size (); + + try + { + sink.open (params); + } + catch (const char *exception) + { + PCL_ERROR ("[pcl::IFSWriter::write] Error : %s!\n", file_name.c_str (), exception); + sink.close (); + return (-1); + } + + if (!sink.is_open ()) + { + PCL_ERROR ("[pcl::IFSWriter::write] Could not open file '%s'! Error : %s\n", file_name.c_str (), strerror(errno)); + sink.close (); + return (-1); + } + + // copy header + memcpy (sink.data (), &header[0], data_idx); + + // Copy the data + memcpy (sink.data () + data_idx, &cloud.data[0], cloud.data.size ()); + + sink.close (); + + return (0); +}