1#[cfg(test)]
6mod tests {
7 use crate::IcpRegistration;
8 use crate::KdTree3D;
9 use crate::PointCloud;
10 use crate::PointCloudFilter;
11 use crate::point_cloud::*;
12 fn flat_grid(nx: usize, ny: usize, dx: f64) -> PointCloud {
14 PointCloud::from_grid(nx, ny, |_x, _y| 0.0, dx)
15 }
16 #[test]
17 fn test_from_grid_point_count() {
18 let cloud = flat_grid(5, 7, 1.0);
19 assert_eq!(cloud.len(), 35);
20 }
21 #[test]
22 fn test_centroid_symmetric_cloud() {
23 let mut cloud = PointCloud::new();
24 for &x in &[-1.0f64, 1.0] {
25 for &y in &[-1.0f64, 1.0] {
26 for &z in &[-1.0f64, 1.0] {
27 cloud.add_point([x, y, z]);
28 }
29 }
30 }
31 let c = cloud.centroid();
32 assert!(c[0].abs() < 1e-10, "cx = {}", c[0]);
33 assert!(c[1].abs() < 1e-10, "cy = {}", c[1]);
34 assert!(c[2].abs() < 1e-10, "cz = {}", c[2]);
35 }
36 #[test]
37 fn test_kdtree_nearest_single_point() {
38 let pts = vec![[1.0f64, 2.0, 3.0]];
39 let tree = KdTree3D::build(&pts);
40 let (idx, d2) = tree.nearest_neighbor([1.0, 2.0, 3.0]);
41 assert_eq!(idx, 0);
42 assert!(d2 < 1e-12);
43 }
44 #[test]
45 fn test_kdtree_k_nearest_returns_k() {
46 let pts: Vec<[f64; 3]> = (0..20).map(|i| [i as f64, 0.0, 0.0]).collect();
47 let tree = KdTree3D::build(&pts);
48 let k = 5;
49 let result = tree.k_nearest([0.0, 0.0, 0.0], k);
50 assert_eq!(result.len(), k);
51 assert!(result[0].1 < result[result.len() - 1].1 + 1e-12);
52 }
53 #[test]
54 fn test_voxel_downsample_fewer_points() {
55 let cloud = flat_grid(10, 10, 0.1);
56 let downsampled = PointCloudFilter::voxel_downsample(&cloud, 1.0);
57 assert!(
58 downsampled.len() < cloud.len(),
59 "expected fewer points after downsampling, got {} vs {}",
60 downsampled.len(),
61 cloud.len()
62 );
63 }
64 #[test]
65 fn test_bounding_box_contains_centroid() {
66 let cloud = flat_grid(5, 5, 1.0);
67 let (mn, mx) = cloud.bounding_box();
68 let c = cloud.centroid();
69 for i in 0..3 {
70 assert!(
71 mn[i] <= c[i] + 1e-9 && c[i] <= mx[i] + 1e-9,
72 "axis {}: centroid {} not in [{}, {}]",
73 i,
74 c[i],
75 mn[i],
76 mx[i]
77 );
78 }
79 }
80 #[test]
81 fn test_scale_uniform_doubles_distances() {
82 let mut cloud = PointCloud::new();
83 cloud.add_point([1.0, 0.0, 0.0]);
84 cloud.add_point([3.0, 0.0, 0.0]);
85 let d_before = dist2(cloud.points[0], cloud.points[1]).sqrt();
86 cloud.scale_uniform(2.0);
87 let d_after = dist2(cloud.points[0], cloud.points[1]).sqrt();
88 assert!((d_after - 2.0 * d_before).abs() < 1e-10);
89 }
90 #[test]
91 fn test_estimate_normals_sphere_surface() {
92 let mut cloud = PointCloud::new();
93 let n = 10usize;
94 for i in 0..n {
95 for j in 0..n {
96 let theta = std::f64::consts::PI * i as f64 / (n - 1) as f64;
97 let phi = 2.0 * std::f64::consts::PI * j as f64 / n as f64;
98 let x = theta.sin() * phi.cos();
99 let y = theta.sin() * phi.sin();
100 let z = theta.cos();
101 cloud.add_point([x, y, z]);
102 }
103 }
104 let normals = estimate_normals(&cloud, 8);
105 assert_eq!(normals.len(), cloud.len());
106 let mut ok = 0usize;
107 for (p, n) in cloud.points.iter().zip(normals.iter()) {
108 let r = (p[0].powi(2) + p[1].powi(2) + p[2].powi(2)).sqrt();
109 if r < 1e-6 {
110 continue;
111 }
112 let dot = (p[0] * n[0] + p[1] * n[1] + p[2] * n[2]) / r;
113 if dot.abs() > 0.5 {
114 ok += 1;
115 }
116 }
117 assert!(
118 ok as f64 > cloud.len() as f64 * 0.7,
119 "fewer than 70% of sphere normals are radial: {ok}/{}",
120 cloud.len()
121 );
122 }
123 #[test]
124 fn test_voxel_downsample_free_fn() {
125 let cloud = flat_grid(8, 8, 0.1);
126 let down = voxel_downsample(&cloud, 0.5);
127 assert!(down.len() < cloud.len(), "expected fewer points");
128 }
129 #[test]
130 fn test_statistical_outlier_removal_keeps_inliers() {
131 let cloud = flat_grid(5, 5, 1.0);
132 let filtered = statistical_outlier_removal(&cloud, 4, 2.0);
133 assert!(!filtered.is_empty(), "expected some points to remain");
134 }
135 #[test]
136 fn test_compute_bounding_box_free_fn() {
137 let cloud = flat_grid(5, 5, 1.0);
138 let (mn, mx) = compute_bounding_box(&cloud);
139 assert!((mn[0] - 0.0).abs() < 1e-9, "min x={}", mn[0]);
140 assert!((mn[1] - 0.0).abs() < 1e-9, "min y={}", mn[1]);
141 assert!((mx[0] - 4.0).abs() < 1e-9, "max x={}", mx[0]);
142 assert!((mx[1] - 4.0).abs() < 1e-9, "max y={}", mx[1]);
143 }
144 #[test]
145 fn test_icp_align_identity() {
146 let cloud = flat_grid(4, 4, 1.0);
147 let (m, err) = icp_align(&cloud, &cloud, 20);
148 assert!(
149 err < 1e-6,
150 "ICP error on identity should be near 0, got {err}"
151 );
152 assert!((m[0] - 1.0).abs() < 1e-6, "m[0]={}", m[0]);
153 assert!((m[5] - 1.0).abs() < 1e-6, "m[5]={}", m[5]);
154 assert!((m[10] - 1.0).abs() < 1e-6, "m[10]={}", m[10]);
155 }
156 #[test]
157 fn test_icp_align_translation() {
158 let mut source = PointCloud::new();
159 let mut target = PointCloud::new();
160 for i in 0..8 {
161 for j in 0..8 {
162 target.add_point([i as f64, j as f64, 0.0]);
163 source.add_point([i as f64 + 0.05, j as f64, 0.0]);
164 }
165 }
166 let (_m, err) = icp_align(&source, &target, 100);
167 assert!(
168 err < 0.01,
169 "ICP should reduce error after translation, err={err}"
170 );
171 }
172 #[test]
173 fn test_kdtree_range_search() {
174 let pts: Vec<[f64; 3]> = (0..10).map(|i| [i as f64, 0.0, 0.0]).collect();
175 let tree = KdTree3D::build(&pts);
176 let mut found = tree.range_search([0.0, 0.0, 0.0], 1.5);
177 found.sort_unstable();
178 assert_eq!(found, vec![0, 1], "range search result: {found:?}");
179 }
180 #[test]
181 fn test_radius_outlier_removal() {
182 let mut cloud = PointCloud::new();
183 for i in 0..5 {
184 for j in 0..5 {
185 cloud.add_point([i as f64 * 0.1, j as f64 * 0.1, 0.0]);
186 }
187 }
188 cloud.add_point([100.0, 100.0, 100.0]);
189 let filtered = PointCloudFilter::radius_outlier_removal(&cloud, 0.3, 2);
190 assert!(filtered.len() < cloud.len(), "outlier should be removed");
191 for p in &filtered.points {
192 assert!(p[0] < 50.0, "outlier survived: {:?}", p);
193 }
194 }
195 #[test]
196 fn test_normal_estimation_unit_vectors() {
197 let cloud = flat_grid(5, 5, 1.0);
198 let normals = estimate_normals(&cloud, 6);
199 for (i, n) in normals.iter().enumerate() {
200 let len = (n[0].powi(2) + n[1].powi(2) + n[2].powi(2)).sqrt();
201 assert!((len - 1.0).abs() < 1e-10, "normal {i} not unit: len={len}");
202 }
203 }
204 #[test]
205 fn test_translate_moves_centroid() {
206 let mut cloud = flat_grid(4, 4, 1.0);
207 let before = cloud.centroid();
208 cloud.translate([5.0, -3.0, 2.0]);
209 let after = cloud.centroid();
210 assert!((after[0] - (before[0] + 5.0)).abs() < 1e-9);
211 assert!((after[1] - (before[1] - 3.0)).abs() < 1e-9);
212 assert!((after[2] - (before[2] + 2.0)).abs() < 1e-9);
213 }
214 #[test]
215 fn test_bounding_box_correct() {
216 let pts = vec![[1.0, 2.0, 3.0], [-1.0, 0.0, 5.0], [0.0, 4.0, 1.0]];
217 let cloud = PointCloud::from_points(pts);
218 let (mn, mx) = cloud.bounding_box();
219 assert!((mn[0] - (-1.0)).abs() < 1e-10);
220 assert!((mn[1] - 0.0).abs() < 1e-10);
221 assert!((mn[2] - 1.0).abs() < 1e-10);
222 assert!((mx[0] - 1.0).abs() < 1e-10);
223 assert!((mx[1] - 4.0).abs() < 1e-10);
224 assert!((mx[2] - 5.0).abs() < 1e-10);
225 }
226 #[test]
227 fn test_centroid_expansion() {
228 let pts = vec![[0.0, 0.0, 0.0], [2.0, 0.0, 0.0], [1.0, 2.0, 0.0]];
229 let cloud = PointCloud::from_points(pts);
230 let c = cloud.centroid();
231 assert!((c[0] - 1.0).abs() < 1e-10);
232 assert!((c[1] - (2.0 / 3.0)).abs() < 1e-10);
233 assert!(c[2].abs() < 1e-10);
234 }
235 #[test]
236 fn test_downsample_reduces_count() {
237 let cloud = flat_grid(10, 10, 0.1);
238 let down = cloud.voxel_downsample(1.0);
239 assert!(
240 down.len() < cloud.len(),
241 "expected fewer points after voxel downsampling: {} < {}",
242 down.len(),
243 cloud.len()
244 );
245 }
246 #[test]
247 fn test_outlier_removal_expansion() {
248 let mut pts: Vec<[f64; 3]> = (0..5)
249 .flat_map(|i| (0..5).map(move |j| [i as f64 * 0.1, j as f64 * 0.1, 0.0]))
250 .collect();
251 pts.push([100.0, 100.0, 100.0]);
252 let cloud = PointCloud::from_points(pts.clone());
253 let filtered = cloud.statistical_outlier_removal(4, 1.5);
254 assert!(filtered.len() < cloud.len(), "outlier should be removed");
255 }
256 #[test]
257 fn test_icp_zero_transform_same_set() {
258 let pts: Vec<[f64; 3]> = (0..5)
259 .flat_map(|i| (0..5).map(move |j| [i as f64, j as f64, 0.0]))
260 .collect();
261 let transform = icp_point_to_point(&pts, &pts, 20);
262 for (i, row_i) in transform.iter().enumerate().take(3) {
263 for (j, &val_ij) in row_i.iter().enumerate() {
264 let expected = if i == j { 1.0 } else { 0.0 };
265 assert!(
266 (val_ij - expected).abs() < 1e-4,
267 "rotation[{i}][{j}] = {} expected {expected}",
268 val_ij
269 );
270 }
271 }
272 for &v in &transform[3] {
273 assert!(v.abs() < 1e-4, "translation should be near zero: {v}");
274 }
275 }
276 #[test]
277 fn test_compute_point_cloud_normals_unit() {
278 let cloud = flat_grid(5, 5, 1.0);
279 let normals = compute_point_cloud_normals(&cloud.points, 6);
280 assert_eq!(normals.len(), cloud.len());
281 for n in &normals {
282 let len = (n[0].powi(2) + n[1].powi(2) + n[2].powi(2)).sqrt();
283 assert!((len - 1.0).abs() < 1e-10, "normal not unit: len={len}");
284 }
285 }
286 #[test]
287 fn test_fpfh_feature_length_and_finite() {
288 let cloud = flat_grid(5, 5, 1.0);
289 let normals = compute_point_cloud_normals(&cloud.points, 6);
290 let feat = fpfh_feature(&cloud.points, &normals, 12, 2.0);
291 assert_eq!(feat.len(), 33, "FPFH descriptor should have 33 bins");
292 for &v in &feat {
293 assert!(v.is_finite(), "FPFH value must be finite: {v}");
294 }
295 }
296 #[test]
297 fn test_fps_returns_k_points() {
298 let pts: Vec<[f64; 3]> = (0..20).map(|i| [i as f64, 0.0, 0.0]).collect();
299 let selected = farthest_point_sampling(&pts, 5, 0);
300 assert_eq!(selected.len(), 5, "FPS should return exactly k points");
301 }
302 #[test]
303 fn test_fps_no_duplicates() {
304 let pts: Vec<[f64; 3]> = (0..20).map(|i| [i as f64, 0.0, 0.0]).collect();
305 let selected = farthest_point_sampling(&pts, 10, 0);
306 let unique: std::collections::HashSet<usize> = selected.iter().copied().collect();
307 assert_eq!(
308 unique.len(),
309 selected.len(),
310 "FPS should return distinct indices"
311 );
312 }
313 #[test]
314 fn test_fps_valid_indices() {
315 let pts: Vec<[f64; 3]> = (0..15).map(|i| [i as f64, (i as f64).sin(), 0.0]).collect();
316 let selected = farthest_point_sampling(&pts, 7, 3);
317 for &idx in &selected {
318 assert!(
319 idx < pts.len(),
320 "FPS index {idx} out of range (n={})",
321 pts.len()
322 );
323 }
324 }
325 #[test]
326 fn test_fps_spread_for_line() {
327 let pts: Vec<[f64; 3]> = (0..=10).map(|i| [i as f64, 0.0, 0.0]).collect();
328 let selected = farthest_point_sampling(&pts, 2, 0);
329 assert_eq!(selected.len(), 2);
330 let d = dist2_pts(pts[selected[0]], pts[selected[1]]).sqrt();
331 assert!(
332 d >= 9.0,
333 "two FPS points on a line should be near the ends: d={d}"
334 );
335 }
336 #[test]
337 fn test_fps_point_cloud_method() {
338 let cloud = flat_grid(8, 8, 1.0);
339 let sampled = cloud.farthest_point_sample(10);
340 assert_eq!(sampled.len(), 10);
341 }
342 #[test]
343 fn test_fps_k_larger_than_n_clamped() {
344 let pts = vec![[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [2.0, 0.0, 0.0]];
345 let selected = farthest_point_sampling(&pts, 100, 0);
346 assert_eq!(selected.len(), 3, "FPS k > n should return n points");
347 }
348 #[test]
349 fn test_ransac_plane_flat_grid() {
350 let cloud = flat_grid(6, 6, 1.0);
351 let result = cloud.fit_plane_ransac(50, 0.01);
352 assert!(
353 result.is_some(),
354 "RANSAC should find a plane in a flat grid"
355 );
356 let r = result.unwrap();
357 assert!(
358 r.normal[2].abs() > 0.9,
359 "plane normal should point ~z: {:?}",
360 r.normal
361 );
362 assert!(
363 r.n_inliers >= 36,
364 "all flat-grid points should be inliers, got {}",
365 r.n_inliers
366 );
367 }
368 #[test]
369 fn test_ransac_plane_normal_is_unit() {
370 let pts: Vec<[f64; 3]> = (0..5)
371 .flat_map(|i| (0..5).map(move |j| [i as f64, j as f64, 0.0]))
372 .collect();
373 let result = ransac_fit_plane(&pts, 30, 0.01).expect("should find plane");
374 let len = length(result.normal);
375 assert!((len - 1.0).abs() < 1e-6, "plane normal not unit: len={len}");
376 }
377 #[test]
378 fn test_ransac_plane_few_points_returns_none() {
379 let pts = vec![[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]];
380 let result = ransac_fit_plane(&pts, 10, 0.1);
381 assert!(result.is_none(), "fewer than 3 points should return None");
382 }
383 #[test]
384 fn test_ransac_plane_inlier_distance() {
385 let pts: Vec<[f64; 3]> = (0..6)
386 .flat_map(|i| (0..6).map(move |j| [i as f64, j as f64, 0.0]))
387 .collect();
388 let threshold = 0.05;
389 let result = ransac_fit_plane(&pts, 20, threshold).expect("should find plane");
390 let n = result.normal;
391 let d = dot(n, result.point_on_plane);
392 for &i in &result.inliers {
393 let dist = (dot(n, pts[i]) - d).abs();
394 assert!(
395 dist <= threshold + 1e-9,
396 "inlier {i} at distance {dist} > threshold {threshold}"
397 );
398 }
399 }
400 #[test]
401 fn test_aabb_extent_unit_cube() {
402 let pts = vec![
403 [0.0, 0.0, 0.0],
404 [1.0, 0.0, 0.0],
405 [1.0, 1.0, 0.0],
406 [0.0, 1.0, 0.0],
407 [0.0, 0.0, 1.0],
408 [1.0, 0.0, 1.0],
409 [1.0, 1.0, 1.0],
410 [0.0, 1.0, 1.0],
411 ];
412 let (dims, center) = aabb_extent(&pts);
413 for i in 0..3 {
414 assert!((dims[i] - 1.0).abs() < 1e-9, "dims[{i}]={}", dims[i]);
415 assert!((center[i] - 0.5).abs() < 1e-9, "center[{i}]={}", center[i]);
416 }
417 }
418 #[test]
419 fn test_aabb_extent_empty() {
420 let (dims, center) = aabb_extent(&[]);
421 assert_eq!(dims, [0.0; 3]);
422 assert_eq!(center, [0.0; 3]);
423 }
424 #[test]
425 fn test_pca_obb_axes_orthonormal() {
426 let cloud = flat_grid(5, 5, 1.0);
427 let (axes, _half_extents, _center) = pca_obb(&cloud.points);
428 for (k, ax) in axes.iter().enumerate() {
429 let len = length(*ax);
430 assert!((len - 1.0).abs() < 0.1, "axis {k} not unit: len={len}");
431 }
432 }
433 #[test]
434 fn test_pca_obb_contains_all_points() {
435 let pts: Vec<[f64; 3]> = (0..5)
436 .flat_map(|i| (0..5).map(move |j| [i as f64, j as f64, 0.0]))
437 .collect();
438 let (axes, half_extents, center) = pca_obb(&pts);
439 for &p in &pts {
440 let d = sub(p, center);
441 for k in 0..3 {
442 let proj = dot(d, axes[k]).abs();
443 assert!(
444 proj <= half_extents[k] + 1e-6,
445 "point {:?} projection {proj} > half_extent {} on axis {k}",
446 p,
447 half_extents[k]
448 );
449 }
450 }
451 }
452 #[test]
453 fn test_pca_obb_point_cloud_method() {
454 let cloud = flat_grid(4, 4, 1.0);
455 let (axes, he, _center) = cloud.pca_obb();
456 assert_eq!(axes.len(), 3);
457 assert_eq!(he.len(), 3);
458 for &h in &he {
459 assert!(
460 h.is_finite() && h >= 0.0,
461 "half_extent must be finite and non-negative: {h}"
462 );
463 }
464 }
465 #[test]
466 fn test_voxel_downsample_single_voxel() {
467 let pts = vec![[0.1, 0.1, 0.0], [0.2, 0.1, 0.0], [0.1, 0.2, 0.0]];
468 let cloud = PointCloud::from_points(pts.clone());
469 let down = PointCloudFilter::voxel_downsample(&cloud, 1.0);
470 assert_eq!(down.len(), 1, "all points in one voxel → 1 output point");
471 let avg_x = pts.iter().map(|p| p[0]).sum::<f64>() / 3.0;
472 let avg_y = pts.iter().map(|p| p[1]).sum::<f64>() / 3.0;
473 assert!((down.points[0][0] - avg_x).abs() < 1e-9);
474 assert!((down.points[0][1] - avg_y).abs() < 1e-9);
475 }
476 #[test]
477 fn test_statistical_outlier_removal_preserves_dense_cluster() {
478 let mut pts: Vec<[f64; 3]> = (0..5)
479 .flat_map(|i| (0..5).map(move |j| [i as f64 * 0.1, j as f64 * 0.1, 0.0]))
480 .collect();
481 pts.push([1000.0, 1000.0, 1000.0]);
482 let cloud = PointCloud::from_points(pts);
483 let filtered = PointCloudFilter::statistical_outlier_removal(&cloud, 5, 1.0);
484 for p in &filtered.points {
485 assert!(p[0] < 100.0, "outlier survived: {:?}", p);
486 }
487 }
488 #[test]
489 fn test_knn_sorted_ascending() {
490 let pts: Vec<[f64; 3]> = (0..10).map(|i| [i as f64, 0.0, 0.0]).collect();
491 let tree = KdTree3D::build(&pts);
492 let result = tree.k_nearest([0.0, 0.0, 0.0], 5);
493 for w in result.windows(2) {
494 assert!(w[0].1 <= w[1].1 + 1e-12, "k-nearest not sorted: {:?}", w);
495 }
496 }
497 #[test]
498 fn test_icp_result_apply_to_cloud() {
499 let src = flat_grid(3, 3, 1.0);
500 let tgt = flat_grid(3, 3, 1.0);
501 let result = IcpRegistration::align(&src, &tgt);
502 let transformed = result.apply_to(&src);
503 assert_eq!(
504 transformed.len(),
505 src.len(),
506 "apply_to should preserve point count"
507 );
508 }
509 #[test]
510 fn test_point_cloud_from_points() {
511 let pts = vec![[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]];
512 let cloud = PointCloud::from_points(pts.clone());
513 assert_eq!(cloud.len(), 2);
514 for (i, &p) in pts.iter().enumerate() {
515 for (k, &pk) in p.iter().enumerate() {
516 assert!((cloud.points[i][k] - pk).abs() < 1e-12);
517 }
518 }
519 }
520 #[test]
521 fn test_estimate_normals_pca_populated() {
522 let mut cloud = flat_grid(5, 5, 1.0);
523 cloud.estimate_normals_pca(6);
524 assert_eq!(
525 cloud.normals.len(),
526 cloud.points.len(),
527 "normals should match point count"
528 );
529 }
530 #[test]
531 fn test_point_cloud_is_empty() {
532 let cloud = PointCloud::new();
533 assert!(cloud.is_empty());
534 let mut cloud2 = PointCloud::new();
535 cloud2.add_point([1.0, 0.0, 0.0]);
536 assert!(!cloud2.is_empty());
537 }
538 #[test]
539 fn test_principal_curvatures_count_matches_points() {
540 let cloud = flat_grid(5, 5, 1.0);
541 let curvs = cloud.compute_principal_curvatures(6);
542 assert_eq!(
543 curvs.len(),
544 cloud.len(),
545 "curvature output length should match point count"
546 );
547 }
548 #[test]
549 fn test_principal_curvatures_finite_values() {
550 let cloud = flat_grid(5, 5, 1.0);
551 let curvs = cloud.compute_principal_curvatures(5);
552 for (i, (k1, k2)) in curvs.iter().enumerate() {
553 assert!(k1.is_finite(), "kappa1 at point {i} is not finite: {k1}");
554 assert!(k2.is_finite(), "kappa2 at point {i} is not finite: {k2}");
555 }
556 }
557 #[test]
558 fn test_principal_curvatures_flat_grid_small() {
559 let cloud = flat_grid(6, 6, 1.0);
560 let curvs = cloud.compute_principal_curvatures(8);
561 for (k1, _k2) in &curvs {
562 assert!(
563 *k1 >= -1e-6,
564 "smallest eigenvalue of covariance should be non-negative: {k1}"
565 );
566 }
567 }
568 #[test]
569 fn test_principal_curvatures_empty_cloud() {
570 let cloud = PointCloud::new();
571 let curvs = cloud.compute_principal_curvatures(5);
572 assert!(
573 curvs.is_empty(),
574 "empty cloud should yield empty curvatures"
575 );
576 }
577 #[test]
578 fn test_voxel_downsample_method_reduces_count() {
579 let cloud = flat_grid(10, 10, 0.1);
580 let down = cloud.voxel_downsample(1.0);
581 assert!(
582 down.len() < cloud.len(),
583 "voxel_downsample should reduce point count: {} < {}",
584 down.len(),
585 cloud.len()
586 );
587 }
588 #[test]
589 fn test_voxel_downsample_method_nonempty_result() {
590 let cloud = flat_grid(4, 4, 0.5);
591 let down = cloud.voxel_downsample(0.1);
592 assert!(!down.is_empty(), "downsampled cloud should not be empty");
593 }
594 #[test]
595 fn test_icp_register_same_cloud_low_error() {
596 let cloud = flat_grid(4, 4, 1.0);
597 let result = cloud.icp_register(&cloud);
598 assert!(
599 result.final_error < 1e-3,
600 "ICP same-cloud error should be near 0, got {}",
601 result.final_error
602 );
603 }
604 #[test]
605 fn test_icp_register_returns_valid_transform() {
606 let cloud = flat_grid(4, 4, 1.0);
607 let result = cloud.icp_register(&cloud);
608 for k in 0..3 {
609 assert!(
610 (result.rotation[k][k] - 1.0).abs() < 0.1,
611 "rotation[{k}][{k}] should be close to 1 for identity, got {}",
612 result.rotation[k][k]
613 );
614 }
615 }
616 #[test]
617 fn test_icp_register_apply_to_preserves_count() {
618 let src = flat_grid(3, 3, 1.0);
619 let tgt = flat_grid(3, 3, 1.0);
620 let result = src.icp_register(&tgt);
621 let transformed = result.apply_to(&src);
622 assert_eq!(
623 transformed.len(),
624 src.len(),
625 "apply_to should preserve point count"
626 );
627 }
628}