Skip to content

Commit fbf6c5f

Browse files
9and39and3
committed
ADD-WIP: working version for compute distance, to be checked with gh
1 parent 8c4a897 commit fbf6c5f

File tree

10 files changed

+882
-243
lines changed

10 files changed

+882
-243
lines changed

‎src/diffCheck/geometry/DFMesh.cc‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
#include "diffCheck/geometry/DFMesh.hh"
22
#include "diffCheck/IOManager.hh"
33

4+
#include <open3d/t/geometry/RaycastingScene.h>
5+
46

57
namespace diffCheck::geometry
68
{
@@ -89,4 +91,39 @@ namespace diffCheck::geometry
8991
this->NormalsFace = tempMesh_ptr->NormalsFace;
9092
this->ColorsFace = tempMesh_ptr->ColorsFace;
9193
}
94+
95+
std::vector<float> DFMesh::ComputeDistance(const diffCheck::geometry::DFPointCloud &targetCloud, bool useAbs)
96+
{
97+
auto rayCastingScene = open3d::t::geometry::RaycastingScene();
98+
99+
std::vector<Eigen::Vector3d> vertices = this->Vertices;
100+
std::vector<float> verticesPosition;
101+
for (const auto& vertex : vertices) {
102+
verticesPosition.insert(verticesPosition.end(), vertex.data(), vertex.data() + 3);
103+
}
104+
open3d::core::Tensor verticesPositionTensor(verticesPosition.data(), {static_cast<int64_t>(vertices.size()), 3}, open3d::core::Dtype::Float32);
105+
std::vector<uint32_t> triangles;
106+
for (int i = 0; i < this->Faces.size(); i++) {
107+
triangles.push_back(static_cast<uint32_t>(this->Faces[i].x()));
108+
triangles.push_back(static_cast<uint32_t>(this->Faces[i].y()));
109+
triangles.push_back(static_cast<uint32_t>(this->Faces[i].z()));
110+
}
111+
open3d::core::Tensor trianglesTensor(triangles.data(), {static_cast<int64_t>(this->Faces.size()), 3}, open3d::core::Dtype::UInt32);
112+
rayCastingScene.AddTriangles(verticesPositionTensor, trianglesTensor);
113+
114+
auto pointCloudO3dCopy = targetCloud;
115+
std::shared_ptr<open3d::geometry::PointCloud> pointCloudO3d_ptr = pointCloudO3dCopy.Cvt2O3DPointCloud();
116+
std::vector<float> cloudPoints;
117+
for (const auto& point : pointCloudO3d_ptr->points_) {
118+
cloudPoints.insert(cloudPoints.end(), point.data(), point.data() + 3);
119+
}
120+
open3d::core::Tensor cloudPointsTensor(cloudPoints.data(), {static_cast<int64_t>(pointCloudO3d_ptr->points_.size()), 3}, open3d::core::Dtype::Float32);
121+
122+
open3d::core::Tensor sdf = rayCastingScene.ComputeSignedDistance(cloudPointsTensor);
123+
if (useAbs)
124+
sdf = sdf.Abs();
125+
std::vector<float> sdfVector(sdf.GetDataPtr<float>(), sdf.GetDataPtr<float>() + sdf.NumElements());
126+
127+
return sdfVector;
128+
}
92129
} // namespace diffCheck::geometry

‎src/diffCheck/geometry/DFMesh.hh‎

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,8 @@
33
#include <Eigen/Core>
44
#include <open3d/Open3D.h>
55

6-
#include <diffCheck/geometry/DFPointCloud.hh>
7-
#include <diffCheck/transformation/DFTransformation.hh>
6+
#include "diffCheck/geometry/DFPointCloud.hh"
7+
#include "diffCheck/transformation/DFTransformation.hh"
88

99
namespace diffCheck::geometry
1010
{
@@ -85,6 +85,18 @@ namespace diffCheck::geometry
8585
/// @brief Number of faces in the mesh
8686
int GetNumFaces() const { return this->Faces.size(); }
8787

88+
public: ///< Distance calculations
89+
/**
90+
* @brief Compute the distance between the df mesh and a df point cloud. It
91+
* can be considered as a point to plane distance.
92+
*
93+
* @param target the target cloud in format df
94+
* @param useAbs if true, the absolute value of the distance is returned
95+
* @return std::vector<float> the distance between the point cloud and the mesh
96+
*/
97+
std::vector<float> ComputeDistance(const diffCheck::geometry::DFPointCloud &targetMesh, bool useAbs = true);
98+
99+
88100
public: ///< Basic mesh data
89101
/// @brief Eigen vector of 3D vertices
90102
std::vector<Eigen::Vector3d> Vertices;

‎src/diffCheck/geometry/DFPointCloud.cc‎

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,6 @@
22

33
#include "diffCheck/IOManager.hh"
44

5-
65
namespace diffCheck::geometry
76
{
87
void DFPointCloud::Cvt2DFPointCloud(const std::shared_ptr<open3d::geometry::PointCloud> &O3DPointCloud)
@@ -33,18 +32,6 @@ namespace diffCheck::geometry
3332
return O3DPointCloud;
3433
}
3534

36-
std::vector<double> DFPointCloud::ComputeP2PDistance(std::shared_ptr<geometry::DFPointCloud> target)
37-
{
38-
std::vector<double> errors;
39-
auto O3DSourcePointCloud = this->Cvt2O3DPointCloud();
40-
auto O3DTargetPointCloud = target->Cvt2O3DPointCloud();
41-
42-
std::vector<double> distances;
43-
44-
distances = O3DSourcePointCloud->ComputePointCloudDistance(*O3DTargetPointCloud);
45-
return distances;
46-
}
47-
4835
std::vector<Eigen::Vector3d> DFPointCloud::ComputeBoundingBox()
4936
{
5037
auto O3DPointCloud = this->Cvt2O3DPointCloud();
@@ -132,4 +119,17 @@ namespace diffCheck::geometry
132119
this->Colors = cloud->Colors;
133120
this->Normals = cloud->Normals;
134121
}
122+
123+
std::vector<double> DFPointCloud::ComputeDistance(const diffCheck::geometry::DFPointCloud &targetCloud, bool useAbs)
124+
{
125+
auto O3DSourcePointCloud = this->Cvt2O3DPointCloud();
126+
auto targetCloudCopy = targetCloud;
127+
auto O3DTargetPointCloud = targetCloudCopy.Cvt2O3DPointCloud();
128+
std::vector<double> distances;
129+
distances = O3DSourcePointCloud->ComputePointCloudDistance(*O3DTargetPointCloud);
130+
if (useAbs)
131+
for (auto &dist : distances)
132+
dist = std::abs(dist);
133+
return distances;
134+
}
135135
}

‎src/diffCheck/geometry/DFPointCloud.hh‎

Lines changed: 15 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
#include <Eigen/Core>
44
#include <open3d/Open3D.h>
55

6-
#include <diffCheck/transformation/DFTransformation.hh>
6+
#include "diffCheck/transformation/DFTransformation.hh"
77

88

99
namespace diffCheck::geometry
@@ -37,19 +37,6 @@ namespace diffCheck::geometry
3737
std::shared_ptr<open3d::geometry::PointCloud> Cvt2O3DPointCloud();
3838

3939
public: ///< Utilities
40-
/**
41-
* @brief Compute the "point to point" distance between this and another point clouds.
42-
*
43-
* For every point in the source point cloud, it looks in the KDTree of the target point cloud and finds the closest point.
44-
* It returns a vector of distances, one for each point in the source point cloud.
45-
*
46-
* @param target The target diffCheck point cloud
47-
* @return std::vector<double> A vector of distances, one for each point in the source point cloud.
48-
*
49-
* @see https://github.com/isl-org/Open3D/blob/main/cpp/open3d/geometry/PointCloud.cpp
50-
*/
51-
std::vector<double> ComputeP2PDistance(std::shared_ptr<geometry::DFPointCloud> target);
52-
5340
/**
5441
* @brief Compute the bounding box of the point cloud and stores it as member of the DFPointCloud object
5542
*
@@ -122,6 +109,20 @@ namespace diffCheck::geometry
122109
*/
123110
void LoadFromPLY(const std::string &path);
124111

112+
public: ///< Distance calculations
113+
/**
114+
* @brief Compute the distance between two point clouds.
115+
* For every point in the source point cloud, it looks in the KDTree of the target point cloud and finds the closest point.
116+
* It returns a vector of distances, one for each point in the source point cloud.
117+
*
118+
* @param target the target point cloud in format df
119+
* @param useAbs if true, the absolute value of the distance is returned
120+
* @return std::vector<double> the distance between the two point clouds
121+
*
122+
* @see https://github.com/isl-org/Open3D/blob/main/cpp/open3d/geometry/PointCloud.cpp
123+
*/
124+
std::vector<double> ComputeDistance(const DFPointCloud &targetCloud, bool useAbs = true);
125+
125126
public: ///< Getters
126127
/// @brief Number of points in the point cloud
127128
int GetNumPoints() const { return this->Points.size(); }

‎src/diffCheck/registrations/DFGlobalRegistrations.cc‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ namespace diffCheck::registrations
1717

1818
std::shared_ptr<geometry::DFPointCloud> dfPointCloudPtrAfterTrans = std::make_shared<geometry::DFPointCloud>();
1919
dfPointCloudPtrAfterTrans->Cvt2DFPointCloud(o3DPointCloudAfterTrans);
20-
std::vector<double> registrationErrors = dfPointCloudPtrAfterTrans->ComputeP2PDistance(target);
20+
std::vector<double> registrationErrors = dfPointCloudPtrAfterTrans->ComputeDistance(*target);
2121
errors.push_back(std::accumulate(registrationErrors.begin(), registrationErrors.end(), 0.0) / registrationErrors.size());
2222
}
2323
return errors;

‎src/diffCheckApp.cc‎

Lines changed: 41 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
int main()
1010
{
1111
// import clouds
12-
std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtr
12+
std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtr
1313
= std::make_shared<diffCheck::geometry::DFPointCloud>();
1414
std::shared_ptr<diffCheck::geometry::DFMesh> dfMeshPtr
1515
= std::make_shared<diffCheck::geometry::DFMesh>();
@@ -21,56 +21,57 @@ int main()
2121
dfPointCloudPtr->LoadFromPLY(pathCloud);
2222
dfMeshPtr->LoadFromPLY(pathMesh);
2323

24-
open3d::geometry::TriangleMesh meshO3d = *dfMeshPtr->Cvt2O3DTriangleMesh();
25-
open3d::geometry::PointCloud pointCloudO3d = *dfPointCloudPtr->Cvt2O3DPointCloud();
26-
27-
auto rayCastingScene = open3d::t::geometry::RaycastingScene();
28-
// Get the vertices of the mesh
29-
std::vector<Eigen::Vector3d> vertices = meshO3d.vertices_;
30-
31-
// Convert the vertices to a tensor
32-
std::vector<float> verticesPosition;
33-
for (const auto& vertex : vertices) {
34-
verticesPosition.insert(verticesPosition.end(), vertex.data(), vertex.data() + 3);
35-
}
36-
open3d::core::Tensor verticesPositionTensor(verticesPosition.data(), {static_cast<int64_t>(vertices.size()), 3}, open3d::core::Dtype::Float32);
37-
38-
std::vector<uint32_t> triangles;
39-
for (int i = 0; i < meshO3d.triangles_.size(); i++) {
40-
triangles.push_back(static_cast<uint32_t>(meshO3d.triangles_[i].x()));
41-
triangles.push_back(static_cast<uint32_t>(meshO3d.triangles_[i].y()));
42-
triangles.push_back(static_cast<uint32_t>(meshO3d.triangles_[i].z()));
43-
}
44-
open3d::core::Tensor trianglesTensor(triangles.data(), {static_cast<int64_t>(meshO3d.triangles_.size()), 3}, open3d::core::Dtype::UInt32);
24+
// open3d::geometry::TriangleMesh meshO3d = *dfMeshPtr->Cvt2O3DTriangleMesh();
25+
// open3d::geometry::PointCloud pointCloudO3d = *dfPointCloudPtr->Cvt2O3DPointCloud();
26+
27+
// auto rayCastingScene = open3d::t::geometry::RaycastingScene();
28+
// // Get the vertices of the mesh
29+
// std::vector<Eigen::Vector3d> vertices = meshO3d.vertices_;
30+
31+
// // Convert the vertices to a tensor
32+
// std::vector<float> verticesPosition;
33+
// for (const auto& vertex : vertices) {
34+
// verticesPosition.insert(verticesPosition.end(), vertex.data(), vertex.data() + 3);
35+
// }
36+
// open3d::core::Tensor verticesPositionTensor(verticesPosition.data(), {static_cast<int64_t>(vertices.size()), 3}, open3d::core::Dtype::Float32);
37+
38+
// std::vector<uint32_t> triangles;
39+
// for (int i = 0; i < meshO3d.triangles_.size(); i++) {
40+
// triangles.push_back(static_cast<uint32_t>(meshO3d.triangles_[i].x()));
41+
// triangles.push_back(static_cast<uint32_t>(meshO3d.triangles_[i].y()));
42+
// triangles.push_back(static_cast<uint32_t>(meshO3d.triangles_[i].z()));
43+
// }
44+
// open3d::core::Tensor trianglesTensor(triangles.data(), {static_cast<int64_t>(meshO3d.triangles_.size()), 3}, open3d::core::Dtype::UInt32);
4545

46-
rayCastingScene.AddTriangles(verticesPositionTensor, trianglesTensor);
46+
// rayCastingScene.AddTriangles(verticesPositionTensor, trianglesTensor);
4747

48-
// compute the cloud to mesh signed distance
49-
std::vector<float> cloudPoints;
50-
for (const auto& point : pointCloudO3d.points_) {
51-
cloudPoints.insert(cloudPoints.end(), point.data(), point.data() + 3);
52-
}
53-
open3d::core::Tensor cloudPointsTensor(cloudPoints.data(), {static_cast<int64_t>(pointCloudO3d.points_.size()), 3}, open3d::core::Dtype::Float32);
48+
// // compute the cloud to mesh signed distance
49+
// std::vector<float> cloudPoints;
50+
// for (const auto& point : pointCloudO3d.points_) {
51+
// cloudPoints.insert(cloudPoints.end(), point.data(), point.data() + 3);
52+
// }
53+
// open3d::core::Tensor cloudPointsTensor(cloudPoints.data(), {static_cast<int64_t>(pointCloudO3d.points_.size()), 3}, open3d::core::Dtype::Float32);
5454

5555

56-
open3d::core::Tensor sdf = rayCastingScene.ComputeSignedDistance(cloudPointsTensor);
56+
// open3d::core::Tensor sdf = rayCastingScene.ComputeSignedDistance(cloudPointsTensor);
5757

58-
// if true, get the absolute value of the sdf
59-
if (true) {
60-
sdf = sdf.Abs();
61-
}
58+
// // FIXME: replace with bool parameter
59+
// // if true, get the absolute value of the sdf
60+
// if (true) {
61+
// sdf = sdf.Abs();
62+
// }
6263

6364
// convert sdf to a vector
64-
std::vector<float> sdfVector(sdf.GetDataPtr<float>(), sdf.GetDataPtr<float>() + sdf.NumElements());
65+
std::vector<float> sdfVector = dfMeshPtr->ComputeDistance(*dfPointCloudPtr);
6566

6667

6768
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
6869
// compute point cloud to point cloud distance
69-
auto distances = dfPointCloudPtr->ComputeP2PDistance(dfPointCloudPtr);
70-
for (auto &dist : distances)
71-
{
72-
std::cout << dist << std::endl;
73-
}
70+
std::vector<double> distances = dfPointCloudPtr->ComputeDistance(*dfPointCloudPtr);
71+
// for (auto &dist : distances)
72+
// {
73+
// std::cout << dist << std::endl;
74+
// }
7475

7576

7677

‎src/diffCheckBindings.cc‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,9 @@ PYBIND11_MODULE(diffcheck_bindings, m) {
2929
.def(py::init<>())
3030
.def(py::init<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>>())
3131

32-
.def("compute_P2PDistance", &diffCheck::geometry::DFPointCloud::ComputeP2PDistance)
32+
.def("compute_distance", &diffCheck::geometry::DFPointCloud::ComputeDistance,
33+
py::arg("targetCloud"),
34+
py::arg("isAbs") = true)
3335
.def("compute_BoundingBox", &diffCheck::geometry::DFPointCloud::ComputeBoundingBox)
3436

3537
.def("voxel_downsample", &diffCheck::geometry::DFPointCloud::VoxelDownsample,
@@ -65,6 +67,10 @@ PYBIND11_MODULE(diffcheck_bindings, m) {
6567
.def(py::init<>())
6668
.def(py::init<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3i>, std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>>())
6769

70+
.def("compute_distance", &diffCheck::geometry::DFMesh::ComputeDistance,
71+
py::arg("targetCloud"),
72+
py::arg("isAbs") = true)
73+
6874
.def("load_from_PLY", &diffCheck::geometry::DFMesh::LoadFromPLY)
6975

7076
.def("sample_points_uniformly", &diffCheck::geometry::DFMesh::SampleCloudUniform)

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@ diffCheck/df_geometries.py
66
diffCheck/df_joint_detector.py
77
diffCheck/df_transformations.py
88
diffCheck/df_util.py
9-
diffCheck/diffcheck_bindings.cp39-win_amd64.pyd
9+
diffCheck/diffcheck_bindings.cp38-win_amd64.pyd
1010
diffCheck.egg-info/PKG-INFO
1111
diffCheck.egg-info/SOURCES.txt
1212
diffCheck.egg-info/dependency_links.txt

0 commit comments

Comments
 (0)