From 35b16d8d56c5d6e757251ef136dbd86fffd01525 Mon Sep 17 00:00:00 2001 From: Jad Date: Fri, 3 Mar 2023 12:16:33 +0200 Subject: [PATCH 1/6] Added install log and build folders to gitignore --- .gitignore | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 From 9c354446d1d0012f64f1ab446be614154363f3db Mon Sep 17 00:00:00 2001 From: Jad Date: Fri, 3 Mar 2023 12:17:36 +0200 Subject: [PATCH 2/6] pcl lib in order to convert from pointcloud to ply --- CMakeLists.txt | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) 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() From 4039d9f20279fe0b3b47e4283bea2310e152d40d Mon Sep 17 00:00:00 2001 From: Jad Date: Fri, 3 Mar 2023 12:17:41 +0200 Subject: [PATCH 3/6] pcl lib in order to convert from pointcloud to ply --- package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 From 0b0386046e7684ab3d1a40080cbd5814da255685 Mon Sep 17 00:00:00 2001 From: Jad Date: Fri, 3 Mar 2023 12:20:34 +0200 Subject: [PATCH 4/6] created savePLYFile function --- include/plycpp/plycpp.h | 116 +++++++++++++++++++++------------------- 1 file changed, 61 insertions(+), 55 deletions(-) diff --git a/include/plycpp/plycpp.h b/include/plycpp/plycpp.h index 149f935..d8bbf82 100644 --- a/include/plycpp/plycpp.h +++ b/include/plycpp/plycpp.h @@ -28,7 +28,8 @@ #include #include #include - +#include +#include namespace plycpp { @@ -53,16 +54,18 @@ namespace plycpp /** Error message. */ std::string exception_; + public: - Exception(const std::string& msg) + Exception(const std::string &msg) : exception_(msg.c_str()) - {} + { + } }; - template + template struct KeyData { - KeyData(const Key key, const T& data) + KeyData(const Key key, const T &data) { this->key = key; this->data = data; @@ -74,28 +77,29 @@ namespace plycpp /// 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 + template class IndexedList { private: - typedef KeyData > MyKeyData; - typedef std::vector Container; + 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) + std::shared_ptr operator[](const Key &key) { - auto it = std::find_if(begin(), end(), [&key](const MyKeyData& a){ return a.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 + const std::shared_ptr operator[](const Key &key) const { - auto it = std::find_if(begin(), end(), [&key](const MyKeyData& a){ return a.key == key; }); + auto it = std::find_if(begin(), end(), [&key](const MyKeyData &a) { return a.key == key; }); if (it != end()) return it->data; else @@ -111,7 +115,7 @@ namespace plycpp return false; } - void push_back(const Key& key, const std::shared_ptr& data) + void push_back(const Key &key, const std::shared_ptr &data) { container.push_back(MyKeyData(key, data)); } @@ -134,31 +138,31 @@ namespace plycpp class ElementArray; typedef std::shared_ptr PropertyArrayConstPtr; typedef std::shared_ptr PropertyArrayPtr; - typedef IndexedList PLYData; + typedef IndexedList PLYData; class PropertyArray { public: PropertyArray(const std::type_index type, const size_t size, const bool isList = false); - template + template bool isOfType() const { return type == std::type_index(typeid(T)); } - template - const T* ptr() const + template + const T *ptr() const { assert(isOfType()); - return reinterpret_cast(&data[0]); + return reinterpret_cast(&data[0]); } - template - T* ptr() + template + T *ptr() { assert(isOfType()); - return reinterpret_cast(data.data()); + return reinterpret_cast(data.data()); } const size_t size() const @@ -167,20 +171,20 @@ namespace plycpp return data.size() / stepSize; } - template - const T& at(const size_t i) const + template + const T &at(const size_t i) const { assert(isOfType()); - assert((i+1) * stepSize <= data.size()); - return *reinterpret_cast(&data[i * stepSize]); + assert((i + 1) * stepSize <= data.size()); + return *reinterpret_cast(&data[i * stepSize]); } - template - T& at(const size_t i) + template + T &at(const size_t i) { assert(isOfType()); assert((i + 1) * stepSize <= data.size()); - return *reinterpret_cast(&data[i * stepSize]); + return *reinterpret_cast(&data[i * stepSize]); } std::vector data; @@ -194,28 +198,32 @@ namespace plycpp public: ElementArray(const size_t size) : size_(size) - {} + { + } - IndexedList properties; + IndexedList properties; size_t size() const { return size_; } + private: size_t size_; }; /// Load PLY data - void load(const std::string& filename, PLYData& 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); + 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) + template + void packProperties(std::vector> properties, OutputVector &output) { output.clear(); @@ -226,8 +234,8 @@ namespace plycpp const size_t nbProperties = properties.size(); // Pointers to actual data - std::vector ptsData; - for (auto& prop : properties) + std::vector ptsData; + for (auto &prop : properties) { // Check type consistency if (!prop || !prop->isOfType() || prop->size() != size) @@ -249,16 +257,16 @@ namespace plycpp } /// Unpack a multichannel vector into a list of properties. - template - void unpackProperties(const OutputVector& cloud, std::vector < std::shared_ptr >& 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) + std::vector ptsData; + for (auto &prop : properties) { prop.reset(new PropertyArray(std::type_index(typeid(T)), size)); ptsData.push_back(prop->ptr()); @@ -274,37 +282,36 @@ namespace plycpp } } - - template - void toPointCloud(const PLYData& plyData, Cloud& cloud) + 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"] }; + std::vector> properties{plyVertex->properties["x"], plyVertex->properties["y"], plyVertex->properties["z"]}; packProperties(properties, cloud); } - template - void toNormalCloud(const PLYData& plyData, Cloud& 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"] }; + std::vector> properties{plyVertex->properties["nx"], plyVertex->properties["ny"], plyVertex->properties["nz"]}; packProperties(properties, cloud); } - template - void fromPointCloud(const Cloud& points, PLYData& plyData) + template + void fromPointCloud(const Cloud &points, PLYData &plyData) { const size_t size = points.size(); plyData.clear(); - std::vector > positionProperties(3); + std::vector> positionProperties(3); unpackProperties(points, positionProperties); std::shared_ptr vertex(new ElementArray(size)); @@ -315,21 +322,20 @@ namespace plycpp plyData.push_back("vertex", vertex); } - template - void fromPointCloudAndNormals(const Cloud& points, const Cloud& normals, PLYData& plyData) + 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); + std::vector> positionProperties(3); unpackProperties(points, positionProperties); - std::vector > normalProperties(3); + std::vector> normalProperties(3); unpackProperties(normals, normalProperties); std::shared_ptr vertex(new ElementArray(size)); From 17ae27eb618dc8a9f08550d85f583e20000319e9 Mon Sep 17 00:00:00 2001 From: Jad Date: Fri, 3 Mar 2023 12:20:47 +0200 Subject: [PATCH 5/6] created savePLYFile function in cpp --- src/plycpp.cpp | 235 ++++++++++++++++++++++++------------------------- 1 file changed, 116 insertions(+), 119 deletions(-) diff --git a/src/plycpp.cpp b/src/plycpp.cpp index 5d168fb..d3e2c3d 100644 --- a/src/plycpp.cpp +++ b/src/plycpp.cpp @@ -50,21 +50,18 @@ namespace plycpp return myunion.c[0] == 1; */ const uint32_t i = 0x01020304; - return reinterpret_cast(&i)[0] == 1; + 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) } - }; + {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"); @@ -76,34 +73,33 @@ namespace plycpp 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 } - }; + {"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" }, + {CHAR, "char"}, + {UCHAR, "uchar"}, + {SHORT, "short"}, + {USHORT, "ushort"}, + {INT, "int"}, + {UINT, "uint"}, + {FLOAT, "float"}, + {DOUBLE, "double"}, }; - std::type_index parseDataType(const std::string& name) + std::type_index parseDataType(const std::string &name) { - const auto& it = strToDataType.find(name); + const auto &it = strToDataType.find(name); if (it != strToDataType.end()) { return it->second; @@ -112,7 +108,7 @@ namespace plycpp throw Exception(std::string("Unkown data type:" + name)); } - std::string dataTypeToString(const std::type_index& type) + std::string dataTypeToString(const std::type_index &type) { auto it = dataTypeToStr.find(type); if (it == dataTypeToStr.end()) @@ -120,7 +116,7 @@ namespace plycpp return it->second; } - size_t dataTypeToStepSize(const std::type_index& type) + size_t dataTypeToStepSize(const std::type_index &type) { auto it = dataTypeByteSize.find(type); if (it == dataTypeByteSize.end()) @@ -128,17 +124,15 @@ namespace plycpp return it->second; } - PropertyArray::PropertyArray(const std::type_index type, const size_t size, const bool isList) : type(type), - isList(isList), - stepSize(dataTypeToStepSize(type)) + isList(isList), + stepSize(dataTypeToStepSize(type)) { this->data.resize(size * this->stepSize); } - - void splitString(const std::string& input, std::vector& result) + void splitString(const std::string &input, std::vector &result) { result.clear(); std::stringstream ss(input); @@ -150,10 +144,9 @@ namespace plycpp break; result.push_back(elem); } - } - size_t strtol_except(const std::string& in) + size_t strtol_except(const std::string &in) { std::stringstream ss(in); size_t val; @@ -163,118 +156,117 @@ namespace plycpp return val; } - - inline void readASCIIValue(std::ifstream& fin, unsigned char* const ptData, const std::type_index& type) + 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); + *reinterpret_cast(ptData) = static_cast(temp); } else if (type == UCHAR) { fin >> temp; - *reinterpret_cast(ptData) = static_cast(temp); + *reinterpret_cast(ptData) = static_cast(temp); } else if (type == SHORT) { - fin >> *reinterpret_cast(ptData); + fin >> *reinterpret_cast(ptData); } else if (type == USHORT) { - fin >> *reinterpret_cast(ptData); + fin >> *reinterpret_cast(ptData); } else if (type == INT) { - fin >> *reinterpret_cast(ptData); + fin >> *reinterpret_cast(ptData); } else if (type == UINT) { - fin >> *reinterpret_cast(ptData); + fin >> *reinterpret_cast(ptData); } else if (type == FLOAT) { - fin >> *reinterpret_cast(ptData); + fin >> *reinterpret_cast(ptData); } else if (type == DOUBLE) { - fin >> *reinterpret_cast(ptData); + 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) + inline void writeASCIIValue(std::ofstream &fout, unsigned char *const ptData, const std::type_index type) { if (type == CHAR) { - fout << int(*reinterpret_cast(ptData)); + fout << int(*reinterpret_cast(ptData)); } else if (type == UCHAR) { - fout << int(*reinterpret_cast(ptData)); + fout << int(*reinterpret_cast(ptData)); } else if (type == SHORT) { - fout << *reinterpret_cast(ptData); + fout << *reinterpret_cast(ptData); } else if (type == USHORT) { - fout << *reinterpret_cast(ptData); + fout << *reinterpret_cast(ptData); } else if (type == INT) { - fout << *reinterpret_cast(ptData); + fout << *reinterpret_cast(ptData); } else if (type == UINT) { - fout << *reinterpret_cast(ptData); + fout << *reinterpret_cast(ptData); } else if (type == FLOAT) { - fout << *reinterpret_cast(ptData); + fout << *reinterpret_cast(ptData); } else if (type == DOUBLE) { - fout << *reinterpret_cast(ptData); + fout << *reinterpret_cast(ptData); } else throw Exception("Should not happen"); } template - void readDataContent(std::ifstream& fin, PLYData& data) + 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) + std::unordered_map writingPlace; + for (auto &elementTuple : data) { - auto& element = elementTuple.data; - for (auto& propertyTuple : element->properties) + auto &element = elementTuple.data; + for (auto &propertyTuple : element->properties) { - auto& prop = propertyTuple.data; + auto &prop = propertyTuple.data; writingPlace[prop.get()] = prop->data.data(); } } //// Iterate over elements array - for (auto& elementArrayTuple : data) + for (auto &elementArrayTuple : data) { - auto& elementArray = 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) + for (auto &propertyTuple : elementArray->properties) { - auto& prop = propertyTuple.data; + auto &prop = propertyTuple.data; if (!prop->isList) { // Read data - auto& ptData = writingPlace[prop.get()]; + auto &ptData = writingPlace[prop.get()]; // Safety check assert(ptData >= prop->data.data()); assert(ptData + prop->stepSize <= prop->data.data() + prop->data.size()); @@ -282,11 +274,10 @@ namespace plycpp if (format == ASCII) { readASCIIValue(fin, ptData, prop->type); - } else { - fin.read(reinterpret_cast(ptData), prop->stepSize); + fin.read(reinterpret_cast(ptData), prop->stepSize); } // Increment ptData += prop->stepSize; @@ -304,7 +295,7 @@ namespace plycpp } else { - fin.read(reinterpret_cast(&count), sizeof(count)); + fin.read(reinterpret_cast(&count), sizeof(count)); } if (fin.fail() || count != 3) { @@ -312,7 +303,7 @@ namespace plycpp } // Read data - auto& ptData = writingPlace[prop.get()]; + auto &ptData = writingPlace[prop.get()]; const size_t chunkSize = 3 * prop->stepSize; // Safety check @@ -330,7 +321,7 @@ namespace plycpp } else { - fin.read(reinterpret_cast(ptData), chunkSize); + fin.read(reinterpret_cast(ptData), chunkSize); ptData += chunkSize; } } @@ -339,7 +330,7 @@ namespace plycpp } } - void myGetline(std::ifstream& fin, std::string& line) + void myGetline(std::ifstream &fin, std::string &line) { std::getline(fin, line); // Files created with Windows have a carriage return @@ -347,7 +338,7 @@ namespace plycpp line.pop_back(); } - void load(const std::string& filename, PLYData& data) + void load(const std::string &filename, PLYData &data) { // Read header and reserve memory data.clear(); @@ -367,7 +358,9 @@ namespace plycpp if (line != "ply") { - throw Exception("Missing magic number ""ply"""); + throw Exception("Missing magic number " + "ply" + ""); } while (line != "end_header") @@ -387,7 +380,7 @@ namespace plycpp if (lineContent.size() == 3 && lineContent[0] == "element") { // New element - const std::string& name = lineContent[1]; + const std::string &name = lineContent[1]; const size_t count = strtol_except(lineContent[2]); currentElement.reset(new ElementArray(count)); @@ -401,7 +394,7 @@ namespace plycpp // New property const std::type_index dataType = parseDataType(lineContent[1]); - const std::string& name = lineContent[2]; + const std::string &name = lineContent[2]; std::shared_ptr newProperty(new PropertyArray(dataType, currentElement->size())); currentElement->properties.push_back(name, newProperty); @@ -413,7 +406,7 @@ namespace plycpp const std::type_index indexCountType = parseDataType(lineContent[2]); const std::type_index dataType = parseDataType(lineContent[3]); - const std::string& name = lineContent[4]; + const std::string &name = lineContent[4]; if (indexCountType != UCHAR) throw Exception("Only uchar is supported as counting type for lists"); @@ -421,7 +414,6 @@ namespace plycpp std::shared_ptr newProperty(new PropertyArray(dataType, 3 * currentElement->size(), true)); currentElement->properties.push_back(name, newProperty); } - } if (fin.fail()) @@ -429,7 +421,6 @@ namespace plycpp throw Exception("Issue while parsing header"); } - ///////////////////////////////////// // Read data if (format == "ascii") @@ -447,9 +438,7 @@ namespace plycpp 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")) + if ((isBigEndianArchitecture_ && format != "binary_big_endian") || (!isBigEndianArchitecture_ && format != "binary_little_endian")) throw Exception("Endianness conversion is not supported yet"); readDataContent(fin, data); @@ -469,43 +458,42 @@ namespace plycpp } } - - template - void writeDataContent(std::ofstream& fout, const PLYData& data) + 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) + std::unordered_map readingPlace; + for (auto &elementTuple : data) { - auto& element = elementTuple.data; - for (auto& propertyTuple : element->properties) + auto &element = elementTuple.data; + for (auto &propertyTuple : element->properties) { - auto& prop = propertyTuple.data; + auto &prop = propertyTuple.data; readingPlace[prop.get()] = prop->data.data(); } } //// Iterate over elements array - for (auto& elementArrayTuple : data) + for (auto &elementArrayTuple : data) { - auto& elementArray = 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) + for (auto &propertyTuple : elementArray->properties) { - auto& prop = propertyTuple.data; + auto &prop = propertyTuple.data; // Write data - auto& ptData = readingPlace[prop.get()]; + 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); + fout.write(reinterpret_cast(ptData), prop->stepSize); else { writeASCIIValue(fout, ptData, prop->type); @@ -519,13 +507,13 @@ namespace plycpp { const unsigned char count = 3; // Write the number of elements - fout.write(reinterpret_cast(&count), sizeof(unsigned char)); + 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); + fout.write(reinterpret_cast(ptData), chunckSize); ptData += chunckSize; } else @@ -541,10 +529,7 @@ namespace plycpp fout << " "; ptData += prop->stepSize; } - } - - } if (format == FileFormat::ASCII) { @@ -554,7 +539,7 @@ namespace plycpp } } - void save(const std::string& filename, const PLYData& data, const FileFormat format) + void save(const std::string &filename, const PLYData &data, const FileFormat format) { std::ofstream fout(filename, std::ios::binary); @@ -577,24 +562,24 @@ namespace plycpp } // Iterate over elements array - for (const auto& elementArrayTuple : data) + for (const auto &elementArrayTuple : data) { - const auto& elementArrayName = elementArrayTuple.key; - auto& elementArray = 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) + for (const auto &propertyTuple : elementArray->properties) { - auto& propName = propertyTuple.key; - auto& prop = propertyTuple.data; + 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); + const auto &itTypeName = dataTypeToStr.find(prop->type); if (itTypeName == dataTypeToStr.end()) throw Exception("Should not happen"); @@ -616,7 +601,6 @@ namespace plycpp fout << "property list uchar " << itTypeName->second << " " << propName << std::endl; } - } } fout << "end_header" << std::endl; @@ -635,10 +619,23 @@ namespace plycpp 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; + 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); + } } \ No newline at end of file From 4e4ba09a6556251900400ceab2677cec28bb8ec9 Mon Sep 17 00:00:00 2001 From: Jad Date: Mon, 6 Mar 2023 13:20:35 +0200 Subject: [PATCH 6/6] fixed pr --- include/plycpp/plycpp.h | 618 ++++++++++----------- src/example.cpp | 287 +++++----- src/plycpp.cpp | 1126 ++++++++++++++++++--------------------- 3 files changed, 937 insertions(+), 1094 deletions(-) diff --git a/include/plycpp/plycpp.h b/include/plycpp/plycpp.h index d8bbf82..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,332 +22,296 @@ #pragma once -#include -#include -#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); - - /// 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); - } - -} \ 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/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 d3e2c3d..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,620 +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"); - } - } - void savePLYFile(const std::string &filename, pcl::PointCloud::Ptr cloud, const FileFormat format) - { - plycpp::PLYData data; - typedef std::vector> Cloud; - Cloud points; - 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); - } -} \ 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