diff --git a/cpp/open3d/geometry/Octree.cpp b/cpp/open3d/geometry/Octree.cpp index 23efcb06e81..f2d6652e8e2 100644 --- a/cpp/open3d/geometry/Octree.cpp +++ b/cpp/open3d/geometry/Octree.cpp @@ -49,6 +49,35 @@ std::shared_ptr OctreeNode::ConstructFromJsonValue( return node; } +std::shared_ptr OctreeNode::ConstructFromBinaryStream( + const std::string& in, size_t& offset) { + if (in.size() < offset + 18) return nullptr; + + std::shared_ptr node = nullptr; + if (std::string(&in[offset], 18) == "OctreeInternalNode") { + node = std::make_shared(); + } else if (std::string(&in[offset], 23) == "OctreeInternalPointNode") { + node = std::make_shared(); + } else if (std::string(&in[offset], 19) == "OctreeColorLeafNode") { + node = std::make_shared(); + } else if (std::string(&in[offset], 24) == "OctreePointColorLeafNode") { + node = std::make_shared(); + } else { + utility::LogError("Unhandled class id {}", + std::string(&in[offset], 24)); + } + + // Convert from binary + if (node != nullptr) { + bool deserialization_success = + node->DeserializeFromBinaryStream(in, offset); + if (!deserialization_success) { + node = nullptr; + } + } + return node; +} + std::shared_ptr OctreeInternalNode::GetInsertionNodeInfo( const std::shared_ptr& node_info, const Eigen::Vector3d& point) { @@ -120,6 +149,46 @@ bool OctreeInternalNode::ConvertFromJsonValue(const Json::Value& value) { return rc; } +bool OctreeInternalNode::SerializeToBinaryStream(std::string& out) const { + bool rc = true; + // Write class identifier + const char class_id[] = "OctreeInternalNode"; + out.append(class_id, sizeof(class_id)); + // Write children + for (int cid = 0; cid < 8; ++cid) { + uint8_t has_child = (children_[cid] != nullptr); + out.append(reinterpret_cast(&has_child), + sizeof(has_child)); + if (has_child) { + rc = rc && children_[cid]->SerializeToBinaryStream(out); + } + } + return rc; +} + +bool OctreeInternalNode::DeserializeFromBinaryStream(const std::string& in, + size_t& offset) { + // Read and check class identifier + if (in.size() < offset + 19) return false; + if (std::string(&in[offset], 18) != "OctreeInternalNode") return false; + offset += 19; + // Read children + for (int cid = 0; cid < 8; ++cid) { + if (in.size() < offset + sizeof(uint8_t)) return false; + uint8_t has_child = *reinterpret_cast(&in[offset]); + offset += sizeof(uint8_t); + if (has_child) { + children_[cid] = OctreeNode::ConstructFromBinaryStream(in, offset); + if (!children_[cid]) { + return false; + } + } else { + children_[cid] = nullptr; + } + } + return true; +} + std::function()> OctreeInternalPointNode::GetInitFunction() { return []() -> std::shared_ptr { @@ -190,6 +259,68 @@ bool OctreeInternalPointNode::ConvertFromJsonValue(const Json::Value& value) { return rc; } +bool OctreeInternalPointNode::SerializeToBinaryStream(std::string& out) const { + bool rc = true; + // Write class identifier + const char class_id[] = "OctreeInternalPointNode"; + out.append(class_id, sizeof(class_id)); + // Write children + for (int cid = 0; cid < 8; ++cid) { + uint8_t has_child = (children_[cid] != nullptr); + out.append(reinterpret_cast(&has_child), + sizeof(has_child)); + if (has_child) { + rc = rc && children_[cid]->SerializeToBinaryStream(out); + } + } + // Write indices + uint64_t num_indices = indices_.size(); + out.append(reinterpret_cast(&num_indices), + sizeof(num_indices)); + for (uint64_t idx : indices_) { + out.append(reinterpret_cast(&idx), sizeof(idx)); + } + return rc; +} + +bool OctreeInternalPointNode::DeserializeFromBinaryStream(const std::string& in, + size_t& offset) { + // Read and check class identifier + if (in.size() < offset + 24) return false; + if (std::string(&in[offset], 23) != "OctreeInternalPointNode") return false; + offset += 24; + + // Read children + for (int cid = 0; cid < 8; ++cid) { + if (in.size() < offset + sizeof(uint8_t)) return false; + uint8_t has_child = *reinterpret_cast(&in[offset]); + offset += sizeof(uint8_t); + if (has_child) { + children_[cid] = OctreeNode::ConstructFromBinaryStream(in, offset); + if (!children_[cid]) { + return false; + } + } else { + children_[cid] = nullptr; + } + } + + // Read indices size + if (in.size() < offset + sizeof(uint64_t)) return false; + uint64_t indices_size = *reinterpret_cast(&in[offset]); + offset += sizeof(uint64_t); + // Read indices + indices_.clear(); + for (uint64_t i = 0; i < indices_size; ++i) { + if (in.size() < offset + sizeof(uint64_t)) return false; + uint64_t idx = *reinterpret_cast(&in[offset]); + offset += sizeof(uint64_t); + indices_.push_back(idx); + } + + return true; +} + std::function()> OctreeColorLeafNode::GetInitFunction() { return []() -> std::shared_ptr { @@ -247,6 +378,29 @@ bool OctreeColorLeafNode::ConvertFromJsonValue(const Json::Value& value) { return EigenVector3dFromJsonArray(color_, value["color"]); } +bool OctreeColorLeafNode::SerializeToBinaryStream(std::string& out) const { + // Write class identifier + const char class_id[] = "OctreeColorLeafNode"; + out.append(class_id, sizeof(class_id)); + // Write color_ (Eigen::Vector3d) + out.append(reinterpret_cast(color_.data()), + sizeof(double) * 3); + return true; +} + +bool OctreeColorLeafNode::DeserializeFromBinaryStream(const std::string& in, + size_t& offset) { + // Read and check class identifier + if (in.size() < offset + 20) return false; + if (std::string(&in[offset], 19) != "OctreeColorLeafNode") return false; + offset += 20; + // Read color_ (Eigen::Vector3d) + if (in.size() < offset + sizeof(double) * 3) return false; + std::memcpy(color_.data(), &in[offset], sizeof(double) * 3); + offset += sizeof(double) * 3; + return true; +} + std::function()> OctreePointColorLeafNode::GetInitFunction() { return []() -> std::shared_ptr { @@ -322,6 +476,52 @@ bool OctreePointColorLeafNode::ConvertFromJsonValue(const Json::Value& value) { return true; } +bool OctreePointColorLeafNode::SerializeToBinaryStream(std::string& out) const { + bool rc = true; + // Write class identifier + const char class_id[] = "OctreePointColorLeafNode"; + out.append(class_id, sizeof(class_id)); + // Write color_ (Eigen::Vector3d) + out.append(reinterpret_cast(color_.data()), + sizeof(double) * 3); + // Write indices + uint64_t num_indices = indices_.size(); + out.append(reinterpret_cast(&num_indices), + sizeof(num_indices)); + for (uint64_t idx : indices_) { + out.append(reinterpret_cast(&idx), sizeof(idx)); + } + return rc; +} + +bool OctreePointColorLeafNode::DeserializeFromBinaryStream( + const std::string& in, size_t& offset) { + // Read and check class identifier + if (in.size() < offset + 25) return false; + if (std::string(&in[offset], 24) != "OctreePointColorLeafNode") + return false; + offset += 25; + // Read color_ (Eigen::Vector3d) + if (in.size() < offset + sizeof(double) * 3) return false; + std::memcpy(color_.data(), &in[offset], sizeof(double) * 3); + offset += sizeof(double) * 3; + + // Read indices size + if (in.size() < offset + sizeof(uint64_t)) return false; + uint64_t indices_size = *reinterpret_cast(&in[offset]); + offset += sizeof(uint64_t); + // Read indices + indices_.clear(); + for (uint64_t i = 0; i < indices_size; ++i) { + if (in.size() < offset + sizeof(uint64_t)) return false; + uint64_t idx = *reinterpret_cast(&in[offset]); + offset += sizeof(uint64_t); + indices_.push_back(idx); + } + + return true; +} + Octree::Octree(const Octree& src_octree) : Geometry3D(Geometry::GeometryType::Octree), origin_(src_octree.origin_), @@ -786,6 +986,41 @@ bool Octree::ConvertFromJsonValue(const Json::Value& value) { return rc; } +bool Octree::SerializeToBinaryStream(std::string& out) const { + bool rc = true; + // Serialize the basic attributes + out.clear(); + out.append(reinterpret_cast(origin_.data()), + sizeof(double) * 3); + out.append(reinterpret_cast(&size_), sizeof(double)); + out.append(reinterpret_cast(&max_depth_), sizeof(uint64_t)); + // Serialize the root node + if (root_node_) { + rc = rc && root_node_->SerializeToBinaryStream(out); + } + return rc; +} + +bool Octree::DeserializeFromBinaryStream(const std::string& in) { + size_t offset = 0; + // Deserialize the basic attributes + if (in.size() < offset + sizeof(double) * 3) return false; + std::memcpy(origin_.data(), &in[offset], sizeof(double) * 3); + offset += sizeof(double) * 3; + + if (in.size() < offset + sizeof(size_)) return false; + std::memcpy(&size_, &in[offset], sizeof(size_)); + offset += sizeof(double); + + if (in.size() < offset + sizeof(uint64_t)) return false; + std::memcpy(&max_depth_, &in[offset], sizeof(uint64_t)); + offset += sizeof(uint64_t); + + // Deserialize the root node + root_node_ = OctreeNode::ConstructFromBinaryStream(in, offset); + return true; +} + void Octree::CreateFromVoxelGrid(const geometry::VoxelGrid& voxel_grid) { origin_ = voxel_grid.origin_; size_ = (voxel_grid.GetMaxBound() - origin_).maxCoeff(); diff --git a/cpp/open3d/geometry/Octree.h b/cpp/open3d/geometry/Octree.h index 5b3b6d90c52..3d26d769df4 100644 --- a/cpp/open3d/geometry/Octree.h +++ b/cpp/open3d/geometry/Octree.h @@ -9,6 +9,7 @@ #include #include +#include #include #include "open3d/geometry/Geometry3D.h" @@ -77,6 +78,14 @@ class OctreeNode : public utility::IJsonConvertible { /// Factory function to construct an OctreeNode by parsing the json value. static std::shared_ptr ConstructFromJsonValue( const Json::Value& value); + /// Factory function to construct an OctreeNode by reading from a binary + /// stream. + static std::shared_ptr ConstructFromBinaryStream( + const std::string& in, size_t& offset); + + virtual bool SerializeToBinaryStream(std::string& out) const = 0; + virtual bool DeserializeFromBinaryStream(const std::string& in, + size_t& offset) = 0; }; /// \class OctreeInternalNode @@ -121,6 +130,8 @@ class OctreeInternalNode : public OctreeNode { bool ConvertToJsonValue(Json::Value& value) const override; bool ConvertFromJsonValue(const Json::Value& value) override; + bool SerializeToBinaryStream(std::string& out) const; + bool DeserializeFromBinaryStream(const std::string& in, size_t& offset); public: /// Use vector instead of C-array for Pybind11, otherwise, need to define @@ -156,6 +167,8 @@ class OctreeInternalPointNode : public OctreeInternalNode { bool ConvertToJsonValue(Json::Value& value) const override; bool ConvertFromJsonValue(const Json::Value& value) override; + bool SerializeToBinaryStream(std::string& out) const; + bool DeserializeFromBinaryStream(const std::string& in, size_t& offset); public: /// Indices of points associated with any children of this node @@ -199,6 +212,8 @@ class OctreeColorLeafNode : public OctreeLeafNode { bool ConvertToJsonValue(Json::Value& value) const override; bool ConvertFromJsonValue(const Json::Value& value) override; + bool SerializeToBinaryStream(std::string& out) const; + bool DeserializeFromBinaryStream(const std::string& in, size_t& offset); /// TODO: flexible data, with lambda function for handling node /// Color of the node. Eigen::Vector3d color_ = Eigen::Vector3d(0, 0, 0); @@ -233,6 +248,8 @@ class OctreePointColorLeafNode : public OctreeColorLeafNode { bool ConvertToJsonValue(Json::Value& value) const override; bool ConvertFromJsonValue(const Json::Value& value) override; + bool SerializeToBinaryStream(std::string& out) const; + bool DeserializeFromBinaryStream(const std::string& in, size_t& offset); /// Associated point indices with this node. std::vector indices_; @@ -301,6 +318,8 @@ class Octree : public Geometry3D, public utility::IJsonConvertible { const Eigen::Vector3d& center) override; bool ConvertToJsonValue(Json::Value& value) const override; bool ConvertFromJsonValue(const Json::Value& value) override; + bool SerializeToBinaryStream(std::string& out) const; + bool DeserializeFromBinaryStream(const std::string& in); public: /// \brief Convert octree from point cloud. diff --git a/cpp/open3d/io/OctreeIO.cpp b/cpp/open3d/io/OctreeIO.cpp index ae43c3724b2..585a3e5a58a 100644 --- a/cpp/open3d/io/OctreeIO.cpp +++ b/cpp/open3d/io/OctreeIO.cpp @@ -21,6 +21,7 @@ static const std::unordered_map< std::function> file_extension_to_octree_read_function{ {"json", ReadOctreeFromJson}, + {"bin", ReadOctreeFromBIN}, }; static const std::unordered_map< @@ -28,6 +29,7 @@ static const std::unordered_map< std::function> file_extension_to_octree_write_function{ {"json", WriteOctreeToJson}, + {"bin", WriteOctreeToBIN}, }; std::shared_ptr CreateOctreeFromFile( @@ -90,5 +92,22 @@ bool WriteOctreeToJson(const std::string &filename, const geometry::Octree &octree) { return WriteIJsonConvertibleToJSON(filename, octree); } + +bool ReadOctreeFromBIN(const std::string &filename, geometry::Octree &octree) { + std::string bin_data; + if (!ReadOctreeBinaryStreamFromBIN(filename, bin_data)) { + return false; + } + // Deserialize bin_data into octree + octree.DeserializeFromBinaryStream(bin_data); + return true; +} + +bool WriteOctreeToBIN(const std::string &filename, + const geometry::Octree &octree) { + std::string bin_data; + octree.SerializeToBinaryStream(bin_data); + return WriteOctreeBinaryStreamToBIN(filename, bin_data); +} } // namespace io } // namespace open3d diff --git a/cpp/open3d/io/OctreeIO.h b/cpp/open3d/io/OctreeIO.h index 57ee3b3f400..80605f3dda7 100644 --- a/cpp/open3d/io/OctreeIO.h +++ b/cpp/open3d/io/OctreeIO.h @@ -38,5 +38,16 @@ bool ReadOctreeFromJson(const std::string &filename, geometry::Octree &octree); bool WriteOctreeToJson(const std::string &filename, const geometry::Octree &octree); +bool ReadOctreeFromBIN(const std::string &filename, geometry::Octree &octree); + +bool WriteOctreeToBIN(const std::string &filename, + const geometry::Octree &octree); + +bool ReadOctreeBinaryStreamFromBIN(const std::string &filename, + std::string &bin_data); + +bool WriteOctreeBinaryStreamToBIN(const std::string &filename, + const std::string &bin_data); + } // namespace io } // namespace open3d diff --git a/cpp/open3d/io/file_format/FileBIN.cpp b/cpp/open3d/io/file_format/FileBIN.cpp index 27ef362f13b..27c135d5aa4 100644 --- a/cpp/open3d/io/file_format/FileBIN.cpp +++ b/cpp/open3d/io/file_format/FileBIN.cpp @@ -9,6 +9,7 @@ #include #include "open3d/io/FeatureIO.h" +#include "open3d/io/OctreeIO.h" #include "open3d/utility/FileSystem.h" #include "open3d/utility/Logging.h" @@ -83,5 +84,43 @@ bool WriteFeatureToBIN(const std::string &filename, return success; } +bool ReadOctreeBinaryStreamFromBIN(const std::string &filename, + std::string &bin_data) { + std::ifstream file(filename, std::ios::binary); + if (!file.is_open()) { + utility::LogWarning("Read BIN failed: unable to open file: {}", + filename); + return false; + } + + // Read file contents into bin_data + bin_data.assign((std::istreambuf_iterator(file)), + std::istreambuf_iterator()); + + file.close(); + return true; +} + +bool WriteOctreeBinaryStreamToBIN(const std::string &filename, + const std::string &bin_data) { + std::ofstream file(filename, std::ios::binary); + if (!file.is_open()) { + utility::LogWarning("Write BIN failed: unable to open file: {}", + filename); + return false; + } + + file.write(bin_data.data(), bin_data.size()); + if (!file) { + utility::LogWarning("Write BIN failed: error writing to file: {}", + filename); + file.close(); + return false; + } + + file.close(); + return true; +} + } // namespace io } // namespace open3d diff --git a/cpp/pybind/t/geometry/pointcloud.cpp b/cpp/pybind/t/geometry/pointcloud.cpp index f872b9fcd68..b343debe08c 100644 --- a/cpp/pybind/t/geometry/pointcloud.cpp +++ b/cpp/pybind/t/geometry/pointcloud.cpp @@ -261,9 +261,10 @@ void pybind_pointcloud_definitions(py::module& m) { "non-negative number less than number of points in the " "input pointcloud.", "start_index"_a = 0); - pointcloud.def("remove_radius_outliers", &PointCloud::RemoveRadiusOutliers, - "nb_points"_a, "search_radius"_a, - R"(Remove points that have less than nb_points neighbors in a + pointcloud.def( + "remove_radius_outliers", &PointCloud::RemoveRadiusOutliers, + "nb_points"_a, "search_radius"_a, + R"(Remove points that have less than nb_points neighbors in a sphere of a given search radius. Args: diff --git a/cpp/tests/geometry/Octree.cpp b/cpp/tests/geometry/Octree.cpp index 9ed6f4cf33f..e25da60f75f 100644 --- a/cpp/tests/geometry/Octree.cpp +++ b/cpp/tests/geometry/Octree.cpp @@ -406,5 +406,24 @@ TEST(Octree, ConvertToJsonValue) { EXPECT_TRUE(src_octree == dst_octree); } +TEST(Octree, ConvertFromBinaryStream) { + geometry::PointCloud pcd; + data::PLYPointCloud pointcloud_ply; + io::ReadPointCloud(pointcloud_ply.GetPath(), pcd); + size_t max_depth = 5; + geometry::Octree src_octree(max_depth); + src_octree.ConvertFromPointCloud(pcd, 0.01); + + // Serialize to binary + std::string binary_data; + src_octree.SerializeToBinaryStream(binary_data); + + // Deserialize from binary + geometry::Octree dst_octree; + dst_octree.DeserializeFromBinaryStream(binary_data); + + EXPECT_TRUE(src_octree == dst_octree); +} + } // namespace tests } // namespace open3d diff --git a/cpp/tests/io/OctreeIO.cpp b/cpp/tests/io/OctreeIO.cpp index b7b7b083cf9..4e3735e91f6 100644 --- a/cpp/tests/io/OctreeIO.cpp +++ b/cpp/tests/io/OctreeIO.cpp @@ -22,15 +22,16 @@ namespace open3d { namespace tests { -void WriteReadAndAssertEqual(const geometry::Octree& src_octree) { +void WriteReadAndAssertEqual(const geometry::Octree& src_octree, + const std::string& file_name) { // Write to file - std::string file_name = - utility::filesystem::GetTempDirectoryPath() + "/temp_octree.json"; - EXPECT_TRUE(io::WriteOctree(file_name, src_octree)); + std::string full_file_name = + utility::filesystem::GetTempDirectoryPath() + file_name; + EXPECT_TRUE(io::WriteOctree(full_file_name, src_octree)); // Read from file geometry::Octree dst_octree; - EXPECT_TRUE(io::ReadOctree(file_name, dst_octree)); + EXPECT_TRUE(io::ReadOctree(full_file_name, dst_octree)); EXPECT_TRUE(src_octree == dst_octree); } @@ -39,7 +40,8 @@ TEST(OctreeIO, EmptyTree) { ExpectEQ(octree.origin_, Eigen::Vector3d(0, 0, 0)); EXPECT_EQ(octree.size_, 0); - WriteReadAndAssertEqual(octree); + WriteReadAndAssertEqual(octree, "/temp_octree.json"); + WriteReadAndAssertEqual(octree, "/temp_octree.bin"); } TEST(OctreeIO, ZeroDepth) { @@ -49,10 +51,11 @@ TEST(OctreeIO, ZeroDepth) { octree.InsertPoint(point, geometry::OctreeColorLeafNode::GetInitFunction(), geometry::OctreeColorLeafNode::GetUpdateFunction(color)); - WriteReadAndAssertEqual(octree); + WriteReadAndAssertEqual(octree, "/temp_octree.json"); + WriteReadAndAssertEqual(octree, "/temp_octree.bin"); } -TEST(OctreeIO, JsonFileIOFragment) { +TEST(OctreeIO, FileIOFragment) { // Create octree geometry::PointCloud pcd; data::PLYPointCloud pointcloud_ply; @@ -61,10 +64,11 @@ TEST(OctreeIO, JsonFileIOFragment) { geometry::Octree octree(max_depth); octree.ConvertFromPointCloud(pcd, 0.01); - WriteReadAndAssertEqual(octree); + WriteReadAndAssertEqual(octree, "/temp_octree.json"); + WriteReadAndAssertEqual(octree, "/temp_octree.bin"); } -TEST(OctreeIO, JsonFileIOSevenCubes) { +TEST(OctreeIO, FileIOSevenCubes) { // Build octree std::vector points{ Eigen::Vector3d(0.5, 0.5, 0.5), Eigen::Vector3d(1.5, 0.5, 0.5), @@ -83,7 +87,8 @@ TEST(OctreeIO, JsonFileIOSevenCubes) { geometry::OctreeColorLeafNode::GetUpdateFunction(colors[i])); } - WriteReadAndAssertEqual(octree); + WriteReadAndAssertEqual(octree, "/temp_octree.json"); + WriteReadAndAssertEqual(octree, "/temp_octree.bin"); } } // namespace tests