ruvector_robotics/perception/
scene_graph.rs

1//! Scene-graph construction from point clouds and object lists.
2//!
3//! The [`SceneGraphBuilder`] turns raw sensor data into a structured
4//! [`SceneGraph`] of objects and spatial relationships.
5
6use std::collections::HashMap;
7
8use crate::bridge::{Point3D, PointCloud, SceneEdge, SceneGraph, SceneObject};
9use crate::perception::clustering;
10use crate::perception::config::SceneGraphConfig;
11
12// ---------------------------------------------------------------------------
13// Builder
14// ---------------------------------------------------------------------------
15
16/// Builds [`SceneGraph`] instances from point clouds or pre-classified
17/// object lists using spatial-hash clustering with union-find.
18#[derive(Debug, Clone)]
19pub struct PointCloudSceneGraphBuilder {
20    config: SceneGraphConfig,
21}
22
23impl PointCloudSceneGraphBuilder {
24    /// Create a new builder with the given configuration.
25    pub fn new(config: SceneGraphConfig) -> Self {
26        Self { config }
27    }
28
29    /// Build a scene graph by clustering a raw point cloud.
30    ///
31    /// 1. Points are discretised into a spatial hash grid.
32    /// 2. Adjacent cells are merged via union-find.
33    /// 3. Each cluster above `min_cluster_size` becomes a `SceneObject`.
34    /// 4. Edges are created between objects whose centres are within
35    ///    `edge_distance_threshold`.
36    pub fn build_from_point_cloud(&self, cloud: &PointCloud) -> SceneGraph {
37        if cloud.is_empty() {
38            return SceneGraph::default();
39        }
40
41        let clusters = clustering::cluster_point_cloud(cloud, self.config.cluster_radius);
42
43        // Convert clusters to SceneObjects (cap at max_objects).
44        let mut objects: Vec<SceneObject> = clusters
45            .into_iter()
46            .filter(|pts| pts.len() >= self.config.min_cluster_size)
47            .take(self.config.max_objects)
48            .enumerate()
49            .map(|(id, pts)| Self::cluster_to_object(id, &pts))
50            .collect();
51
52        objects.sort_by(|a, b| a.id.cmp(&b.id));
53        let edges = self.create_edges(&objects);
54
55        SceneGraph::new(objects, edges, cloud.timestamp_us)
56    }
57
58    /// Build a scene graph from a pre-existing list of objects.
59    ///
60    /// Edges are created between objects whose centres are within
61    /// `edge_distance_threshold`.
62    pub fn build_from_objects(&self, objects: &[SceneObject]) -> SceneGraph {
63        let objects_vec: Vec<SceneObject> = objects
64            .iter()
65            .take(self.config.max_objects)
66            .cloned()
67            .collect();
68
69        let edges = self.create_edges(&objects_vec);
70        SceneGraph::new(objects_vec, edges, 0)
71    }
72
73    /// Merge multiple scene graphs into one, deduplicating objects that share
74    /// the same `id`.
75    pub fn merge_scenes(&self, scenes: &[SceneGraph]) -> SceneGraph {
76        let mut seen_ids: HashMap<usize, SceneObject> = HashMap::new();
77        let mut latest_ts: i64 = 0;
78
79        for scene in scenes {
80            latest_ts = latest_ts.max(scene.timestamp);
81            for obj in &scene.objects {
82                // Keep the first occurrence of each id.
83                seen_ids.entry(obj.id).or_insert_with(|| obj.clone());
84            }
85        }
86
87        let mut objects: Vec<SceneObject> = seen_ids.into_values().collect();
88        objects.sort_by(|a, b| a.id.cmp(&b.id));
89
90        let truncated: Vec<SceneObject> = objects
91            .into_iter()
92            .take(self.config.max_objects)
93            .collect();
94
95        let edges = self.create_edges(&truncated);
96        SceneGraph::new(truncated, edges, latest_ts)
97    }
98
99    // -- private helpers ----------------------------------------------------
100
101    fn cluster_to_object(id: usize, points: &[Point3D]) -> SceneObject {
102        debug_assert!(!points.is_empty(), "cluster_to_object called with empty slice");
103        let (mut min_x, mut min_y, mut min_z) = (f64::MAX, f64::MAX, f64::MAX);
104        let (mut max_x, mut max_y, mut max_z) = (f64::MIN, f64::MIN, f64::MIN);
105        let (mut sum_x, mut sum_y, mut sum_z) = (0.0_f64, 0.0_f64, 0.0_f64);
106
107        for p in points {
108            let (px, py, pz) = (p.x as f64, p.y as f64, p.z as f64);
109            min_x = min_x.min(px);
110            min_y = min_y.min(py);
111            min_z = min_z.min(pz);
112            max_x = max_x.max(px);
113            max_y = max_y.max(py);
114            max_z = max_z.max(pz);
115            sum_x += px;
116            sum_y += py;
117            sum_z += pz;
118        }
119
120        let n = points.len() as f64;
121        let center = [sum_x / n, sum_y / n, sum_z / n];
122        let extent = [
123            (max_x - min_x) / 2.0,
124            (max_y - min_y) / 2.0,
125            (max_z - min_z) / 2.0,
126        ];
127
128        SceneObject {
129            id,
130            center,
131            extent,
132            confidence: 1.0,
133            label: format!("cluster_{}", id),
134            velocity: None,
135        }
136    }
137
138    fn create_edges(&self, objects: &[SceneObject]) -> Vec<SceneEdge> {
139        let mut edges = Vec::new();
140        let threshold = self.config.edge_distance_threshold;
141
142        for i in 0..objects.len() {
143            for j in (i + 1)..objects.len() {
144                let d = Self::distance_3d(&objects[i].center, &objects[j].center);
145                if d <= threshold {
146                    let relation = if d < threshold * 0.33 {
147                        "adjacent"
148                    } else if d < threshold * 0.66 {
149                        "near"
150                    } else {
151                        "far"
152                    };
153                    edges.push(SceneEdge {
154                        from: objects[i].id,
155                        to: objects[j].id,
156                        distance: d,
157                        relation: relation.to_string(),
158                    });
159                }
160            }
161        }
162
163        edges
164    }
165
166    fn distance_3d(a: &[f64; 3], b: &[f64; 3]) -> f64 {
167        ((a[0] - b[0]).powi(2) + (a[1] - b[1]).powi(2) + (a[2] - b[2]).powi(2)).sqrt()
168    }
169}
170
171// ---------------------------------------------------------------------------
172// Tests
173// ---------------------------------------------------------------------------
174
175#[cfg(test)]
176mod tests {
177    use super::*;
178
179    fn make_cloud(raw: &[[f32; 3]]) -> PointCloud {
180        let points: Vec<Point3D> = raw.iter().map(|p| Point3D::new(p[0], p[1], p[2])).collect();
181        PointCloud::new(points, 1000)
182    }
183
184    #[test]
185    fn test_empty_cloud() {
186        let builder = PointCloudSceneGraphBuilder::new(SceneGraphConfig::default());
187        let graph = builder.build_from_point_cloud(&PointCloud::default());
188        assert!(graph.objects.is_empty());
189        assert!(graph.edges.is_empty());
190    }
191
192    #[test]
193    fn test_single_cluster() {
194        let builder = PointCloudSceneGraphBuilder::new(SceneGraphConfig {
195            cluster_radius: 1.0,
196            min_cluster_size: 3,
197            max_objects: 10,
198            edge_distance_threshold: 5.0,
199        });
200        let cloud = make_cloud(&[
201            [0.0, 0.0, 0.0],
202            [0.1, 0.0, 0.0],
203            [0.0, 0.1, 0.0],
204            [0.1, 0.1, 0.0],
205        ]);
206        let graph = builder.build_from_point_cloud(&cloud);
207        assert_eq!(graph.objects.len(), 1);
208        assert!(graph.edges.is_empty()); // Only one object, no edges.
209    }
210
211    #[test]
212    fn test_room_point_cloud() {
213        // Simulate a room with two walls (clusters far apart).
214        let builder = PointCloudSceneGraphBuilder::new(SceneGraphConfig {
215            cluster_radius: 0.5,
216            min_cluster_size: 3,
217            max_objects: 10,
218            edge_distance_threshold: 50.0,
219        });
220
221        let mut points = Vec::new();
222        // Wall 1: cluster around (0, 0, 0)
223        for i in 0..5 {
224            points.push([i as f32 * 0.1, 0.0, 0.0]);
225        }
226        // Wall 2: cluster around (10, 0, 0)
227        for i in 0..5 {
228            points.push([10.0 + i as f32 * 0.1, 0.0, 0.0]);
229        }
230
231        let cloud = make_cloud(&points);
232        let graph = builder.build_from_point_cloud(&cloud);
233        assert_eq!(graph.objects.len(), 2);
234        // Both walls should be connected since threshold is 50.
235        assert!(!graph.edges.is_empty());
236    }
237
238    #[test]
239    fn test_separated_clusters_no_edge() {
240        let builder = PointCloudSceneGraphBuilder::new(SceneGraphConfig {
241            cluster_radius: 0.5,
242            min_cluster_size: 3,
243            max_objects: 10,
244            edge_distance_threshold: 2.0,
245        });
246
247        let cloud = make_cloud(&[
248            // Cluster A
249            [0.0, 0.0, 0.0],
250            [0.1, 0.0, 0.0],
251            [0.0, 0.1, 0.0],
252            // Cluster B -- far away (100 units)
253            [100.0, 0.0, 0.0],
254            [100.1, 0.0, 0.0],
255            [100.0, 0.1, 0.0],
256        ]);
257
258        let graph = builder.build_from_point_cloud(&cloud);
259        assert_eq!(graph.objects.len(), 2);
260        // Should NOT have edges -- clusters are 100 units apart, threshold is 2.
261        assert!(graph.edges.is_empty());
262    }
263
264    #[test]
265    fn test_build_from_objects() {
266        let builder = PointCloudSceneGraphBuilder::new(SceneGraphConfig {
267            edge_distance_threshold: 5.0,
268            ..SceneGraphConfig::default()
269        });
270
271        let objects = vec![
272            SceneObject::new(0, [0.0, 0.0, 0.0], [1.0, 1.0, 1.0]),
273            SceneObject::new(1, [3.0, 0.0, 0.0], [1.0, 1.0, 1.0]),
274            SceneObject::new(2, [100.0, 0.0, 0.0], [1.0, 1.0, 1.0]),
275        ];
276
277        let graph = builder.build_from_objects(&objects);
278        assert_eq!(graph.objects.len(), 3);
279
280        // Objects 0 and 1 are 3.0 apart (within threshold),
281        // object 2 is 100.0 away (outside threshold).
282        assert_eq!(graph.edges.len(), 1);
283        assert_eq!(graph.edges[0].from, 0);
284        assert_eq!(graph.edges[0].to, 1);
285    }
286
287    #[test]
288    fn test_merge_deduplication() {
289        let builder = PointCloudSceneGraphBuilder::new(SceneGraphConfig {
290            edge_distance_threshold: 10.0,
291            ..SceneGraphConfig::default()
292        });
293
294        let scene_a = SceneGraph::new(
295            vec![
296                SceneObject::new(0, [0.0, 0.0, 0.0], [1.0, 1.0, 1.0]),
297                SceneObject::new(1, [2.0, 0.0, 0.0], [1.0, 1.0, 1.0]),
298            ],
299            vec![],
300            100,
301        );
302
303        let scene_b = SceneGraph::new(
304            vec![
305                SceneObject::new(1, [2.0, 0.0, 0.0], [1.0, 1.0, 1.0]), // duplicate id
306                SceneObject::new(2, [4.0, 0.0, 0.0], [1.0, 1.0, 1.0]),
307            ],
308            vec![],
309            200,
310        );
311
312        let merged = builder.merge_scenes(&[scene_a, scene_b]);
313        // Should have 3 unique objects: ids 0, 1, 2.
314        assert_eq!(merged.objects.len(), 3);
315        assert_eq!(merged.timestamp, 200);
316    }
317
318    #[test]
319    fn test_merge_preserves_latest_timestamp() {
320        let builder = PointCloudSceneGraphBuilder::new(SceneGraphConfig::default());
321        let s1 = SceneGraph::new(vec![], vec![], 50);
322        let s2 = SceneGraph::new(vec![], vec![], 300);
323        let s3 = SceneGraph::new(vec![], vec![], 100);
324        let merged = builder.merge_scenes(&[s1, s2, s3]);
325        assert_eq!(merged.timestamp, 300);
326    }
327
328    #[test]
329    fn test_edge_relations() {
330        let builder = PointCloudSceneGraphBuilder::new(SceneGraphConfig {
331            edge_distance_threshold: 30.0,
332            ..SceneGraphConfig::default()
333        });
334
335        let objects = vec![
336            SceneObject::new(0, [0.0, 0.0, 0.0], [1.0, 1.0, 1.0]),
337            SceneObject::new(1, [5.0, 0.0, 0.0], [1.0, 1.0, 1.0]),  // ~5 < 9.9 => adjacent
338            SceneObject::new(2, [15.0, 0.0, 0.0], [1.0, 1.0, 1.0]), // ~15 < 19.8 => near
339            SceneObject::new(3, [25.0, 0.0, 0.0], [1.0, 1.0, 1.0]), // ~25 < 30 => far
340        ];
341
342        let graph = builder.build_from_objects(&objects);
343
344        // Check that adjacent relation exists for objects 0 and 1.
345        let edge_0_1 = graph
346            .edges
347            .iter()
348            .find(|e| e.from == 0 && e.to == 1);
349        assert!(edge_0_1.is_some());
350        assert_eq!(edge_0_1.unwrap().relation, "adjacent");
351    }
352
353    #[test]
354    fn test_max_objects_cap() {
355        let builder = PointCloudSceneGraphBuilder::new(SceneGraphConfig {
356            cluster_radius: 0.5,
357            min_cluster_size: 1,
358            max_objects: 2,
359            edge_distance_threshold: 100.0,
360        });
361
362        let cloud = make_cloud(&[
363            [0.0, 0.0, 0.0],
364            [50.0, 0.0, 0.0],
365            [100.0, 0.0, 0.0],
366        ]);
367
368        let graph = builder.build_from_point_cloud(&cloud);
369        // min_cluster_size=1, so each point is its own cluster.
370        // max_objects=2, so at most 2 objects.
371        assert!(graph.objects.len() <= 2);
372    }
373}
ruvector_robotics/perception/scene_graph.rs

ruvector_robotics/perception/
scene_graph.rs
