Merge pull request #45 from diffCheckOrg/distance_calculation_visualisation

Distance calculation visualisation

Author: 9and3

.gitignore

@@ -246,4 +246,4 @@ cython_debug/
 #.idea/
 
 # egg-info
-*.egg-info/
+*.egg-info/

deps/eigen

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

deps/pybind11

@@ -1,6 +1,8 @@
 #include "diffCheck/geometry/DFMesh.hh"
 #include "diffCheck/IOManager.hh"
 
+#include <open3d/t/geometry/RaycastingScene.h>
+
 
 namespace diffCheck::geometry
 {
@@ -145,4 +147,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.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
 {
@@ -100,6 +100,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/DFMesh.cc

@@ -116,18 +116,6 @@ namespace diffCheck::geometry
         return cilantroPointCloud;
     }
 
-    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();
@@ -273,6 +261,18 @@ namespace diffCheck::geometry
         this->Normals = cloud->Normals;
     }
 
+    std::vector<double> DFPointCloud::ComputeDistance(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;
+    }
+
     void DFPointCloud::AddPoints(const DFPointCloud &pointCloud)
     {
         this->Points.insert(this->Points.end(), pointCloud.Points.begin(), pointCloud.Points.end());
@@ -290,4 +290,4 @@ namespace diffCheck::geometry
         center /= this->Points.size();
         return center;
     }
-}
+}

src/diffCheck/geometry/DFMesh.hh

@@ -4,7 +4,7 @@
 #include <Eigen/Core>
 #include <open3d/Open3D.h>
 
-#include <diffCheck/transformation/DFTransformation.hh>
+#include "diffCheck/transformation/DFTransformation.hh"
 
 #include <cilantro/utilities/point_cloud.hpp>
 #include <cilantro/core/nearest_neighbors.hpp>
@@ -48,19 +48,6 @@ namespace diffCheck::geometry
         std::shared_ptr<cilantro::PointCloud3f> Cvt2CilantroPointCloud();
 
     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
          * 
@@ -156,6 +143,19 @@ 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
+         * @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(std::shared_ptr<DFPointCloud> target);
+
         /**
          * @brief adds the points, colors and normals from another point cloud
          * 

src/diffCheck/geometry/DFPointCloud.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/diffCheck/geometry/DFPointCloud.hh

@@ -29,7 +29,8 @@ 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"))
         .def("compute_BoundingBox", &diffCheck::geometry::DFPointCloud::ComputeBoundingBox)
 
         .def("voxel_downsample", &diffCheck::geometry::DFPointCloud::VoxelDownsample,
@@ -75,6 +76,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)