Merge pull request #31 from diffCheckOrg/ktree_impl

Adding API functions for compute distance (mesh-cloud cloud-mesh)

src/diffCheck.hh

@@ -1,6 +1,8 @@
 #pragma once
 
 #include <open3d/Open3D.h>
+#include <open3d/t/geometry/RaycastingScene.h>
+
 #include <loguru.hpp>
 
 // diffCheck includes

src/diffCheck/geometry/DFMesh.cc

@@ -1,6 +1,8 @@
 #include "diffCheck/geometry/DFMesh.hh"
 #include "diffCheck/IOManager.hh"
 
+#include <open3d/t/geometry/RaycastingScene.h>
+
 
 namespace diffCheck::geometry
 {
@@ -89,4 +91,39 @@ namespace diffCheck::geometry
         this->NormalsFace = tempMesh_ptr->NormalsFace;
         this->ColorsFace = tempMesh_ptr->ColorsFace;
     }
+
+    std::vector<float> DFMesh::ComputeDistance(const diffCheck::geometry::DFPointCloud &targetCloud, bool useAbs)
+    {
+        auto rayCastingScene = open3d::t::geometry::RaycastingScene();
+
+        std::vector<Eigen::Vector3d> vertices = this->Vertices;
+        std::vector<float> verticesPosition;
+        for (const auto& vertex : vertices) {
+            verticesPosition.insert(verticesPosition.end(), vertex.data(), vertex.data() + 3);
+        }
+        open3d::core::Tensor verticesPositionTensor(verticesPosition.data(), {static_cast<int64_t>(vertices.size()), 3}, open3d::core::Dtype::Float32);
+        std::vector<uint32_t> triangles;
+        for (int i = 0; i < this->Faces.size(); i++) {
+            triangles.push_back(static_cast<uint32_t>(this->Faces[i].x()));
+            triangles.push_back(static_cast<uint32_t>(this->Faces[i].y()));
+            triangles.push_back(static_cast<uint32_t>(this->Faces[i].z()));
+        }
+        open3d::core::Tensor trianglesTensor(triangles.data(), {static_cast<int64_t>(this->Faces.size()), 3}, open3d::core::Dtype::UInt32);
+        rayCastingScene.AddTriangles(verticesPositionTensor, trianglesTensor);
+
+        auto pointCloudO3dCopy = targetCloud;
+        std::shared_ptr<open3d::geometry::PointCloud> pointCloudO3d_ptr = pointCloudO3dCopy.Cvt2O3DPointCloud();
+        std::vector<float> cloudPoints;
+        for (const auto& point : pointCloudO3d_ptr->points_) {
+            cloudPoints.insert(cloudPoints.end(), point.data(), point.data() + 3);
+        }
+        open3d::core::Tensor cloudPointsTensor(cloudPoints.data(), {static_cast<int64_t>(pointCloudO3d_ptr->points_.size()), 3}, open3d::core::Dtype::Float32);
+
+        open3d::core::Tensor sdf = rayCastingScene.ComputeSignedDistance(cloudPointsTensor);
+        if (useAbs)
+            sdf = sdf.Abs();
+        std::vector<float> sdfVector(sdf.GetDataPtr<float>(), sdf.GetDataPtr<float>() + sdf.NumElements());
+
+        return sdfVector;
+    }
 } // namespace diffCheck::geometry

src/diffCheck/geometry/DFMesh.hh

@@ -3,8 +3,8 @@
 #include <Eigen/Core>
 #include <open3d/Open3D.h>
 
-#include <diffCheck/geometry/DFPointCloud.hh>
-#include <diffCheck/transformation/DFTransformation.hh>
+#include "diffCheck/geometry/DFPointCloud.hh"
+#include "diffCheck/transformation/DFTransformation.hh"
 
 namespace diffCheck::geometry
 {
@@ -85,6 +85,18 @@ namespace diffCheck::geometry
         /// @brief Number of faces in the mesh
         int GetNumFaces() const { return this->Faces.size(); }
 
+    public:  ///< Distance calculations
+        /**
+         * @brief Compute the distance between the df mesh and a df point cloud. It
+         * can be considered as a point to plane distance.
+         * 
+         * @param target the target cloud in format df
+         * @param useAbs if true, the absolute value of the distance is returned
+         * @return std::vector<float> the distance between the point cloud and the mesh
+         */
+        std::vector<float> ComputeDistance(const diffCheck::geometry::DFPointCloud &targetMesh, bool useAbs = true);
+
+
     public:  ///< Basic mesh data
         /// @brief Eigen vector of 3D vertices
         std::vector<Eigen::Vector3d> Vertices;

src/diffCheck/geometry/DFPointCloud.cc

@@ -2,7 +2,6 @@
 
 #include "diffCheck/IOManager.hh"
 
-
 namespace diffCheck::geometry
 {
     void DFPointCloud::Cvt2DFPointCloud(const std::shared_ptr<open3d::geometry::PointCloud> &O3DPointCloud)
@@ -33,18 +32,6 @@ namespace diffCheck::geometry
         return O3DPointCloud;
     }
 
-    std::vector<double> DFPointCloud::ComputeP2PDistance(std::shared_ptr<geometry::DFPointCloud> target)
-    {
-        std::vector<double> errors;
-        auto O3DSourcePointCloud = this->Cvt2O3DPointCloud();
-        auto O3DTargetPointCloud = target->Cvt2O3DPointCloud();
-
-        std::vector<double> distances;
-
-        distances = O3DSourcePointCloud->ComputePointCloudDistance(*O3DTargetPointCloud);
-        return distances;
-    }
-
     std::vector<Eigen::Vector3d> DFPointCloud::ComputeBoundingBox()
     {
         auto O3DPointCloud = this->Cvt2O3DPointCloud();
@@ -132,4 +119,17 @@ namespace diffCheck::geometry
         this->Colors = cloud->Colors;
         this->Normals = cloud->Normals;
     }
+
+    std::vector<double> DFPointCloud::ComputeDistance(const diffCheck::geometry::DFPointCloud &targetCloud, bool useAbs)
+    {
+        auto O3DSourcePointCloud = this->Cvt2O3DPointCloud();
+        auto targetCloudCopy = targetCloud;
+        auto O3DTargetPointCloud = targetCloudCopy.Cvt2O3DPointCloud();
+        std::vector<double> distances;
+        distances = O3DSourcePointCloud->ComputePointCloudDistance(*O3DTargetPointCloud);
+        if (useAbs)
+            for (auto &dist : distances)
+                dist = std::abs(dist);
+        return distances;
+    }
 }

src/diffCheck/geometry/DFPointCloud.hh

@@ -3,7 +3,7 @@
 #include <Eigen/Core>
 #include <open3d/Open3D.h>
 
-#include <diffCheck/transformation/DFTransformation.hh>
+#include "diffCheck/transformation/DFTransformation.hh"
 
 
 namespace diffCheck::geometry
@@ -37,19 +37,6 @@ namespace diffCheck::geometry
         std::shared_ptr<open3d::geometry::PointCloud> Cvt2O3DPointCloud();
 
     public:  ///< Utilities
-        /** 
-        * @brief Compute the "point to point" distance between this and another point clouds. 
-        * 
-        * For every point in the source point cloud, it looks in the KDTree of the target point cloud and finds the closest point.
-        * It returns a vector of distances, one for each point in the source point cloud.
-        * 
-        * @param target The target diffCheck point cloud
-        * @return std::vector<double> A vector of distances, one for each point in the source point cloud.
-        * 
-        * @see https://github.com/isl-org/Open3D/blob/main/cpp/open3d/geometry/PointCloud.cpp
-        */
-        std::vector<double> ComputeP2PDistance(std::shared_ptr<geometry::DFPointCloud> target);
-
         /**
          * @brief Compute the bounding box of the point cloud and stores it as member of the DFPointCloud object
          * 
@@ -122,6 +109,20 @@ namespace diffCheck::geometry
          */
         void LoadFromPLY(const std::string &path);
 
+    public:  ///< Distance calculations
+        /**
+         * @brief Compute the distance between two point clouds.
+         * For every point in the source point cloud, it looks in the KDTree of the target point cloud and finds the closest point.
+         * It returns a vector of distances, one for each point in the source point cloud.
+         * 
+         * @param target the target point cloud in format df
+         * @param useAbs if true, the absolute value of the distance is returned
+         * @return std::vector<double> the distance between the two point clouds
+         * 
+         * @see https://github.com/isl-org/Open3D/blob/main/cpp/open3d/geometry/PointCloud.cpp
+         */
+        std::vector<double> ComputeDistance(const DFPointCloud &targetCloud, bool useAbs = true);
+
     public:  ///< Getters
         /// @brief Number of points in the point cloud
         int GetNumPoints() const { return this->Points.size(); }

src/diffCheck/registrations/DFGlobalRegistrations.cc

@@ -17,7 +17,7 @@ namespace diffCheck::registrations
 
             std::shared_ptr<geometry::DFPointCloud> dfPointCloudPtrAfterTrans = std::make_shared<geometry::DFPointCloud>();
             dfPointCloudPtrAfterTrans->Cvt2DFPointCloud(o3DPointCloudAfterTrans);
-            std::vector<double> registrationErrors = dfPointCloudPtrAfterTrans->ComputeP2PDistance(target);
+            std::vector<double> registrationErrors = dfPointCloudPtrAfterTrans->ComputeDistance(*target);
             errors.push_back(std::accumulate(registrationErrors.begin(), registrationErrors.end(), 0.0) / registrationErrors.size());
         }
         return errors;

src/diffCheckApp.cc

@@ -3,48 +3,101 @@
 #include <iostream>
 #include <fstream>
 
+#include <open3d/Open3D.h>
+// #include <open3d/t/geometry/RaycastingScene.h>
 
 int main()
 {
   // import clouds
-  std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtr 
+  std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtr
       = std::make_shared<diffCheck::geometry::DFPointCloud>();
   std::shared_ptr<diffCheck::geometry::DFMesh> dfMeshPtr 
       = std::make_shared<diffCheck::geometry::DFMesh>();
 
   // create a sphere from o3d
-  std::string pathCloud = R"(C:\andre\Downloads\moved_04.ply)";
+  std::string pathCloud = R"(C:\Users\andre\Downloads\moved_04.ply)";
   std::string pathMesh = R"(C:\Users\andre\Downloads\meshtest.ply)";
 
-  // dfPointCloudPtr->LoadFromPLY(pathCloud);
+  dfPointCloudPtr->LoadFromPLY(pathCloud);
   dfMeshPtr->LoadFromPLY(pathMesh);
 
-  open3d::geometry::TriangleMesh meshO3d = *dfMeshPtr->Cvt2O3DTriangleMesh();
+//   open3d::geometry::TriangleMesh meshO3d = *dfMeshPtr->Cvt2O3DTriangleMesh();
+//   open3d::geometry::PointCloud pointCloudO3d = *dfPointCloudPtr->Cvt2O3DPointCloud();
 
+//   auto rayCastingScene = open3d::t::geometry::RaycastingScene();
+//   // Get the vertices of the mesh
+//   std::vector<Eigen::Vector3d> vertices = meshO3d.vertices_;
 
-  // convert the sphere to a diffCheck point cloud
-  // auto o3dPointCloud = meshO3d.SamplePointsUniformly(1000);
+//   // Convert the vertices to a tensor
+//   std::vector<float> verticesPosition;
+//   for (const auto& vertex : vertices) {
+//       verticesPosition.insert(verticesPosition.end(), vertex.data(), vertex.data() + 3);
+//   }
+//   open3d::core::Tensor verticesPositionTensor(verticesPosition.data(), {static_cast<int64_t>(vertices.size()), 3}, open3d::core::Dtype::Float32);
 
-  std::shared_ptr<open3d::geometry::PointCloud> tightBBOX = std::make_shared<open3d::geometry::PointCloud>();
+//   std::vector<uint32_t> triangles;
+//   for (int i = 0; i < meshO3d.triangles_.size(); i++) {
+//       triangles.push_back(static_cast<uint32_t>(meshO3d.triangles_[i].x()));
+//       triangles.push_back(static_cast<uint32_t>(meshO3d.triangles_[i].y()));
+//       triangles.push_back(static_cast<uint32_t>(meshO3d.triangles_[i].z()));
+//   }
+//   open3d::core::Tensor trianglesTensor(triangles.data(), {static_cast<int64_t>(meshO3d.triangles_.size()), 3}, open3d::core::Dtype::UInt32);
+
+//   rayCastingScene.AddTriangles(verticesPositionTensor, trianglesTensor);
 
-  // compute the bounding box
-  open3d::geometry::OrientedBoundingBox bbox = meshO3d.GetMinimalOrientedBoundingBox();
-  std::vector<Eigen::Vector3d> bboxPts = bbox.GetBoxPoints();
-  for (auto &pt : bboxPts)
-  {
-    tightBBOX->points_.push_back(pt);
-  }
+//   // compute the cloud to mesh signed distance
+//   std::vector<float> cloudPoints;
+//   for (const auto& point : pointCloudO3d.points_) {
+//       cloudPoints.insert(cloudPoints.end(), point.data(), point.data() + 3);
+//   }
+//   open3d::core::Tensor cloudPointsTensor(cloudPoints.data(), {static_cast<int64_t>(pointCloudO3d.points_.size()), 3}, open3d::core::Dtype::Float32);
 
 
-  dfPointCloudPtr->Cvt2DFPointCloud(tightBBOX);
+//   open3d::core::Tensor sdf = rayCastingScene.ComputeSignedDistance(cloudPointsTensor);
 
+//   // FIXME: replace with bool parameter
+//   // if true, get the absolute value of the sdf
+//   if (true) {
+//       sdf = sdf.Abs();
+//   }
 
+  // convert sdf to a vector
+  std::vector<float> sdfVector = dfMeshPtr->ComputeDistance(*dfPointCloudPtr);
 
 
-  std::shared_ptr<diffCheck::visualizer::Visualizer> vis = std::make_shared<diffCheck::visualizer::Visualizer>();
-  vis->AddPointCloud(dfPointCloudPtr);
-  // vis->AddMesh(dfMeshPtr);
-  vis->Run();
+  // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+  // compute point cloud to point cloud distance
+  std::vector<double> distances = dfPointCloudPtr->ComputeDistance(*dfPointCloudPtr);
+//   for (auto &dist : distances)
+//   {
+//     std::cout << dist << std::endl;
+//   }
+
+
+
+  // // convert the sphere to a diffCheck point cloud
+  // // auto o3dPointCloud = meshO3d.SamplePointsUniformly(1000);
+
+  // std::shared_ptr<open3d::geometry::PointCloud> tightBBOX = std::make_shared<open3d::geometry::PointCloud>();
+
+  // // compute the bounding box
+  // open3d::geometry::OrientedBoundingBox bbox = meshO3d.GetMinimalOrientedBoundingBox();
+  // std::vector<Eigen::Vector3d> bboxPts = bbox.GetBoxPoints();
+  // for (auto &pt : bboxPts)
+  // {
+  //   tightBBOX->points_.push_back(pt);
+  // }
+
+
+  // dfPointCloudPtr->Cvt2DFPointCloud(tightBBOX);
+
+
+
+
+  // std::shared_ptr<diffCheck::visualizer::Visualizer> vis = std::make_shared<diffCheck::visualizer::Visualizer>();
+  // vis->AddPointCloud(dfPointCloudPtr);
+  // // vis->AddMesh(dfMeshPtr);
+  // vis->Run();
 
 
   return 0;

src/diffCheckBindings.cc

@@ -29,7 +29,9 @@ PYBIND11_MODULE(diffcheck_bindings, m) {
         .def(py::init<>())
         .def(py::init<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>>())
 
-        .def("compute_P2PDistance", &diffCheck::geometry::DFPointCloud::ComputeP2PDistance)
+        .def("compute_distance", &diffCheck::geometry::DFPointCloud::ComputeDistance,
+            py::arg("target_cloud"),
+            py::arg("is_abs") = true)
         .def("compute_BoundingBox", &diffCheck::geometry::DFPointCloud::ComputeBoundingBox)
 
         .def("voxel_downsample", &diffCheck::geometry::DFPointCloud::VoxelDownsample,
@@ -65,6 +67,10 @@ PYBIND11_MODULE(diffcheck_bindings, m) {
         .def(py::init<>())
         .def(py::init<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3i>, std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>>())
 
+        .def("compute_distance", &diffCheck::geometry::DFMesh::ComputeDistance,
+            py::arg("target_cloud"),
+            py::arg("is_abs") = true)
+
         .def("load_from_PLY", &diffCheck::geometry::DFMesh::LoadFromPLY)
 
         .def("sample_points_uniformly", &diffCheck::geometry::DFMesh::SampleCloudUniform)

src/gh/components/DF_tester/code.py

@@ -25,4 +25,8 @@ def RunScript(self):
         else:
             ghenv.Component.AddRuntimeMessage(RML.Remark, "Bindings imported.")
 
-        return is_binding_imported
+        return is_binding_imported
+
+# if __name__ == "__main__":
+#     tester = DFTester()
+#     tester.RunScript()

src/gh/diffCheck/diffCheck.egg-info/SOURCES.txt

@@ -7,7 +7,6 @@ diffCheck/df_geometries.py
 diffCheck/df_joint_detector.py
 diffCheck/df_transformations.py
 diffCheck/df_util.py
-diffCheck/diffcheck_bindings.cp39-win_amd64.pyd
 diffCheck.egg-info/PKG-INFO
 diffCheck.egg-info/SOURCES.txt
 diffCheck.egg-info/dependency_links.txt