1515namespace open3d {
1616namespace tests {
1717
18- // A point cloud that is a plane with some noise.
19- static geometry::PointCloud CreateNoisyPlane (size_t n_points = 100 ,
20- double noise_std = 0.01 ) {
21- geometry::PointCloud pcd;
22- pcd.points_ .resize (n_points);
23-
24- std::mt19937 rng (0 ); // Fixed seed so that the test is deterministic
25- std::uniform_real_distribution<double > dist_xy (-1.0 , 1.0 );
26- std::uniform_real_distribution<double > dist_z (-noise_std, noise_std);
27-
28- for (size_t i = 0 ; i < n_points; ++i) {
29- pcd.points_ [i] =
30- Eigen::Vector3d (dist_xy (rng), dist_xy (rng), dist_z (rng));
31- }
32- return pcd;
33- }
34-
35- // Computes the average absolute distance to the Z=0 plane.
36- static double AveragePlaneDistance (const geometry::PointCloud& pcd) {
37- if (pcd.IsEmpty ()) {
38- return 0.0 ;
39- }
40- double total_dist = 0.0 ;
41- for (const auto & point : pcd.points_ ) {
42- total_dist += std::abs (point.z ());
43- }
44- return total_dist / pcd.points_ .size ();
45- }
46-
4718namespace {
48- // Helper functions copied from PointCloudSmoothing.cpp for testing.
19+ // Helper functions for point cloud smoothing tests
4920Eigen::Vector3d ComputeCentroid (const std::vector<Eigen::Vector3d>& points,
5021 const std::vector<int >& indices) {
5122 Eigen::Vector3d centroid = Eigen::Vector3d::Zero ();
5223 if (indices.empty ()) {
5324 return centroid;
5425 }
26+
27+ const int n_points = static_cast <int >(points.size ());
5528 int valid_count = 0 ;
5629 for (int idx : indices) {
57- if (idx < 0 || idx >= static_cast < int >(points. size ()) ) {
30+ if (idx < 0 || idx >= n_points ) {
5831 continue ;
5932 }
6033 centroid += points[idx];
@@ -76,11 +49,11 @@ Eigen::Vector3d ComputeWeightedCentroid(const open3d::geometry::PointCloud& pcd,
7649 }
7750
7851 double sum_w = 0.0 ;
79- const size_t n_points = pcd.points_ .size ();
52+ const int n_points = static_cast < int >( pcd.points_ .size () );
8053
8154 for (size_t i = 0 ; i < indices.size (); ++i) {
8255 int idx = indices[i];
83- if (idx < 0 || idx >= static_cast < int >( n_points) ) {
56+ if (idx < 0 || idx >= n_points) {
8457 continue ;
8558 }
8659
@@ -102,18 +75,24 @@ Eigen::Matrix3d ComputeWeightedCovariance(
10275 const std::vector<double >& weights,
10376 const Eigen::Vector3d& centroid) {
10477 Eigen::Matrix3d C = Eigen::Matrix3d::Zero ();
78+
10579 if (indices.size () != weights.size () || indices.empty ()) {
10680 return C;
10781 }
108- const size_t n_points = pcd.points_ .size ();
82+
83+ const int n_points = static_cast <int >(pcd.points_ .size ());
84+
10985 for (size_t i = 0 ; i < indices.size (); ++i) {
11086 const int idx = indices[i];
111- if (idx < 0 || idx >= static_cast <int >(n_points)) {
87+
88+ if (idx < 0 || idx >= n_points) {
11289 continue ;
11390 }
91+
11492 const Eigen::Vector3d diff = pcd.points_ [idx] - centroid;
11593 C.noalias () += weights[i] * diff * diff.transpose ();
11694 }
95+
11796 return C;
11897}
11998
@@ -124,6 +103,35 @@ Eigen::Vector3d ProjectOntoPlane(const Eigen::Vector3d& p,
124103}
125104} // namespace
126105
106+ // A point cloud that is a plane with some noise.
107+ static geometry::PointCloud CreateNoisyPlane (size_t n_points = 100 ,
108+ double noise_std = 0.01 ) {
109+ geometry::PointCloud pcd;
110+ pcd.points_ .resize (n_points);
111+
112+ std::mt19937 rng (0 ); // Fixed seed so that the test is deterministic
113+ std::uniform_real_distribution<double > dist_xy (-1.0 , 1.0 );
114+ std::uniform_real_distribution<double > dist_z (-noise_std, noise_std);
115+
116+ for (size_t i = 0 ; i < n_points; ++i) {
117+ pcd.points_ [i] =
118+ Eigen::Vector3d (dist_xy (rng), dist_xy (rng), dist_z (rng));
119+ }
120+ return pcd;
121+ }
122+
123+ // Computes the average absolute distance to the Z=0 plane.
124+ static double AveragePlaneDistance (const geometry::PointCloud& pcd) {
125+ if (pcd.IsEmpty ()) {
126+ return 0.0 ;
127+ }
128+ double total_dist = 0.0 ;
129+ for (const auto & point : pcd.points_ ) {
130+ total_dist += std::abs (point.z ());
131+ }
132+ return total_dist / pcd.points_ .size ();
133+ }
134+
127135TEST (PointCloudSmoothingHelpers, ComputeCentroid) {
128136 std::vector<Eigen::Vector3d> points = {
129137 {0 , 0 , 0 }, {1 , 1 , 1 }, {2 , 2 , 2 }, {-1 , -1 , -1 }};
0 commit comments