diff --git a/.gitignore b/.gitignore index a069a70..0a45ccf 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,5 @@ .vs/ -CMakeSettings.json \ No newline at end of file +CMakeSettings.json +/build +/install +/log \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index a041ce0..c7da41d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,8 @@ add_compile_options(-Wall -Wextra -Wpedantic) # find dependencies find_package(ament_cmake REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(PCL REQUIRED) get_filename_component(MODELS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/models" @@ -27,10 +29,16 @@ target_include_directories(${PROJECT_NAME} $ $ ) +ament_target_dependencies(${PROJECT_NAME} PUBLIC + pcl_conversions +) install( DIRECTORY include/ DESTINATION include ) - +target_link_libraries(${PROJECT_NAME} PUBLIC ${PCL_LIBRARIES}) +ament_export_dependencies( + pcl_conversions +) ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) ament_package() diff --git a/include/plycpp/plycpp.h b/include/plycpp/plycpp.h index 149f935..a56cf2b 100644 --- a/include/plycpp/plycpp.h +++ b/include/plycpp/plycpp.h @@ -9,8 +9,8 @@ // copies of the Software, and to permit persons to whom the Software is // furnished to do so, subject to the following conditions : // -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. // // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, @@ -22,326 +22,296 @@ #pragma once -#include +#include +#include #include +#include +#include #include -#include -#include #include +#include - -namespace plycpp -{ - extern const std::type_index CHAR; - extern const std::type_index UCHAR; - extern const std::type_index SHORT; - extern const std::type_index USHORT; - extern const std::type_index INT; - extern const std::type_index UINT; - extern const std::type_index FLOAT; - extern const std::type_index DOUBLE; - - enum FileFormat - { - ASCII, - BINARY - }; - - class Exception : public std::exception - { - protected: - /** Error message. - */ - std::string exception_; - public: - Exception(const std::string& msg) - : exception_(msg.c_str()) - {} - }; - - template - struct KeyData - { - KeyData(const Key key, const T& data) - { - this->key = key; - this->data = data; - } - - Key key; - T data; - }; - - /// A list of elements that can be accessed through a given key for convenience. - /// Access by key is not meant to be fast, but practical. - template - class IndexedList - { - private: - typedef KeyData > MyKeyData; - typedef std::vector Container; - public: - typedef typename Container::iterator iterator; - typedef typename Container::const_iterator const_iterator; - - std::shared_ptr operator[] (const Key& key) - { - auto it = std::find_if(begin(), end(), [&key](const MyKeyData& a){ return a.key == key; }); - if (it != end()) - return it->data; - else - throw Exception("Invalid key."); - } - - const std::shared_ptr operator[] (const Key& key) const - { - auto it = std::find_if(begin(), end(), [&key](const MyKeyData& a){ return a.key == key; }); - if (it != end()) - return it->data; - else - throw Exception("Invalid key."); - } - - bool has_key(const Key &key) - { - auto it = std::find_if(begin(), end(), [&key](const MyKeyData &a) { return a.key == key; }); - if (it != end()) - return true; - else - return false; - } - - void push_back(const Key& key, const std::shared_ptr& data) - { - container.push_back(MyKeyData(key, data)); - } - - void clear() - { - container.clear(); - } - - iterator begin() { return container.begin(); }; - const_iterator begin() const { return container.begin(); }; - iterator end() { return container.end(); }; - const_iterator end() const { return container.end(); }; - - private: - Container container; - }; - - class PropertyArray; - class ElementArray; - typedef std::shared_ptr PropertyArrayConstPtr; - typedef std::shared_ptr PropertyArrayPtr; - typedef IndexedList PLYData; - - class PropertyArray - { - public: - PropertyArray(const std::type_index type, const size_t size, const bool isList = false); - - template - bool isOfType() const - { - return type == std::type_index(typeid(T)); - } - - template - const T* ptr() const - { - assert(isOfType()); - return reinterpret_cast(&data[0]); - } - - template - T* ptr() - { - assert(isOfType()); - return reinterpret_cast(data.data()); - } - - const size_t size() const - { - assert(data.size() % stepSize == 0); - return data.size() / stepSize; - } - - template - const T& at(const size_t i) const - { - assert(isOfType()); - assert((i+1) * stepSize <= data.size()); - return *reinterpret_cast(&data[i * stepSize]); - } - - template - T& at(const size_t i) - { - assert(isOfType()); - assert((i + 1) * stepSize <= data.size()); - return *reinterpret_cast(&data[i * stepSize]); - } - - std::vector data; - const std::type_index type; - const unsigned int stepSize; - const bool isList = false; - }; - - class ElementArray - { - public: - ElementArray(const size_t size) - : size_(size) - {} - - IndexedList properties; - - size_t size() const - { - return size_; - } - private: - size_t size_; - }; - - /// Load PLY data - void load(const std::string& filename, PLYData& data); - - /// Save PLY data - void save(const std::string& filename, const PLYData& data, const FileFormat format = FileFormat::BINARY); - - /// Pack n properties -- each represented by a vector of type T -- - /// into a multichannel vector (e.g. of type vector >) - template - void packProperties(std::vector > properties, OutputVector& output) - { - output.clear(); - - if (properties.empty() || !properties.front()) - throw Exception("Missing properties"); - - const size_t size = properties.front()->size(); - const size_t nbProperties = properties.size(); - - // Pointers to actual data - std::vector ptsData; - for (auto& prop : properties) - { - // Check type consistency - if (!prop || !prop->isOfType() || prop->size() != size) - { - throw Exception(std::string("Missing properties or type inconsistency. I was expecting data of type ") + typeid(T).name()); - } - ptsData.push_back(prop->ptr()); - } - - // Packing - output.resize(size); - for (size_t i = 0; i < size; ++i) - { - for (size_t j = 0; j < nbProperties; ++j) - { - output[i][j] = ptsData[j][i]; - } - } - } - - /// Unpack a multichannel vector into a list of properties. - template - void unpackProperties(const OutputVector& cloud, std::vector < std::shared_ptr >& properties) - { - const size_t size = cloud.size(); - const size_t nbProperties = properties.size(); - - // Initialize and get - // Pointers to actual property data - std::vector ptsData; - for (auto& prop : properties) - { - prop.reset(new PropertyArray(std::type_index(typeid(T)), size)); - ptsData.push_back(prop->ptr()); - } - - // Copy data - for (size_t i = 0; i < size; ++i) - { - for (size_t j = 0; j < nbProperties; ++j) - { - ptsData[j][i] = cloud[i][j]; - } - } - } - - - template - void toPointCloud(const PLYData& plyData, Cloud& cloud) - { - cloud.clear(); - auto plyVertex = plyData["vertex"]; - if (plyVertex->size() == 0) - return; - std::vector > properties { plyVertex->properties["x"], plyVertex->properties["y"], plyVertex->properties["z"] }; - packProperties(properties, cloud); - } - - template - void toNormalCloud(const PLYData& plyData, Cloud& cloud) - { - cloud.clear(); - auto plyVertex = plyData["vertex"]; - if (plyVertex->size() == 0) - return; - std::vector > properties{ plyVertex->properties["nx"], plyVertex->properties["ny"], plyVertex->properties["nz"] }; - packProperties(properties, cloud); - } - - template - void fromPointCloud(const Cloud& points, PLYData& plyData) - { - const size_t size = points.size(); - - plyData.clear(); - - std::vector > positionProperties(3); - unpackProperties(points, positionProperties); - - std::shared_ptr vertex(new ElementArray(size)); - vertex->properties.push_back("x", positionProperties[0]); - vertex->properties.push_back("y", positionProperties[1]); - vertex->properties.push_back("z", positionProperties[2]); - - plyData.push_back("vertex", vertex); - } - - template - void fromPointCloudAndNormals(const Cloud& points, const Cloud& normals, PLYData& plyData) - { - const size_t size = points.size(); - - if (size != normals.size()) - throw Exception("Inconsistent size"); - - - plyData.clear(); - - std::vector > positionProperties(3); - unpackProperties(points, positionProperties); - - std::vector > normalProperties(3); - unpackProperties(normals, normalProperties); - - std::shared_ptr vertex(new ElementArray(size)); - vertex->properties.push_back("x", positionProperties[0]); - vertex->properties.push_back("y", positionProperties[1]); - vertex->properties.push_back("z", positionProperties[2]); - - vertex->properties.push_back("nx", normalProperties[0]); - vertex->properties.push_back("ny", normalProperties[1]); - vertex->properties.push_back("nz", normalProperties[2]); - - plyData.push_back("vertex", vertex); - } - -} \ No newline at end of file +namespace plycpp { +extern const std::type_index CHAR; +extern const std::type_index UCHAR; +extern const std::type_index SHORT; +extern const std::type_index USHORT; +extern const std::type_index INT; +extern const std::type_index UINT; +extern const std::type_index FLOAT; +extern const std::type_index DOUBLE; + +enum FileFormat { ASCII, BINARY }; + +class Exception : public std::exception { +protected: + /** Error message. + */ + std::string exception_; + +public: + Exception(const std::string &msg) : exception_(msg.c_str()) {} +}; + +template struct KeyData { + KeyData(const Key key, const T &data) { + this->key = key; + this->data = data; + } + + Key key; + T data; +}; + +/// A list of elements that can be accessed through a given key for convenience. +/// Access by key is not meant to be fast, but practical. +template class IndexedList { +private: + typedef KeyData> MyKeyData; + typedef std::vector Container; + +public: + typedef typename Container::iterator iterator; + typedef typename Container::const_iterator const_iterator; + + std::shared_ptr operator[](const Key &key) { + auto it = std::find_if(begin(), end(), + [&key](const MyKeyData &a) { return a.key == key; }); + if (it != end()) + return it->data; + else + throw Exception("Invalid key."); + } + + const std::shared_ptr operator[](const Key &key) const { + auto it = std::find_if(begin(), end(), + [&key](const MyKeyData &a) { return a.key == key; }); + if (it != end()) + return it->data; + else + throw Exception("Invalid key."); + } + + bool has_key(const Key &key) { + auto it = std::find_if(begin(), end(), + [&key](const MyKeyData &a) { return a.key == key; }); + if (it != end()) + return true; + else + return false; + } + + void push_back(const Key &key, const std::shared_ptr &data) { + container.push_back(MyKeyData(key, data)); + } + + void clear() { container.clear(); } + + iterator begin() { return container.begin(); }; + const_iterator begin() const { return container.begin(); }; + iterator end() { return container.end(); }; + const_iterator end() const { return container.end(); }; + +private: + Container container; +}; + +class PropertyArray; +class ElementArray; +typedef std::shared_ptr PropertyArrayConstPtr; +typedef std::shared_ptr PropertyArrayPtr; +typedef IndexedList PLYData; + +class PropertyArray { +public: + PropertyArray(const std::type_index type, const size_t size, + const bool isList = false); + + template bool isOfType() const { + return type == std::type_index(typeid(T)); + } + + template const T *ptr() const { + assert(isOfType()); + return reinterpret_cast(&data[0]); + } + + template T *ptr() { + assert(isOfType()); + return reinterpret_cast(data.data()); + } + + const size_t size() const { + assert(data.size() % stepSize == 0); + return data.size() / stepSize; + } + + template const T &at(const size_t i) const { + assert(isOfType()); + assert((i + 1) * stepSize <= data.size()); + return *reinterpret_cast(&data[i * stepSize]); + } + + template T &at(const size_t i) { + assert(isOfType()); + assert((i + 1) * stepSize <= data.size()); + return *reinterpret_cast(&data[i * stepSize]); + } + + std::vector data; + const std::type_index type; + const unsigned int stepSize; + const bool isList = false; +}; + +class ElementArray { +public: + ElementArray(const size_t size) : size_(size) {} + + IndexedList properties; + + size_t size() const { return size_; } + +private: + size_t size_; +}; + +/// Load PLY data +void load(const std::string &filename, PLYData &data); + +/// Save PLY data +void save(const std::string &filename, const PLYData &data, + const FileFormat format = FileFormat::BINARY); + +/// Custom Save function +void savePLYFile(const std::string &filename, + pcl::PointCloud::Ptr cloud, + const FileFormat format); +/// Pack n properties -- each represented by a vector of type T -- +/// into a multichannel vector (e.g. of type vector >) +template +void packProperties( + std::vector> properties, + OutputVector &output) { + output.clear(); + + if (properties.empty() || !properties.front()) + throw Exception("Missing properties"); + + const size_t size = properties.front()->size(); + const size_t nbProperties = properties.size(); + + // Pointers to actual data + std::vector ptsData; + for (auto &prop : properties) { + // Check type consistency + if (!prop || !prop->isOfType() || prop->size() != size) { + throw Exception(std::string("Missing properties or type inconsistency. I " + "was expecting data of type ") + + typeid(T).name()); + } + ptsData.push_back(prop->ptr()); + } + + // Packing + output.resize(size); + for (size_t i = 0; i < size; ++i) { + for (size_t j = 0; j < nbProperties; ++j) { + output[i][j] = ptsData[j][i]; + } + } +} + +/// Unpack a multichannel vector into a list of properties. +template +void unpackProperties(const OutputVector &cloud, + std::vector> &properties) { + const size_t size = cloud.size(); + const size_t nbProperties = properties.size(); + + // Initialize and get + // Pointers to actual property data + std::vector ptsData; + for (auto &prop : properties) { + prop.reset(new PropertyArray(std::type_index(typeid(T)), size)); + ptsData.push_back(prop->ptr()); + } + + // Copy data + for (size_t i = 0; i < size; ++i) { + for (size_t j = 0; j < nbProperties; ++j) { + ptsData[j][i] = cloud[i][j]; + } + } +} + +template +void toPointCloud(const PLYData &plyData, Cloud &cloud) { + cloud.clear(); + auto plyVertex = plyData["vertex"]; + if (plyVertex->size() == 0) + return; + std::vector> properties{ + plyVertex->properties["x"], plyVertex->properties["y"], + plyVertex->properties["z"]}; + packProperties(properties, cloud); +} + +template +void toNormalCloud(const PLYData &plyData, Cloud &cloud) { + cloud.clear(); + auto plyVertex = plyData["vertex"]; + if (plyVertex->size() == 0) + return; + std::vector> properties{ + plyVertex->properties["nx"], plyVertex->properties["ny"], + plyVertex->properties["nz"]}; + packProperties(properties, cloud); +} + +template +void fromPointCloud(const Cloud &points, PLYData &plyData) { + const size_t size = points.size(); + + plyData.clear(); + + std::vector> positionProperties(3); + unpackProperties(points, positionProperties); + + std::shared_ptr vertex(new ElementArray(size)); + vertex->properties.push_back("x", positionProperties[0]); + vertex->properties.push_back("y", positionProperties[1]); + vertex->properties.push_back("z", positionProperties[2]); + + plyData.push_back("vertex", vertex); +} + +template +void fromPointCloudAndNormals(const Cloud &points, const Cloud &normals, + PLYData &plyData) { + const size_t size = points.size(); + + if (size != normals.size()) + throw Exception("Inconsistent size"); + + plyData.clear(); + + std::vector> positionProperties(3); + unpackProperties(points, positionProperties); + + std::vector> normalProperties(3); + unpackProperties(normals, normalProperties); + + std::shared_ptr vertex(new ElementArray(size)); + vertex->properties.push_back("x", positionProperties[0]); + vertex->properties.push_back("y", positionProperties[1]); + vertex->properties.push_back("z", positionProperties[2]); + + vertex->properties.push_back("nx", normalProperties[0]); + vertex->properties.push_back("ny", normalProperties[1]); + vertex->properties.push_back("nz", normalProperties[2]); + + plyData.push_back("vertex", vertex); +} + +} // namespace plycpp \ No newline at end of file diff --git a/package.xml b/package.xml index 3736460..b6e33c1 100644 --- a/package.xml +++ b/package.xml @@ -8,7 +8,8 @@ Proprietary ament_cmake - + pcl_conversions + ament_cmake diff --git a/src/example.cpp b/src/example.cpp index a899a83..e9435f9 100644 --- a/src/example.cpp +++ b/src/example.cpp @@ -9,8 +9,8 @@ // copies of the Software, and to permit persons to whom the Software is // furnished to do so, subject to the following conditions : // -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. // // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, @@ -22,157 +22,140 @@ //#include #include "plycpp/plycpp.h" +#include #include #include -#include - - -int main() -{ - - try - { - std::cout << "Loading PLY data..." << std::endl; - plycpp::PLYData data; - - - plycpp::load(std::string(MODELS_DIRECTORY) + "/bunny.ply", data); - //plycpp::load(std::string(MODELS_DIRECTORY) + "/bunny_ascii.ply", data); - - // Listing PLY content - { - std::cout << "List of elements and properties:\n" - << "===========================" << std::endl; - for (const auto& element : data) - { - std::cout << "* " << element.key << " -- size: " << element.data->size() << std::endl; - for (const auto& prop : element.data->properties) - { - std::cout << " - " << prop.key - << " -- type: " - << (prop.data->isList ? "list of " : "") - << prop.data->type.name() - << " -- size: " << prop.data->size() << std::endl; - } - } - std::cout << "\n"; - } - - // Example of direct access - { - auto xData = data["vertex"]->properties["x"]; - std::cout << "x value of the first vertex element:\n" << xData->at(0) << std::endl; - std::cout << "\n"; - } - - // Example of raw pointer access - { - auto vertexElement = data["vertex"]; - std::cout << "Coordinates of the 5 first vertices (out of " << vertexElement->size() << "):\n"; - const float* ptX = vertexElement->properties["x"]->ptr(); - const float* ptY = vertexElement->properties["y"]->ptr(); - const float* ptZ = vertexElement->properties["z"]->ptr(); - for (size_t i = 0; i < 5; ++i) - { - assert(i < vertexElement->size()); - std::cout << "* " << ptX[i] << " " << ptY[i] << " " << ptZ[i] << std::endl; - } - std::cout << "\n"; - } - - // Helper functions to repack data - typedef std::vector > Cloud; - Cloud points; - Cloud normals; - plycpp::toPointCloud(data, points); - plycpp::toNormalCloud(data, normals); - std::cout << "Same output of the 5 first vertices:\n"; - for (size_t i = 0; i < 5; ++i) - { - assert(i < points.size()); - std::cout << "* " << points[i][0] << " " << points[i][1] << " " << points[i][2] << std::endl; - } - std::cout << "\n"; - - // Generic method to pack multiple properties of the same type together - try - { - typedef std::vector > RGBACloud; - RGBACloud rgbaCloud; - std::vector properties - { - data["vertex"]->properties["red"], - data["vertex"]->properties["green"], - data["vertex"]->properties["blue"], - data["vertex"]->properties["alpha"] - }; - plycpp::packProperties(properties, rgbaCloud); - std::cout << "RGBA colour of the 5 first vertices:\n"; - for (size_t i = 0; i < 5; ++i) - { - assert(i < rgbaCloud.size()); - std::cout << "* " - << static_cast(rgbaCloud[i][0]) << " " - << static_cast(rgbaCloud[i][1]) << " " - << static_cast(rgbaCloud[i][2]) << " " - << static_cast(rgbaCloud[i][3]) << std::endl; - } - std::cout << "\n"; - } - catch (const plycpp::Exception &e) - { - std::cout << e.what(); - } - - // Property lists are handled in a similar manner - try - { - const auto& vertexIndicesData = data["face"]->properties["vertex_indices"]; - if (vertexIndicesData && vertexIndicesData->isList) - { - // Only triplet lists are supported - assert(vertexIndicesData->size() % 3 == 0); - assert(vertexIndicesData->size() > 3); - - std::cout << "Vertex indices of the first triangle:\n" << "* " - << vertexIndicesData->at(0) << " " - << vertexIndicesData->at(1) << " " - << vertexIndicesData->at(2) << std::endl; - } - else - std::cout << "No valid list of vertex indices." << std::endl; - } - catch(const plycpp::Exception) - { - std::cout << "INvalid or unsupported face elements." << std::endl; - - } - std::cout << "\n"; - // Export a PLY file - { - // Back conversion of the point cloud and normal cloud to PLYData - plycpp::PLYData newPlyData; - plycpp::fromPointCloudAndNormals(points, normals, newPlyData); - - { - std::string filename = "point_cloud_ascii.ply"; - plycpp::save(filename, newPlyData, plycpp::FileFormat::ASCII); - std::cout << "Point cloud exported to " << filename << std::endl; - } - - { - std::string filename = "point_cloud_binary.ply"; - plycpp::save(filename, newPlyData, plycpp::FileFormat::BINARY); - std::cout << "Point cloud exported to " << filename << std::endl; - } - } - } - catch (const plycpp::Exception& e) - { - std::cout << "An exception happened:\n" << e.what() << std::endl; - } - std::cout << "Enter a char to exit..." << std::endl; - std::getchar(); - return 0; +int main() { + + try { + std::cout << "Loading PLY data..." << std::endl; + plycpp::PLYData data; + + plycpp::load(std::string(MODELS_DIRECTORY) + "/bunny.ply", data); + // plycpp::load(std::string(MODELS_DIRECTORY) + "/bunny_ascii.ply", data); + + // Listing PLY content + { + std::cout << "List of elements and properties:\n" + << "===========================" << std::endl; + for (const auto &element : data) { + std::cout << "* " << element.key << " -- size: " << element.data->size() + << std::endl; + for (const auto &prop : element.data->properties) { + std::cout << " - " << prop.key + << " -- type: " << (prop.data->isList ? "list of " : "") + << prop.data->type.name() + << " -- size: " << prop.data->size() << std::endl; + } + } + std::cout << "\n"; + } + + // Example of direct access + { + auto xData = data["vertex"]->properties["x"]; + std::cout << "x value of the first vertex element:\n" + << xData->at(0) << std::endl; + std::cout << "\n"; + } + + // Example of raw pointer access + { + auto vertexElement = data["vertex"]; + std::cout << "Coordinates of the 5 first vertices (out of " + << vertexElement->size() << "):\n"; + const float *ptX = vertexElement->properties["x"]->ptr(); + const float *ptY = vertexElement->properties["y"]->ptr(); + const float *ptZ = vertexElement->properties["z"]->ptr(); + for (size_t i = 0; i < 5; ++i) { + assert(i < vertexElement->size()); + std::cout << "* " << ptX[i] << " " << ptY[i] << " " << ptZ[i] + << std::endl; + } + std::cout << "\n"; + } + + // Helper functions to repack data + typedef std::vector> Cloud; + Cloud points; + Cloud normals; + plycpp::toPointCloud(data, points); + plycpp::toNormalCloud(data, normals); + std::cout << "Same output of the 5 first vertices:\n"; + for (size_t i = 0; i < 5; ++i) { + assert(i < points.size()); + std::cout << "* " << points[i][0] << " " << points[i][1] << " " + << points[i][2] << std::endl; + } + std::cout << "\n"; + + // Generic method to pack multiple properties of the same type together + try { + typedef std::vector> RGBACloud; + RGBACloud rgbaCloud; + std::vector properties{ + data["vertex"]->properties["red"], + data["vertex"]->properties["green"], + data["vertex"]->properties["blue"], + data["vertex"]->properties["alpha"]}; + plycpp::packProperties(properties, rgbaCloud); + std::cout << "RGBA colour of the 5 first vertices:\n"; + for (size_t i = 0; i < 5; ++i) { + assert(i < rgbaCloud.size()); + std::cout << "* " << static_cast(rgbaCloud[i][0]) << " " + << static_cast(rgbaCloud[i][1]) << " " + << static_cast(rgbaCloud[i][2]) << " " + << static_cast(rgbaCloud[i][3]) << std::endl; + } + std::cout << "\n"; + } catch (const plycpp::Exception &e) { + std::cout << e.what(); + } + + // Property lists are handled in a similar manner + try { + const auto &vertexIndicesData = + data["face"]->properties["vertex_indices"]; + if (vertexIndicesData && vertexIndicesData->isList) { + // Only triplet lists are supported + assert(vertexIndicesData->size() % 3 == 0); + assert(vertexIndicesData->size() > 3); + + std::cout << "Vertex indices of the first triangle:\n" + << "* " << vertexIndicesData->at(0) << " " + << vertexIndicesData->at(1) << " " + << vertexIndicesData->at(2) << std::endl; + } else + std::cout << "No valid list of vertex indices." << std::endl; + } catch (const plycpp::Exception) { + std::cout << "INvalid or unsupported face elements." << std::endl; + } + std::cout << "\n"; + // Export a PLY file + { + // Back conversion of the point cloud and normal cloud to PLYData + plycpp::PLYData newPlyData; + plycpp::fromPointCloudAndNormals(points, normals, + newPlyData); + + { + std::string filename = "point_cloud_ascii.ply"; + plycpp::save(filename, newPlyData, plycpp::FileFormat::ASCII); + std::cout << "Point cloud exported to " << filename << std::endl; + } + + { + std::string filename = "point_cloud_binary.ply"; + plycpp::save(filename, newPlyData, plycpp::FileFormat::BINARY); + std::cout << "Point cloud exported to " << filename << std::endl; + } + } + } catch (const plycpp::Exception &e) { + std::cout << "An exception happened:\n" << e.what() << std::endl; + } + + std::cout << "Enter a char to exit..." << std::endl; + std::getchar(); + return 0; } \ No newline at end of file diff --git a/src/plycpp.cpp b/src/plycpp.cpp index 5d168fb..8ce25e6 100644 --- a/src/plycpp.cpp +++ b/src/plycpp.cpp @@ -9,8 +9,8 @@ // copies of the Software, and to permit persons to whom the Software is // furnished to do so, subject to the following conditions : // -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. // // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, @@ -22,623 +22,516 @@ #include "plycpp/plycpp.h" +#include +#include #include -#include -#include #include -#include -#include +#include #include +#include -namespace plycpp -{ - const std::type_index CHAR = std::type_index(typeid(int8_t)); - const std::type_index UCHAR = std::type_index(typeid(uint8_t)); - const std::type_index SHORT = std::type_index(typeid(int16_t)); - const std::type_index USHORT = std::type_index(typeid(uint16_t)); - const std::type_index INT = std::type_index(typeid(int32_t)); - const std::type_index UINT = std::type_index(typeid(uint32_t)); - const std::type_index FLOAT = std::type_index(typeid(float)); - const std::type_index DOUBLE = std::type_index(typeid(double)); - - bool isBigEndianArchitecture() - { - /*union { - uint32_t i; - char c[4]; - } myunion = { 0x01020304 }; - return myunion.c[0] == 1; - */ - const uint32_t i = 0x01020304; - return reinterpret_cast(&i)[0] == 1; - } - - - - const std::unordered_map dataTypeByteSize{ - { CHAR, sizeof(char) }, - { UCHAR, sizeof(unsigned char) }, - { SHORT, sizeof(int16_t) }, - { USHORT, sizeof(uint16_t) }, - { INT, sizeof(int32_t) }, - { UINT, sizeof(uint32_t) }, - { FLOAT, sizeof(float) }, - { DOUBLE, sizeof(double) } - }; - - static_assert(sizeof(char) == 1, "Inconsistent type size"); - static_assert(sizeof(unsigned char) == 1, "Inconsistent type size"); - static_assert(sizeof(int16_t) == 2, "Inconsistent type size"); - static_assert(sizeof(uint16_t) == 2, "Inconsistent type size"); - static_assert(sizeof(int32_t) == 4, "Inconsistent type size"); - static_assert(sizeof(uint32_t) == 4, "Inconsistent type size"); - static_assert(sizeof(float) == 4, "Inconsistent type size"); - static_assert(sizeof(double) == 8, "Inconsistent type size"); - - const std::unordered_map strToDataType{ - { "char", CHAR }, - { "uchar", UCHAR }, - { "short", SHORT }, - { "ushort", USHORT }, - { "int", INT }, - { "int32", INT }, - { "uint", UINT }, - { "uint32", UINT }, - { "float", FLOAT }, - { "float32", FLOAT }, - { "double", DOUBLE }, - { "float64", DOUBLE } - }; - - const std::unordered_map dataTypeToStr{ - { CHAR, "char" }, - { UCHAR, "uchar" }, - { SHORT, "short" }, - { USHORT, "ushort" }, - { INT, "int" }, - { UINT, "uint" }, - { FLOAT, "float" }, - { DOUBLE, "double" }, - }; - - std::type_index parseDataType(const std::string& name) - { - const auto& it = strToDataType.find(name); - if (it != strToDataType.end()) - { - return it->second; - } - else - throw Exception(std::string("Unkown data type:" + name)); - } - - std::string dataTypeToString(const std::type_index& type) - { - auto it = dataTypeToStr.find(type); - if (it == dataTypeToStr.end()) - throw plycpp::Exception("Invalid data type"); - return it->second; - } - - size_t dataTypeToStepSize(const std::type_index& type) - { - auto it = dataTypeByteSize.find(type); - if (it == dataTypeByteSize.end()) - throw plycpp::Exception("Invalid data type"); - return it->second; - } - - - PropertyArray::PropertyArray(const std::type_index type, const size_t size, const bool isList) - : type(type), - isList(isList), - stepSize(dataTypeToStepSize(type)) - { - this->data.resize(size * this->stepSize); - } - - - void splitString(const std::string& input, std::vector& result) - { - result.clear(); - std::stringstream ss(input); - while (true) - { - std::string elem; - ss >> elem; - if (ss.fail()) - break; - result.push_back(elem); - } - - } - - size_t strtol_except(const std::string& in) - { - std::stringstream ss(in); - size_t val; - ss >> val; - if (ss.fail()) - throw Exception("Invalid unsigned integer"); - return val; - } - - - inline void readASCIIValue(std::ifstream& fin, unsigned char* const ptData, const std::type_index& type) - { - int temp; - if (type == CHAR) - { - fin >> temp; - *reinterpret_cast(ptData) = static_cast(temp); - } - else if (type == UCHAR) - { - fin >> temp; - *reinterpret_cast(ptData) = static_cast(temp); - } - else if (type == SHORT) - { - fin >> *reinterpret_cast(ptData); - } - else if (type == USHORT) - { - fin >> *reinterpret_cast(ptData); - } - else if (type == INT) - { - fin >> *reinterpret_cast(ptData); - } - else if (type == UINT) - { - fin >> *reinterpret_cast(ptData); - } - else if (type == FLOAT) - { - fin >> *reinterpret_cast(ptData); - } - else if (type == DOUBLE) - { - fin >> *reinterpret_cast(ptData); - } - else - throw Exception("Should not happen."); - } - - inline void writeASCIIValue(std::ofstream& fout, unsigned char* const ptData, const std::type_index type) - { - if (type == CHAR) - { - fout << int(*reinterpret_cast(ptData)); - } - else if (type == UCHAR) - { - fout << int(*reinterpret_cast(ptData)); - } - else if (type == SHORT) - { - fout << *reinterpret_cast(ptData); - } - else if (type == USHORT) - { - fout << *reinterpret_cast(ptData); - } - else if (type == INT) - { - fout << *reinterpret_cast(ptData); - } - else if (type == UINT) - { - fout << *reinterpret_cast(ptData); - } - else if (type == FLOAT) - { - fout << *reinterpret_cast(ptData); - } - else if (type == DOUBLE) - { - fout << *reinterpret_cast(ptData); - } - else - throw Exception("Should not happen"); - } - - template - void readDataContent(std::ifstream& fin, PLYData& data) - { - /// Store a pointer to the current place where to write next data for each property of each element - std::unordered_map writingPlace; - for (auto& elementTuple : data) - { - auto& element = elementTuple.data; - for (auto& propertyTuple : element->properties) - { - auto& prop = propertyTuple.data; - writingPlace[prop.get()] = prop->data.data(); - } - } - - //// Iterate over elements array - for (auto& elementArrayTuple : data) - { - auto& elementArray = elementArrayTuple.data; - const size_t elementsCount = elementArray->size(); - // Iterate over elements - for (size_t i = 0; i < elementsCount; ++i) - { - // Iterate over properties of the element - for (auto& propertyTuple : elementArray->properties) - { - auto& prop = propertyTuple.data; - - if (!prop->isList) - { - // Read data - auto& ptData = writingPlace[prop.get()]; - // Safety check - assert(ptData >= prop->data.data()); - assert(ptData + prop->stepSize <= prop->data.data() + prop->data.size()); - - if (format == ASCII) - { - readASCIIValue(fin, ptData, prop->type); - - } - else - { - fin.read(reinterpret_cast(ptData), prop->stepSize); - } - // Increment - ptData += prop->stepSize; - } - else - { - // Read count - uint8_t count; - - if (format == ASCII) - { - int temp; - fin >> temp; - count = static_cast(temp); - } - else - { - fin.read(reinterpret_cast(&count), sizeof(count)); - } - if (fin.fail() || count != 3) - { - throw Exception("Only lists of 3 values are supported"); - } - - // Read data - auto& ptData = writingPlace[prop.get()]; - const size_t chunkSize = 3 * prop->stepSize; - - // Safety check - assert(ptData >= prop->data.data()); - assert(ptData + chunkSize <= prop->data.data() + prop->data.size()); - - if (format == ASCII) - { - readASCIIValue(fin, ptData, prop->type); - ptData += prop->stepSize; - readASCIIValue(fin, ptData, prop->type); - ptData += prop->stepSize; - readASCIIValue(fin, ptData, prop->type); - ptData += prop->stepSize; - } - else - { - fin.read(reinterpret_cast(ptData), chunkSize); - ptData += chunkSize; - } - } - } - } - } - } - - void myGetline(std::ifstream& fin, std::string& line) - { - std::getline(fin, line); - // Files created with Windows have a carriage return - if (!line.empty() && line.back() == '\r') - line.pop_back(); - } - - void load(const std::string& filename, PLYData& data) - { - // Read header and reserve memory - data.clear(); - std::string format; - std::string version; - - std::ifstream fin(filename, std::ios::binary); - //fin.sync_with_stdio(false); - - if (!fin.is_open()) - throw Exception(std::string("Unable to open ") + filename); - - std::string line; - myGetline(fin, line); - - std::shared_ptr currentElement = nullptr; - - if (line != "ply") - { - throw Exception("Missing magic number ""ply"""); - } - - while (line != "end_header") - { - myGetline(fin, line); - if (fin.fail()) - throw Exception("Header parsing exception"); - - std::vector lineContent; - splitString(line, lineContent); - - if (lineContent.size() == 3 && lineContent[0] == "format") - { - format = lineContent[1]; - version = lineContent[2]; - } - if (lineContent.size() == 3 && lineContent[0] == "element") - { - // New element - const std::string& name = lineContent[1]; - const size_t count = strtol_except(lineContent[2]); - - currentElement.reset(new ElementArray(count)); - - data.push_back(name, currentElement); - } - else if (lineContent.size() == 3 && lineContent[0] == "property") - { - if (!currentElement) - throw Exception("Header issue!"); - - // New property - const std::type_index dataType = parseDataType(lineContent[1]); - const std::string& name = lineContent[2]; - - std::shared_ptr newProperty(new PropertyArray(dataType, currentElement->size())); - currentElement->properties.push_back(name, newProperty); - } - else if (lineContent.size() == 5 && lineContent[0] == "property" && lineContent[1] == "list") - { - if (!currentElement) - throw Exception("Header issue!"); - - const std::type_index indexCountType = parseDataType(lineContent[2]); - const std::type_index dataType = parseDataType(lineContent[3]); - const std::string& name = lineContent[4]; - - if (indexCountType != UCHAR) - throw Exception("Only uchar is supported as counting type for lists"); - - std::shared_ptr newProperty(new PropertyArray(dataType, 3 * currentElement->size(), true)); - currentElement->properties.push_back(name, newProperty); - } - - } - - if (fin.fail()) - { - throw Exception("Issue while parsing header"); - } - - - ///////////////////////////////////// - // Read data - if (format == "ascii") - { - readDataContent(fin, data); - - if (fin.fail()) - { - throw Exception("Issue while parsing ascii data"); - } - } - else - { - const bool isBigEndianArchitecture_ = isBigEndianArchitecture(); - if (format != "binary_little_endian" && format != "binary_big_endian") - throw Exception("Unknown binary format"); - - - if ((isBigEndianArchitecture_ && format != "binary_big_endian") - || (!isBigEndianArchitecture_ && format != "binary_little_endian")) - throw Exception("Endianness conversion is not supported yet"); - - readDataContent(fin, data); - - if (fin.fail()) - { - throw Exception("Issue while parsing binary data"); - } - - // Ensure we reached the end of file by trying to read a last char - char useless; - fin.read(&useless, 1); - if (!fin.eof()) - { - throw Exception("End of file not reached at the end of parsing."); - } - } - } - - - template - void writeDataContent(std::ofstream& fout, const PLYData& data) - { - /// Store a pointer to the current place from which to read next data for each property of each element - std::unordered_map readingPlace; - for (auto& elementTuple : data) - { - auto& element = elementTuple.data; - for (auto& propertyTuple : element->properties) - { - auto& prop = propertyTuple.data; - readingPlace[prop.get()] = prop->data.data(); - } - } - - //// Iterate over elements array - for (auto& elementArrayTuple : data) - { - auto& elementArray = elementArrayTuple.data; - const size_t elementsCount = elementArray->size(); - // Iterate over elements - for (size_t i = 0; i < elementsCount; ++i) - { - // Iterate over properties of the element - for (auto& propertyTuple : elementArray->properties) - { - auto& prop = propertyTuple.data; - // Write data - auto& ptData = readingPlace[prop.get()]; - if (!prop->isList) - { - // Safety check - assert(ptData >= prop->data.data()); - assert(ptData + prop->stepSize <= prop->data.data() + prop->data.size()); - if (format == FileFormat::BINARY) - fout.write(reinterpret_cast(ptData), prop->stepSize); - else - { - writeASCIIValue(fout, ptData, prop->type); - fout << " "; - } - ptData += prop->stepSize; - } - else - { - if (format == FileFormat::BINARY) - { - const unsigned char count = 3; - // Write the number of elements - fout.write(reinterpret_cast(&count), sizeof(unsigned char)); - // Write data - const size_t chunckSize = 3 * prop->stepSize; - // Safety check - assert(ptData >= prop->data.data()); - assert(ptData + chunckSize <= prop->data.data() + prop->data.size()); - fout.write(reinterpret_cast(ptData), chunckSize); - ptData += chunckSize; - } - else - { - fout << "3 "; - writeASCIIValue(fout, ptData, prop->type); - fout << " "; - ptData += prop->stepSize; - writeASCIIValue(fout, ptData, prop->type); - fout << " "; - ptData += prop->stepSize; - writeASCIIValue(fout, ptData, prop->type); - fout << " "; - ptData += prop->stepSize; - } - - } - - - } - if (format == FileFormat::ASCII) - { - fout << "\n"; - } - } - } - } - - void save(const std::string& filename, const PLYData& data, const FileFormat format) - { - std::ofstream fout(filename, std::ios::binary); - - // Write header - fout << "ply\n"; - switch (format) - { - case FileFormat::ASCII: - fout << "format ascii 1.0\n"; - break; - case FileFormat::BINARY: - if (isBigEndianArchitecture()) - fout << "format binary_big_endian 1.0\n"; - else - fout << "format binary_little_endian 1.0\n"; - break; - default: - throw Exception("Unknown file format. Should not happen."); - break; - } - - // Iterate over elements array - for (const auto& elementArrayTuple : data) - { - const auto& elementArrayName = elementArrayTuple.key; - auto& elementArray = elementArrayTuple.data; - const size_t elementsCount = elementArray->size(); - - fout << "element " << elementArrayName << " " << elementsCount << std::endl; - // Iterate over properties - for (const auto& propertyTuple : elementArray->properties) - { - auto& propName = propertyTuple.key; - auto& prop = propertyTuple.data; - - if (!prop) - throw Exception("Null property " + elementArrayName + " -- " + propName); - - // String name of the property type - const auto& itTypeName = dataTypeToStr.find(prop->type); - if (itTypeName == dataTypeToStr.end()) - throw Exception("Should not happen"); - - if (!prop->isList) - { - if (prop->data.size() != elementsCount * prop->stepSize) - { - throw Exception("Inconsistent size for " + elementArrayName + " -- " + propName); - } - - fout << "property " << itTypeName->second << " " << propName << std::endl; - } - else - { - if (prop->data.size() != 3 * elementsCount * prop->stepSize) - { - throw Exception("Inconsistent size for list " + elementArrayName + " -- " + propName); - } - - fout << "property list uchar " << itTypeName->second << " " << propName << std::endl; - } - - } - } - fout << "end_header" << std::endl; - - // Write data - switch (format) - { - case FileFormat::BINARY: - writeDataContent(fout, data); - break; - case FileFormat::ASCII: - writeDataContent(fout, data); - break; - default: - throw Exception("Unknown file format. Should not happen."); - break; - } - - - if (fout.fail()) - { - throw Exception("Problem while writing binary data"); - } - } -} \ No newline at end of file +namespace plycpp { +const std::type_index CHAR = std::type_index(typeid(int8_t)); +const std::type_index UCHAR = std::type_index(typeid(uint8_t)); +const std::type_index SHORT = std::type_index(typeid(int16_t)); +const std::type_index USHORT = std::type_index(typeid(uint16_t)); +const std::type_index INT = std::type_index(typeid(int32_t)); +const std::type_index UINT = std::type_index(typeid(uint32_t)); +const std::type_index FLOAT = std::type_index(typeid(float)); +const std::type_index DOUBLE = std::type_index(typeid(double)); + +bool isBigEndianArchitecture() { + /*union { +uint32_t i; +char c[4]; +} myunion = { 0x01020304 }; +return myunion.c[0] == 1; +*/ + const uint32_t i = 0x01020304; + return reinterpret_cast(&i)[0] == 1; +} + +const std::unordered_map dataTypeByteSize{ + {CHAR, sizeof(char)}, {UCHAR, sizeof(unsigned char)}, + {SHORT, sizeof(int16_t)}, {USHORT, sizeof(uint16_t)}, + {INT, sizeof(int32_t)}, {UINT, sizeof(uint32_t)}, + {FLOAT, sizeof(float)}, {DOUBLE, sizeof(double)}}; + +static_assert(sizeof(char) == 1, "Inconsistent type size"); +static_assert(sizeof(unsigned char) == 1, "Inconsistent type size"); +static_assert(sizeof(int16_t) == 2, "Inconsistent type size"); +static_assert(sizeof(uint16_t) == 2, "Inconsistent type size"); +static_assert(sizeof(int32_t) == 4, "Inconsistent type size"); +static_assert(sizeof(uint32_t) == 4, "Inconsistent type size"); +static_assert(sizeof(float) == 4, "Inconsistent type size"); +static_assert(sizeof(double) == 8, "Inconsistent type size"); + +const std::unordered_map strToDataType{ + {"char", CHAR}, {"uchar", UCHAR}, {"short", SHORT}, + {"ushort", USHORT}, {"int", INT}, {"int32", INT}, + {"uint", UINT}, {"uint32", UINT}, {"float", FLOAT}, + {"float32", FLOAT}, {"double", DOUBLE}, {"float64", DOUBLE}}; + +const std::unordered_map dataTypeToStr{ + {CHAR, "char"}, {UCHAR, "uchar"}, {SHORT, "short"}, {USHORT, "ushort"}, + {INT, "int"}, {UINT, "uint"}, {FLOAT, "float"}, {DOUBLE, "double"}, +}; + +std::type_index parseDataType(const std::string &name) { + const auto &it = strToDataType.find(name); + if (it != strToDataType.end()) { + return it->second; + } else + throw Exception(std::string("Unkown data type:" + name)); +} + +std::string dataTypeToString(const std::type_index &type) { + auto it = dataTypeToStr.find(type); + if (it == dataTypeToStr.end()) + throw plycpp::Exception("Invalid data type"); + return it->second; +} + +size_t dataTypeToStepSize(const std::type_index &type) { + auto it = dataTypeByteSize.find(type); + if (it == dataTypeByteSize.end()) + throw plycpp::Exception("Invalid data type"); + return it->second; +} + +PropertyArray::PropertyArray(const std::type_index type, const size_t size, + const bool isList) + : type(type), isList(isList), stepSize(dataTypeToStepSize(type)) { + this->data.resize(size * this->stepSize); +} + +void splitString(const std::string &input, std::vector &result) { + result.clear(); + std::stringstream ss(input); + while (true) { + std::string elem; + ss >> elem; + if (ss.fail()) + break; + result.push_back(elem); + } +} + +size_t strtol_except(const std::string &in) { + std::stringstream ss(in); + size_t val; + ss >> val; + if (ss.fail()) + throw Exception("Invalid unsigned integer"); + return val; +} + +inline void readASCIIValue(std::ifstream &fin, unsigned char *const ptData, + const std::type_index &type) { + int temp; + if (type == CHAR) { + fin >> temp; + *reinterpret_cast(ptData) = static_cast(temp); + } else if (type == UCHAR) { + fin >> temp; + *reinterpret_cast(ptData) = static_cast(temp); + } else if (type == SHORT) { + fin >> *reinterpret_cast(ptData); + } else if (type == USHORT) { + fin >> *reinterpret_cast(ptData); + } else if (type == INT) { + fin >> *reinterpret_cast(ptData); + } else if (type == UINT) { + fin >> *reinterpret_cast(ptData); + } else if (type == FLOAT) { + fin >> *reinterpret_cast(ptData); + } else if (type == DOUBLE) { + fin >> *reinterpret_cast(ptData); + } else + throw Exception("Should not happen."); +} + +inline void writeASCIIValue(std::ofstream &fout, unsigned char *const ptData, + const std::type_index type) { + if (type == CHAR) { + fout << int(*reinterpret_cast(ptData)); + } else if (type == UCHAR) { + fout << int(*reinterpret_cast(ptData)); + } else if (type == SHORT) { + fout << *reinterpret_cast(ptData); + } else if (type == USHORT) { + fout << *reinterpret_cast(ptData); + } else if (type == INT) { + fout << *reinterpret_cast(ptData); + } else if (type == UINT) { + fout << *reinterpret_cast(ptData); + } else if (type == FLOAT) { + fout << *reinterpret_cast(ptData); + } else if (type == DOUBLE) { + fout << *reinterpret_cast(ptData); + } else + throw Exception("Should not happen"); +} + +template +void readDataContent(std::ifstream &fin, PLYData &data) { + /// Store a pointer to the current place where to write next data for each + /// property of each element + std::unordered_map writingPlace; + for (auto &elementTuple : data) { + auto &element = elementTuple.data; + for (auto &propertyTuple : element->properties) { + auto &prop = propertyTuple.data; + writingPlace[prop.get()] = prop->data.data(); + } + } + + //// Iterate over elements array + for (auto &elementArrayTuple : data) { + auto &elementArray = elementArrayTuple.data; + const size_t elementsCount = elementArray->size(); + // Iterate over elements + for (size_t i = 0; i < elementsCount; ++i) { + // Iterate over properties of the element + for (auto &propertyTuple : elementArray->properties) { + auto &prop = propertyTuple.data; + + if (!prop->isList) { + // Read data + auto &ptData = writingPlace[prop.get()]; + // Safety check + assert(ptData >= prop->data.data()); + assert(ptData + prop->stepSize <= + prop->data.data() + prop->data.size()); + + if (format == ASCII) { + readASCIIValue(fin, ptData, prop->type); + } else { + fin.read(reinterpret_cast(ptData), prop->stepSize); + } + // Increment + ptData += prop->stepSize; + } else { + // Read count + uint8_t count; + + if (format == ASCII) { + int temp; + fin >> temp; + count = static_cast(temp); + } else { + fin.read(reinterpret_cast(&count), sizeof(count)); + } + if (fin.fail() || count != 3) { + throw Exception("Only lists of 3 values are supported"); + } + + // Read data + auto &ptData = writingPlace[prop.get()]; + const size_t chunkSize = 3 * prop->stepSize; + + // Safety check + assert(ptData >= prop->data.data()); + assert(ptData + chunkSize <= prop->data.data() + prop->data.size()); + + if (format == ASCII) { + readASCIIValue(fin, ptData, prop->type); + ptData += prop->stepSize; + readASCIIValue(fin, ptData, prop->type); + ptData += prop->stepSize; + readASCIIValue(fin, ptData, prop->type); + ptData += prop->stepSize; + } else { + fin.read(reinterpret_cast(ptData), chunkSize); + ptData += chunkSize; + } + } + } + } + } +} + +void myGetline(std::ifstream &fin, std::string &line) { + std::getline(fin, line); + // Files created with Windows have a carriage return + if (!line.empty() && line.back() == '\r') + line.pop_back(); +} + +void load(const std::string &filename, PLYData &data) { + // Read header and reserve memory + data.clear(); + std::string format; + std::string version; + + std::ifstream fin(filename, std::ios::binary); + // fin.sync_with_stdio(false); + + if (!fin.is_open()) + throw Exception(std::string("Unable to open ") + filename); + + std::string line; + myGetline(fin, line); + + std::shared_ptr currentElement = nullptr; + + if (line != "ply") { + throw Exception("Missing magic number " + "ply" + ""); + } + + while (line != "end_header") { + myGetline(fin, line); + if (fin.fail()) + throw Exception("Header parsing exception"); + + std::vector lineContent; + splitString(line, lineContent); + + if (lineContent.size() == 3 && lineContent[0] == "format") { + format = lineContent[1]; + version = lineContent[2]; + } + if (lineContent.size() == 3 && lineContent[0] == "element") { + // New element + const std::string &name = lineContent[1]; + const size_t count = strtol_except(lineContent[2]); + + currentElement.reset(new ElementArray(count)); + + data.push_back(name, currentElement); + } else if (lineContent.size() == 3 && lineContent[0] == "property") { + if (!currentElement) + throw Exception("Header issue!"); + + // New property + const std::type_index dataType = parseDataType(lineContent[1]); + const std::string &name = lineContent[2]; + + std::shared_ptr newProperty( + new PropertyArray(dataType, currentElement->size())); + currentElement->properties.push_back(name, newProperty); + } else if (lineContent.size() == 5 && lineContent[0] == "property" && + lineContent[1] == "list") { + if (!currentElement) + throw Exception("Header issue!"); + + const std::type_index indexCountType = parseDataType(lineContent[2]); + const std::type_index dataType = parseDataType(lineContent[3]); + const std::string &name = lineContent[4]; + + if (indexCountType != UCHAR) + throw Exception("Only uchar is supported as counting type for lists"); + + std::shared_ptr newProperty( + new PropertyArray(dataType, 3 * currentElement->size(), true)); + currentElement->properties.push_back(name, newProperty); + } + } + + if (fin.fail()) { + throw Exception("Issue while parsing header"); + } + + ///////////////////////////////////// + // Read data + if (format == "ascii") { + readDataContent(fin, data); + + if (fin.fail()) { + throw Exception("Issue while parsing ascii data"); + } + } else { + const bool isBigEndianArchitecture_ = isBigEndianArchitecture(); + if (format != "binary_little_endian" && format != "binary_big_endian") + throw Exception("Unknown binary format"); + + if ((isBigEndianArchitecture_ && format != "binary_big_endian") || + (!isBigEndianArchitecture_ && format != "binary_" + "little_" + "endia" + "n")) + throw Exception("Endianness conversion is not supported yet"); + + readDataContent(fin, data); + + if (fin.fail()) { + throw Exception("Issue while parsing binary data"); + } + + // Ensure we reached the end of file by trying to read a last char + char useless; + fin.read(&useless, 1); + if (!fin.eof()) { + throw Exception("End of file not reached at the end of parsing."); + } + } +} + +template +void writeDataContent(std::ofstream &fout, const PLYData &data) { + /// Store a pointer to the current place from which to read next data for each + /// property of each element + std::unordered_map readingPlace; + for (auto &elementTuple : data) { + auto &element = elementTuple.data; + for (auto &propertyTuple : element->properties) { + auto &prop = propertyTuple.data; + readingPlace[prop.get()] = prop->data.data(); + } + } + + //// Iterate over elements array + for (auto &elementArrayTuple : data) { + auto &elementArray = elementArrayTuple.data; + const size_t elementsCount = elementArray->size(); + // Iterate over elements + for (size_t i = 0; i < elementsCount; ++i) { + // Iterate over properties of the element + for (auto &propertyTuple : elementArray->properties) { + auto &prop = propertyTuple.data; + // Write data + auto &ptData = readingPlace[prop.get()]; + if (!prop->isList) { + // Safety check + assert(ptData >= prop->data.data()); + assert(ptData + prop->stepSize <= + prop->data.data() + prop->data.size()); + if (format == FileFormat::BINARY) + fout.write(reinterpret_cast(ptData), prop->stepSize); + else { + writeASCIIValue(fout, ptData, prop->type); + fout << " "; + } + ptData += prop->stepSize; + } else { + if (format == FileFormat::BINARY) { + const unsigned char count = 3; + // Write the number of elements + fout.write(reinterpret_cast(&count), + sizeof(unsigned char)); + // Write data + const size_t chunckSize = 3 * prop->stepSize; + // Safety check + assert(ptData >= prop->data.data()); + assert(ptData + chunckSize <= + prop->data.data() + prop->data.size()); + fout.write(reinterpret_cast(ptData), chunckSize); + ptData += chunckSize; + } else { + fout << "3 "; + writeASCIIValue(fout, ptData, prop->type); + fout << " "; + ptData += prop->stepSize; + writeASCIIValue(fout, ptData, prop->type); + fout << " "; + ptData += prop->stepSize; + writeASCIIValue(fout, ptData, prop->type); + fout << " "; + ptData += prop->stepSize; + } + } + } + if (format == FileFormat::ASCII) { + fout << "\n"; + } + } + } +} + +void save(const std::string &filename, const PLYData &data, + const FileFormat format) { + std::ofstream fout(filename, std::ios::binary); + + // Write header + fout << "ply\n"; + switch (format) { + case FileFormat::ASCII: + fout << "format ascii 1.0\n"; + break; + case FileFormat::BINARY: + if (isBigEndianArchitecture()) + fout << "format binary_big_endian 1.0\n"; + else + fout << "format binary_little_endian 1.0\n"; + break; + default: + throw Exception("Unknown file format. Should not happen."); + break; + } + + // Iterate over elements array + for (const auto &elementArrayTuple : data) { + const auto &elementArrayName = elementArrayTuple.key; + auto &elementArray = elementArrayTuple.data; + const size_t elementsCount = elementArray->size(); + + fout << "element " << elementArrayName << " " << elementsCount << std::endl; + // Iterate over properties + for (const auto &propertyTuple : elementArray->properties) { + auto &propName = propertyTuple.key; + auto &prop = propertyTuple.data; + + if (!prop) + throw Exception("Null property " + elementArrayName + " -- " + + propName); + + // String name of the property type + const auto &itTypeName = dataTypeToStr.find(prop->type); + if (itTypeName == dataTypeToStr.end()) + throw Exception("Should not happen"); + + if (!prop->isList) { + if (prop->data.size() != elementsCount * prop->stepSize) { + throw Exception("Inconsistent size for " + elementArrayName + " -- " + + propName); + } + + fout << "property " << itTypeName->second << " " << propName + << std::endl; + } else { + if (prop->data.size() != 3 * elementsCount * prop->stepSize) { + throw Exception("Inconsistent size for list " + elementArrayName + + " -- " + propName); + } + + fout << "property list uchar " << itTypeName->second << " " << propName + << std::endl; + } + } + } + fout << "end_header" << std::endl; + + // Write data + switch (format) { + case FileFormat::BINARY: + writeDataContent(fout, data); + break; + case FileFormat::ASCII: + writeDataContent(fout, data); + break; + default: + throw Exception("Unknown file format. Should not happen."); + break; + } + + if (fout.fail()) { + throw Exception("Problem while writing binary data"); + } +} +void savePLYFile(const std::string &filename, + pcl::PointCloud::Ptr cloud, + const FileFormat format) { + plycpp::PLYData data; + typedef std::vector> Cloud; + Cloud points; + points.reserve(cloud->size()); + for (size_t index = 0; index < cloud->points.size(); index++) { + points.push_back({cloud->points.at(index).x, cloud->points.at(index).y, + cloud->points.at(index).z}); + } + plycpp::fromPointCloud(points, data); + plycpp::save(filename, data, format); +} +} // namespace plycpp \ No newline at end of file