spatialrust_pipeline/
mvp.rs

1//! MVP point cloud processing pipeline.
2//!
3//! Chains voxel downsampling, normal estimation, plane segmentation, clustering,
4//! and optional ICP registration.
5
6use spatialrust_core::{
7    ExecutionPolicy, ExecutionReceipt, PointCloud, SpatialResult, TransferDirection, TransferStats,
8};
9use spatialrust_features::{NormalEstimationConfig, NormalEstimator};
10use spatialrust_filtering::{VoxelGridDownsample, VoxelGridDownsampleConfig};
11use spatialrust_math::Isometry3;
12use spatialrust_registration::{
13    transform_point_cloud, GicpConfig, GicpRegistration, IcpConfig, IcpRegistration,
14    PointCloudRegistration, PointToPlaneIcp, PointToPlaneIcpConfig, RegistrationResult,
15};
16use spatialrust_segmentation::{
17    EuclideanClusterConfig, EuclideanClusterExtractor, EuclideanClusterResult, RansacPlaneConfig,
18    RansacPlaneSegmentation, RansacPlaneSegmenter,
19};
20
21/// Registration backend used by the MVP pipeline's optional alignment step.
22#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
23pub enum MvpRegistrationMethod {
24    /// Classic point-to-point ICP (target = downsampled cloud).
25    #[default]
26    PointToPoint,
27    /// Point-to-plane ICP using the estimated normals (target = cloud with normals).
28    PointToPlane,
29    /// Generalized ICP (plane-to-plane, target = downsampled cloud).
30    Gicp,
31}
32
33/// Configuration for optional ICP in the MVP pipeline.
34#[derive(Clone, Debug, PartialEq)]
35pub struct MvpIcpConfig {
36    /// Shared registration settings (iterations, correspondence distance, thresholds).
37    pub icp: IcpConfig,
38    /// Optional transform applied to the reference cloud to synthesize a source scan.
39    pub source_transform: Option<Isometry3<f32>>,
40    /// Registration backend to use.
41    pub method: MvpRegistrationMethod,
42}
43
44impl Default for MvpIcpConfig {
45    fn default() -> Self {
46        Self {
47            icp: IcpConfig::default(),
48            source_transform: None,
49            method: MvpRegistrationMethod::PointToPoint,
50        }
51    }
52}
53
54/// Full configuration for the MVP pipeline.
55#[derive(Clone, Debug, PartialEq)]
56pub struct MvpPipelineConfig {
57    /// Voxel downsampling settings.
58    pub voxel: VoxelGridDownsampleConfig,
59    /// Normal estimation settings.
60    pub normals: NormalEstimationConfig,
61    /// Dominant plane segmentation settings.
62    pub plane: RansacPlaneConfig,
63    /// Euclidean clustering settings for non-plane points.
64    pub cluster: EuclideanClusterConfig,
65    /// Optional ICP registration against the downsampled reference cloud.
66    pub icp: Option<MvpIcpConfig>,
67    /// Execution policy for the voxel downsampling stage.
68    pub voxel_policy: ExecutionPolicy,
69    /// Execution policy for the RANSAC plane segmentation stage.
70    pub plane_policy: ExecutionPolicy,
71    /// Execution policy for normal estimation.
72    pub normal_policy: ExecutionPolicy,
73    /// Optional multiplier for GPU normal grid radius (`leaf_size * scale`) under GPU Auto/Gpu.
74    pub normal_gpu_radius_scale: f32,
75    /// Execution policy for Euclidean clustering.
76    pub cluster_policy: ExecutionPolicy,
77}
78
79impl Default for MvpPipelineConfig {
80    fn default() -> Self {
81        Self {
82            voxel: VoxelGridDownsampleConfig::centroid(0.05),
83            normals: NormalEstimationConfig::default(),
84            plane: RansacPlaneConfig::default(),
85            cluster: EuclideanClusterConfig::default(),
86            icp: None,
87            voxel_policy: ExecutionPolicy::Auto,
88            plane_policy: ExecutionPolicy::Auto,
89            normal_policy: ExecutionPolicy::Auto,
90            normal_gpu_radius_scale: 2.0,
91            cluster_policy: ExecutionPolicy::Auto,
92        }
93    }
94}
95
96impl MvpPipelineConfig {
97    /// Creates a config with the given voxel leaf size.
98    #[must_use]
99    pub fn with_voxel_leaf_size(leaf_size: f32) -> Self {
100        Self { voxel: VoxelGridDownsampleConfig::centroid(leaf_size), ..Self::default() }
101    }
102}
103
104/// Output of a completed MVP pipeline run.
105#[derive(Clone, Debug, PartialEq)]
106pub struct MvpPipelineResult {
107    /// Cloud after voxel downsampling.
108    pub downsampled: PointCloud,
109    /// Cloud with estimated normals and curvature.
110    pub with_normals: PointCloud,
111    /// Plane segmentation result.
112    pub plane: RansacPlaneSegmentation,
113    /// Clustering result on plane outliers.
114    pub clusters: EuclideanClusterResult,
115    /// Optional ICP registration result.
116    pub registration: Option<RegistrationResult>,
117    /// Primary pipeline output (labeled cluster cloud).
118    pub output: PointCloud,
119    /// Per-stage execution and transfer accounting.
120    pub receipt: MvpPipelineReceipt,
121}
122
123/// Execution receipt for the stages of one MVP pipeline run.
124#[derive(Clone, Debug, PartialEq)]
125pub struct MvpPipelineReceipt {
126    /// Voxel downsampling receipt.
127    pub voxel: ExecutionReceipt,
128    /// Normal estimation receipt.
129    pub normals: ExecutionReceipt,
130    /// Plane segmentation receipt.
131    pub plane: ExecutionReceipt,
132    /// Euclidean clustering receipt.
133    pub clusters: ExecutionReceipt,
134    /// Optional CPU registration receipt.
135    pub registration: Option<ExecutionReceipt>,
136}
137
138impl MvpPipelineReceipt {
139    /// Aggregates transfer accounting across all pipeline stages.
140    #[must_use]
141    pub fn transfer_stats(&self) -> TransferStats {
142        let mut stats = TransferStats::default();
143        for receipt in [&self.voxel, &self.normals, &self.plane, &self.clusters]
144            .into_iter()
145            .chain(self.registration.iter())
146        {
147            stats.record(TransferDirection::HostToDevice, receipt.host_to_device_bytes());
148            stats.record(TransferDirection::DeviceToDevice, receipt.device_to_device_bytes());
149            stats.record(TransferDirection::DeviceToHost, receipt.device_to_host_bytes());
150        }
151        stats
152    }
153
154    /// Returns total bytes uploaded from host memory.
155    #[must_use]
156    pub fn host_to_device_bytes(&self) -> u64 {
157        self.transfer_stats().host_to_device_bytes()
158    }
159
160    /// Returns total bytes copied between device buffers.
161    #[must_use]
162    pub fn device_to_device_bytes(&self) -> u64 {
163        self.transfer_stats().device_to_device_bytes()
164    }
165
166    /// Returns total bytes read back to host memory.
167    #[must_use]
168    pub fn device_to_host_bytes(&self) -> u64 {
169        self.transfer_stats().device_to_host_bytes()
170    }
171}
172
173/// Builder-style MVP pipeline runner.
174#[derive(Clone, Debug, PartialEq)]
175pub struct MvpPipeline {
176    config: MvpPipelineConfig,
177}
178
179impl MvpPipeline {
180    /// Creates a pipeline from config.
181    #[must_use]
182    pub fn new(config: MvpPipelineConfig) -> Self {
183        Self { config }
184    }
185
186    /// Returns the pipeline config.
187    #[must_use]
188    pub fn config(&self) -> &MvpPipelineConfig {
189        &self.config
190    }
191
192    /// Runs the full MVP pipeline on the input cloud.
193    pub fn run(&self, input: &PointCloud) -> SpatialResult<MvpPipelineResult> {
194        let (downsampled, voxel_receipt) = VoxelGridDownsample::new(self.config.voxel)
195            .filter_with_policy_and_receipt(input, self.config.voxel_policy)?
196            .into_parts();
197
198        let normal_config = {
199            #[cfg(feature = "pipeline-mvp-gpu")]
200            {
201                let mut config = self.config.normals;
202                let estimator = NormalEstimator::new(config);
203                if config.search_radius.is_none()
204                    && estimator.selects_gpu_backend(&downsampled, self.config.normal_policy)
205                {
206                    let radius = (self.config.voxel.leaf_size
207                        * self.config.normal_gpu_radius_scale)
208                        .max(1e-4);
209                    if estimator.gpu_grid_fits(&downsampled, radius) {
210                        config.search_radius = Some(radius);
211                    }
212                }
213                config
214            }
215            #[cfg(not(feature = "pipeline-mvp-gpu"))]
216            {
217                self.config.normals
218            }
219        };
220        let (with_normals, normals_receipt) = NormalEstimator::new(normal_config)
221            .estimate_with_policy_and_receipt(&downsampled, self.config.normal_policy)?
222            .into_parts();
223
224        let (plane, plane_receipt) = RansacPlaneSegmenter::new(self.config.plane)
225            .segment_with_policy_and_receipt(&with_normals, self.config.plane_policy)?
226            .into_parts();
227
228        let (clusters, clusters_receipt) = EuclideanClusterExtractor::new(self.config.cluster)
229            .extract_with_policy_and_receipt(&plane.outliers, self.config.cluster_policy)?
230            .into_parts();
231
232        let (registration, registration_receipt) = if let Some(icp_config) = &self.config.icp {
233            // Point-to-plane aligns against the normal-bearing cloud; the others
234            // use the plain downsampled cloud as the reference target.
235            let target = match icp_config.method {
236                MvpRegistrationMethod::PointToPlane => &with_normals,
237                MvpRegistrationMethod::PointToPoint | MvpRegistrationMethod::Gicp => &downsampled,
238            };
239            let source = if let Some(transform) = icp_config.source_transform {
240                transform_point_cloud(target, transform)?
241            } else {
242                target.clone()
243            };
244            let result = match icp_config.method {
245                MvpRegistrationMethod::PointToPoint => {
246                    IcpRegistration::new(icp_config.icp).align(&source, target)?
247                }
248                MvpRegistrationMethod::PointToPlane => {
249                    PointToPlaneIcp::new(point_to_plane_config(&icp_config.icp))
250                        .align(&source, target)?
251                }
252                MvpRegistrationMethod::Gicp => {
253                    GicpRegistration::new(gicp_config(&icp_config.icp)).align(&source, target)?
254                }
255            };
256            let mut receipt =
257                ExecutionReceipt::new(ExecutionPolicy::CpuSingle, ExecutionPolicy::CpuSingle);
258            receipt.record_stage("registration");
259            (Some(result), Some(receipt))
260        } else {
261            (None, None)
262        };
263
264        Ok(MvpPipelineResult {
265            output: clusters.cloud.clone(),
266            downsampled,
267            with_normals,
268            plane,
269            clusters,
270            registration,
271            receipt: MvpPipelineReceipt {
272                voxel: voxel_receipt,
273                normals: normals_receipt,
274                plane: plane_receipt,
275                clusters: clusters_receipt,
276                registration: registration_receipt,
277            },
278        })
279    }
280}
281
282/// Maps the shared ICP settings onto a point-to-plane configuration.
283fn point_to_plane_config(icp: &IcpConfig) -> PointToPlaneIcpConfig {
284    PointToPlaneIcpConfig {
285        max_iterations: icp.max_iterations,
286        max_correspondence_distance: icp.max_correspondence_distance,
287        transformation_epsilon: icp.transformation_epsilon,
288        fitness_epsilon: icp.fitness_epsilon,
289        min_correspondences: icp.min_correspondences,
290        initial_guess: icp.initial_guess,
291    }
292}
293
294/// Maps the shared ICP settings onto a GICP configuration.
295fn gicp_config(icp: &IcpConfig) -> GicpConfig {
296    GicpConfig {
297        max_iterations: icp.max_iterations,
298        max_correspondence_distance: icp.max_correspondence_distance,
299        transformation_epsilon: icp.transformation_epsilon,
300        fitness_epsilon: icp.fitness_epsilon,
301        min_correspondences: icp.min_correspondences,
302        initial_guess: icp.initial_guess,
303        ..GicpConfig::default()
304    }
305}
306
307#[cfg(test)]
308mod tests {
309    use super::{MvpIcpConfig, MvpPipeline, MvpPipelineConfig};
310    use spatialrust_core::{PointCloudBuilder, StandardSchemas};
311    use spatialrust_math::{Isometry3, Quat, Vec3};
312    use spatialrust_registration::IcpConfig;
313    use spatialrust_segmentation::{EuclideanClusterConfig, RansacPlaneConfig};
314
315    fn sample_cloud() -> spatialrust_core::PointCloud {
316        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
317        for x in 0..10 {
318            for y in 0..10 {
319                builder.push_point([x as f32 * 0.1, y as f32 * 0.1, 0.0]).unwrap();
320            }
321        }
322        builder.push_point([0.0, 0.0, 0.5]).unwrap();
323        builder.push_point([0.1, 0.0, 0.5]).unwrap();
324        builder.build().unwrap()
325    }
326
327    #[test]
328    fn runs_voxel_normals_plane_and_cluster() {
329        let pipeline = MvpPipeline::new(MvpPipelineConfig {
330            voxel: spatialrust_filtering::VoxelGridDownsampleConfig::centroid(0.2),
331            normals: spatialrust_features::NormalEstimationConfig {
332                k_neighbors: 8,
333                min_neighbors: 3,
334                viewpoint: Some(Vec3::new(0.0, 0.0, 10.0)),
335                ..Default::default()
336            },
337            plane: RansacPlaneConfig {
338                distance_threshold: 0.05,
339                max_iterations: 500,
340                min_inliers: 10,
341                seed: 17,
342                ..Default::default()
343            },
344            cluster: EuclideanClusterConfig {
345                cluster_tolerance: 0.3,
346                min_cluster_size: 1,
347                max_cluster_size: usize::MAX,
348                ..Default::default()
349            },
350            icp: None,
351            ..Default::default()
352        });
353
354        let result = pipeline.run(&sample_cloud()).unwrap();
355        assert!(!result.downsampled.is_empty());
356        assert!(result.with_normals.field("normal_x").is_ok());
357        assert!(result.plane.inlier_count >= 10);
358        assert!(result.clusters.cluster_count >= 1);
359        assert!(result.output.field("label").is_ok());
360        assert!(result.registration.is_none());
361        assert_eq!(
362            result.receipt.voxel.resolved_policy(),
363            spatialrust_core::ExecutionPolicy::CpuSingle
364        );
365        assert_eq!(result.receipt.normals.stages(), &["normal-estimation"]);
366        assert_eq!(result.receipt.plane.stages(), &["plane-segmentation"]);
367        assert_eq!(result.receipt.clusters.stages(), &["euclidean-clustering"]);
368        assert_eq!(result.receipt.host_to_device_bytes(), 0);
369    }
370
371    #[test]
372    fn runs_optional_icp_step() {
373        let pipeline = MvpPipeline::new(MvpPipelineConfig {
374            voxel: spatialrust_filtering::VoxelGridDownsampleConfig::centroid(0.2),
375            normals: spatialrust_features::NormalEstimationConfig {
376                k_neighbors: 8,
377                min_neighbors: 3,
378                viewpoint: Some(Vec3::new(0.0, 0.0, 10.0)),
379                ..Default::default()
380            },
381            plane: RansacPlaneConfig {
382                distance_threshold: 0.05,
383                max_iterations: 500,
384                min_inliers: 10,
385                seed: 17,
386                ..Default::default()
387            },
388            cluster: EuclideanClusterConfig {
389                cluster_tolerance: 0.3,
390                min_cluster_size: 1,
391                max_cluster_size: usize::MAX,
392                ..Default::default()
393            },
394            icp: Some(MvpIcpConfig {
395                icp: IcpConfig {
396                    max_correspondence_distance: 0.2,
397                    max_iterations: 30,
398                    ..Default::default()
399                },
400                source_transform: Some(Isometry3::new(
401                    Quat::<f32>::identity(),
402                    Vec3::new(0.03, -0.01, 0.0),
403                )),
404                ..Default::default()
405            }),
406            ..Default::default()
407        });
408
409        let result = pipeline.run(&sample_cloud()).unwrap();
410        let registration = result.registration.expect("expected icp result");
411        assert!(registration.converged);
412    }
413
414    /// Three perpendicular faces, giving point-to-plane/GICP full 6-DoF constraint.
415    fn corner_cloud() -> spatialrust_core::PointCloud {
416        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
417        for i in 0..12 {
418            for j in 0..12 {
419                let (a, b) = (i as f32 * 0.06, j as f32 * 0.06);
420                builder.push_point([a, b, 0.0]).unwrap();
421                builder.push_point([a, 0.0, b + 0.03]).unwrap();
422                builder.push_point([0.0, a + 0.03, b + 0.03]).unwrap();
423            }
424        }
425        builder.build().unwrap()
426    }
427
428    fn run_with_method(method: super::MvpRegistrationMethod) -> super::RegistrationResult {
429        let pipeline = MvpPipeline::new(MvpPipelineConfig {
430            voxel: spatialrust_filtering::VoxelGridDownsampleConfig::centroid(0.05),
431            normals: spatialrust_features::NormalEstimationConfig {
432                k_neighbors: 10,
433                min_neighbors: 3,
434                ..Default::default()
435            },
436            plane: RansacPlaneConfig {
437                distance_threshold: 0.02,
438                max_iterations: 500,
439                min_inliers: 10,
440                seed: 17,
441                ..Default::default()
442            },
443            cluster: EuclideanClusterConfig {
444                cluster_tolerance: 0.3,
445                min_cluster_size: 1,
446                max_cluster_size: usize::MAX,
447                ..Default::default()
448            },
449            icp: Some(MvpIcpConfig {
450                icp: IcpConfig {
451                    max_correspondence_distance: 0.3,
452                    max_iterations: 40,
453                    min_correspondences: 6,
454                    ..Default::default()
455                },
456                source_transform: Some(Isometry3::new(
457                    Quat::from_axis_angle(Vec3::new(0.0, 0.0, 1.0), 0.05),
458                    Vec3::new(0.01, -0.008, 0.012),
459                )),
460                method,
461            }),
462            ..Default::default()
463        });
464        pipeline.run(&corner_cloud()).unwrap().registration.expect("expected registration")
465    }
466
467    #[test]
468    fn runs_point_to_plane_registration() {
469        let result = run_with_method(super::MvpRegistrationMethod::PointToPlane);
470        assert!(result.fitness.is_finite());
471        assert!(result.fitness < 1e-2, "fitness too high: {}", result.fitness);
472    }
473
474    #[test]
475    fn runs_gicp_registration() {
476        let result = run_with_method(super::MvpRegistrationMethod::Gicp);
477        assert!(result.fitness.is_finite());
478        assert!(result.fitness < 1e-1, "fitness too high: {}", result.fitness);
479    }
480
481    #[cfg(feature = "pipeline-mvp-gpu")]
482    #[test]
483    fn runs_with_gpu_voxel_policy() {
484        use spatialrust_core::{DeviceKind, ExecutionPolicy};
485
486        let pipeline = MvpPipeline::new(MvpPipelineConfig {
487            voxel: spatialrust_filtering::VoxelGridDownsampleConfig::centroid(0.2),
488            voxel_policy: ExecutionPolicy::Gpu(DeviceKind::Wgpu),
489            ..Default::default()
490        });
491
492        let result = pipeline.run(&sample_cloud()).unwrap();
493        assert!(!result.downsampled.is_empty());
494        assert_eq!(result.receipt.voxel.resolved_policy(), ExecutionPolicy::Gpu(DeviceKind::Wgpu));
495        assert!(result.receipt.voxel.host_to_device_bytes() > 0);
496        assert!(result.receipt.voxel.device_to_host_bytes() > 0);
497    }
498
499    #[cfg(feature = "pipeline-mvp-gpu")]
500    #[test]
501    fn runs_with_gpu_plane_policy() {
502        use spatialrust_core::{DeviceKind, ExecutionPolicy};
503
504        let pipeline = MvpPipeline::new(MvpPipelineConfig {
505            plane_policy: ExecutionPolicy::Gpu(DeviceKind::Wgpu),
506            ..Default::default()
507        });
508
509        let result = pipeline.run(&sample_cloud()).unwrap();
510        assert!(result.plane.inlier_count >= 10);
511        assert_eq!(result.receipt.plane.resolved_policy(), ExecutionPolicy::Gpu(DeviceKind::Wgpu));
512        assert!(result.receipt.plane.device_to_host_bytes() > 0);
513    }
514
515    #[cfg(feature = "pipeline-mvp-gpu")]
516    #[test]
517    fn runs_with_gpu_normal_policy() {
518        use spatialrust_core::{DeviceKind, ExecutionPolicy};
519
520        let pipeline = MvpPipeline::new(MvpPipelineConfig {
521            normal_policy: ExecutionPolicy::Gpu(DeviceKind::Wgpu),
522            ..Default::default()
523        });
524
525        let result = pipeline.run(&sample_cloud()).unwrap();
526        assert!(result.with_normals.field("normal_x").is_ok());
527        assert_eq!(
528            result.receipt.normals.resolved_policy(),
529            ExecutionPolicy::Gpu(DeviceKind::Wgpu)
530        );
531        assert!(result.receipt.normals.device_to_host_bytes() > 0);
532    }
533
534    #[cfg(feature = "pipeline-mvp-gpu")]
535    #[test]
536    fn auto_normal_policy_derives_gpu_radius_from_voxel_leaf() {
537        use spatialrust_core::ExecutionPolicy;
538        use spatialrust_features::NormalEstimationConfig;
539
540        let pipeline = MvpPipeline::new(MvpPipelineConfig {
541            voxel: spatialrust_filtering::VoxelGridDownsampleConfig::centroid(0.05),
542            normals: NormalEstimationConfig { search_radius: None, ..Default::default() },
543            normal_policy: ExecutionPolicy::Auto,
544            normal_gpu_radius_scale: 2.0,
545            ..Default::default()
546        });
547
548        let result = pipeline.run(&sample_cloud()).unwrap();
549        assert!(result.with_normals.field("normal_x").is_ok());
550    }
551
552    #[cfg(feature = "pipeline-mvp-gpu")]
553    #[test]
554    fn runs_with_gpu_cluster_policy() {
555        use spatialrust_core::{DeviceKind, ExecutionPolicy};
556
557        let pipeline = MvpPipeline::new(MvpPipelineConfig {
558            cluster_policy: ExecutionPolicy::Gpu(DeviceKind::Wgpu),
559            ..Default::default()
560        });
561
562        let result = pipeline.run(&sample_cloud()).unwrap();
563        assert!(result.clusters.cluster_count >= 1);
564        assert_eq!(
565            result.receipt.clusters.resolved_policy(),
566            ExecutionPolicy::Gpu(DeviceKind::Wgpu)
567        );
568        assert!(result.receipt.clusters.host_to_device_bytes() > 0);
569        assert!(result.receipt.clusters.device_to_host_bytes() > 0);
570    }
571}
spatialrust_pipeline/mvp.rs

spatialrust_pipeline/
mvp.rs
