From 7d44266516ac9e1f86e901655ded6b5ea084d129 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Sat, 29 Jun 2024 13:47:24 -0700 Subject: [PATCH] fixed PointCloud::ExportPly --- src/ply.cpp | 165 ++++++++++++++++++++++++++++++--------------- src/ply.h | 2 + src/pointcloud.cpp | 64 ++++++++++-------- 3 files changed, 148 insertions(+), 83 deletions(-) diff --git a/src/ply.cpp b/src/ply.cpp index 5a38685..09a2c9b 100644 --- a/src/ply.cpp +++ b/src/ply.cpp @@ -18,7 +18,6 @@ #include "core/log.h" - static bool CheckLine(std::ifstream& plyFile, const std::string& validLine) { std::string line; @@ -38,11 +37,105 @@ static bool GetNextPlyLine(std::ifstream& plyFile, std::string& lineOut) return false; } +static const char* BinaryAttributeTypeToString(BinaryAttribute::Type type) +{ + switch (type) + { + case BinaryAttribute::Type::Char: + return "char"; + case BinaryAttribute::Type::UChar: + return "uchar"; + case BinaryAttribute::Type::Short: + return "short"; + case BinaryAttribute::Type::UShort: + return "ushort"; + case BinaryAttribute::Type::Int: + return "int"; + case BinaryAttribute::Type::UInt: + return "uint"; + case BinaryAttribute::Type::Float: + return "float"; + case BinaryAttribute::Type::Double: + return "double"; + default: + assert(false); // bad attribute type + return "unknown"; + }; +} + Ply::Ply() : vertexCount(0), vertexSize(0) { ; } +bool Ply::Parse(std::ifstream& plyFile) +{ + if (!ParseHeader(plyFile)) + { + return false; + } + + // read rest of file into data ptr + { + ZoneScopedNC("Ply::Parse() read data", tracy::Color::Yellow); + AllocData(vertexCount); + plyFile.read((char*)data.get(), vertexSize * vertexCount); + } + + return true; +} + +void Ply::Dump(std::ofstream& plyFile) const +{ + DumpHeader(plyFile); + plyFile.write((char*)data.get(), vertexSize * vertexCount); +} + +bool Ply::GetProperty(const std::string& key, BinaryAttribute& binaryAttributeOut) const +{ + auto iter = propertyMap.find(key); + if (iter != propertyMap.end()) + { + binaryAttributeOut = iter->second; + return true; + } + return false; +} + +void Ply::AddProperty(const std::string& key, BinaryAttribute::Type type) +{ + using PropInfoPair = std::pair; + BinaryAttribute attrib(type, vertexSize); + propertyMap.emplace(PropInfoPair(key, attrib)); + vertexSize += attrib.size; +} + +void Ply::AllocData(size_t numVertices) +{ + vertexCount = numVertices; + data.reset(new uint8_t[vertexSize * numVertices]); +} + +void Ply::ForEachVertex(const VertexCallback& cb) const +{ + const uint8_t* ptr = data.get(); + for (size_t i = 0; i < vertexCount; i++) + { + cb(ptr, vertexSize); + ptr += vertexSize; + } +} + +void Ply::ForEachVertexMut(const VertexCallbackMut& cb) +{ + uint8_t* ptr = data.get(); + for (size_t i = 0; i < vertexCount; i++) + { + cb(ptr, vertexSize); + ptr += vertexSize; + } +} + bool Ply::ParseHeader(std::ifstream& plyFile) { ZoneScopedNC("Ply::ParseHeader", tracy::Color::Green); @@ -159,65 +252,29 @@ bool Ply::ParseHeader(std::ifstream& plyFile) return true; } -bool Ply::Parse(std::ifstream& plyFile) +void Ply::DumpHeader(std::ofstream& plyFile) const { - if (!ParseHeader(plyFile)) - { - return false; - } + // ply files have unix line endings. + plyFile << "ply\n"; + plyFile << "format binary_little_endian 1.0\n"; + plyFile << "element vertex " << vertexCount << "\n"; - // read rest of file into data ptr - { - ZoneScopedNC("Ply::Parse() read data", tracy::Color::Yellow); - AllocData(vertexCount); - plyFile.read((char*)data.get(), vertexSize * vertexCount); - } - - return true; -} - -bool Ply::GetProperty(const std::string& key, BinaryAttribute& binaryAttributeOut) const -{ - auto iter = propertyMap.find(key); - if (iter != propertyMap.end()) - { - binaryAttributeOut = iter->second; - return true; - } - return false; -} - -void Ply::AddProperty(const std::string& key, BinaryAttribute::Type type) -{ + // sort properties by offset using PropInfoPair = std::pair; - BinaryAttribute attrib(type, vertexSize); - propertyMap.emplace(PropInfoPair(key, attrib)); - vertexSize += attrib.size; -} - -void Ply::AllocData(size_t numVertices) -{ - data.reset(new uint8_t[vertexSize * numVertices]); -} - -void Ply::ForEachVertex(const VertexCallback& cb) const -{ - const uint8_t* ptr = data.get(); - for (size_t i = 0; i < vertexCount; i++) + std::vector propVec; + propVec.reserve(propertyMap.size()); + for (auto& pair : propertyMap) { - cb(ptr, vertexSize); - ptr += vertexSize; + propVec.push_back(pair); } -} + std::sort(propVec.begin(), propVec.end(), [](const PropInfoPair& a, const PropInfoPair& b) + { + return a.second.offset < b.second.offset; + }); -void Ply::ForEachVertexMut(const VertexCallbackMut& cb) -{ - uint8_t* ptr = data.get(); - for (size_t i = 0; i < vertexCount; i++) + for (auto& pair : propVec) { - cb(ptr, vertexSize); - ptr += vertexSize; + plyFile << "property " << BinaryAttributeTypeToString(pair.second.type) << " " << pair.first << "\n"; } + plyFile << "end_header\n"; } - - diff --git a/src/ply.h b/src/ply.h index b34583e..d94ee5c 100644 --- a/src/ply.h +++ b/src/ply.h @@ -21,6 +21,7 @@ class Ply public: Ply(); bool Parse(std::ifstream& plyFile); + void Dump(std::ofstream& plyFile) const; bool GetProperty(const std::string& key, BinaryAttribute& attributeOut) const; void AddProperty(const std::string& key, BinaryAttribute::Type type); @@ -36,6 +37,7 @@ class Ply protected: bool ParseHeader(std::ifstream& plyFile); + void DumpHeader(std::ofstream& plyFile) const; std::unordered_map propertyMap; std::unique_ptr data; diff --git a/src/pointcloud.cpp b/src/pointcloud.cpp index ca4aa29..1df4c1a 100644 --- a/src/pointcloud.cpp +++ b/src/pointcloud.cpp @@ -132,9 +132,6 @@ bool PointCloud::ImportPly(const std::string& plyFilename) bool PointCloud::ExportPly(const std::string& plyFilename) const { - // AJT: TODO - return false; - std::ofstream plyFile(plyFilename, std::ios::binary); if (!plyFile.is_open()) { @@ -153,37 +150,46 @@ bool PointCloud::ExportPly(const std::string& plyFilename) const ply.AddProperty("green", BinaryAttribute::Type::UChar); ply.AddProperty("blue", BinaryAttribute::Type::UChar); + struct + { + BinaryAttribute x, y, z; + BinaryAttribute nx, ny, nz; + BinaryAttribute red, green, blue; + } props; + + ply.GetProperty("x", props.x); + ply.GetProperty("y", props.y); + ply.GetProperty("z", props.z); + ply.GetProperty("nx", props.nx); + ply.GetProperty("ny", props.ny); + ply.GetProperty("nz", props.nz); + ply.GetProperty("red", props.red); + ply.GetProperty("green", props.green); + ply.GetProperty("blue", props.blue); + ply.AllocData(numPoints); - ply.ForEachVertexMut([this](void* data, size_t size) + uint8_t* cloudData = (uint8_t*)data.get(); + size_t runningSize = 0; + ply.ForEachVertexMut([this, &props, &cloudData, &runningSize](void* plyData, size_t size) { - // + const float* position = positionAttrib.Get(cloudData); + props.x.Write(plyData, position[0]); + props.y.Write(plyData, position[1]); + props.z.Write(plyData, position[2]); + props.nx.Write(plyData, 0.0f); + props.ny.Write(plyData, 0.0f); + props.nz.Write(plyData, 0.0f); + const float* color = colorAttrib.Get(cloudData); + props.red.Write(plyData, (uint8_t)(color[0] * 255.0f)); + props.green.Write(plyData, (uint8_t)(color[1] * 255.0f)); + props.blue.Write(plyData, (uint8_t)(color[2] * 255.0f)); + cloudData += pointSize; + runningSize += pointSize; + assert(runningSize <= GetTotalSize()); // bad, we went outside of data ptr contents. }); - /* - // ply files have unix line endings. - plyFile << "ply\n"; - plyFile << "format binary_little_endian 1.0\n"; - plyFile << "element vertex " << numPoints << "\n"; - plyFile << "property float x\n"; - plyFile << "property float y\n"; - plyFile << "property float z\n"; - plyFile << "property float nx\n"; - plyFile << "property float ny\n"; - plyFile << "property float nz\n"; - plyFile << "property uchar red\n"; - plyFile << "property uchar green\n"; - plyFile << "property uchar blue\n"; - plyFile << "end_header\n"; - */ - - /* - const size_t POINT_SIZE = 27; - for (auto&& p : pointDataVec) - { - plyFile.write((char*)&p, POINT_SIZE); - } - */ + ply.Dump(plyFile); return true; }