ruvector_robotics/bridge/
converters.rs

1//! Type conversion functions between robotics domain types and flat vector
2//! representations suitable for indexing, serialization, or ML inference.
3
4use crate::bridge::{OccupancyGrid, Point3D, PointCloud, Pose, RobotState, SceneGraph};
5
6// Quaternion is used in tests for constructing Pose values.
7#[cfg(test)]
8use crate::bridge::Quaternion;
9
10use std::fmt;
11
12/// Errors that can occur during type conversions.
13#[derive(Debug, Clone, PartialEq, Eq)]
14pub enum ConversionError {
15    /// The input vector length does not match the expected dimensionality.
16    LengthMismatch { expected: usize, got: usize },
17    /// The input collection was empty when a non-empty one was required.
18    EmptyInput,
19}
20
21impl fmt::Display for ConversionError {
22    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
23        match self {
24            Self::LengthMismatch { expected, got } => {
25                write!(f, "length mismatch: expected {expected}, got {got}")
26            }
27            Self::EmptyInput => write!(f, "empty input"),
28        }
29    }
30}
31
32impl std::error::Error for ConversionError {}
33
34/// Convert a [`PointCloud`] to a `Vec` of `[x, y, z]` vectors.
35pub fn point_cloud_to_vectors(cloud: &PointCloud) -> Vec<Vec<f32>> {
36    cloud.points.iter().map(|p| vec![p.x, p.y, p.z]).collect()
37}
38
39/// Convert a [`PointCloud`] to `[x, y, z, intensity]` vectors.
40///
41/// Returns [`ConversionError::LengthMismatch`] when the intensity array length
42/// does not match the number of points.
43pub fn point_cloud_to_vectors_with_intensity(
44    cloud: &PointCloud,
45) -> Result<Vec<Vec<f32>>, ConversionError> {
46    if cloud.points.len() != cloud.intensities.len() {
47        return Err(ConversionError::LengthMismatch {
48            expected: cloud.points.len(),
49            got: cloud.intensities.len(),
50        });
51    }
52    Ok(cloud
53        .points
54        .iter()
55        .zip(cloud.intensities.iter())
56        .map(|(p, &i)| vec![p.x, p.y, p.z, i])
57        .collect())
58}
59
60/// Reconstruct a [`PointCloud`] from `[x, y, z]` vectors.
61///
62/// Each inner vector **must** have exactly 3 elements.
63pub fn vectors_to_point_cloud(
64    vectors: &[Vec<f32>],
65    timestamp: i64,
66) -> Result<PointCloud, ConversionError> {
67    if vectors.is_empty() {
68        return Err(ConversionError::EmptyInput);
69    }
70    let mut points = Vec::with_capacity(vectors.len());
71    for v in vectors {
72        if v.len() != 3 {
73            return Err(ConversionError::LengthMismatch {
74                expected: 3,
75                got: v.len(),
76            });
77        }
78        points.push(Point3D::new(v[0], v[1], v[2]));
79    }
80    Ok(PointCloud::new(points, timestamp))
81}
82
83/// Flatten a [`RobotState`] into `[px, py, pz, vx, vy, vz, ax, ay, az]`.
84pub fn robot_state_to_vector(state: &RobotState) -> Vec<f64> {
85    let mut v = Vec::with_capacity(9);
86    v.extend_from_slice(&state.position);
87    v.extend_from_slice(&state.velocity);
88    v.extend_from_slice(&state.acceleration);
89    v
90}
91
92/// Reconstruct a [`RobotState`] from a 9-element vector and a timestamp.
93pub fn vector_to_robot_state(
94    v: &[f64],
95    timestamp: i64,
96) -> Result<RobotState, ConversionError> {
97    if v.len() != 9 {
98        return Err(ConversionError::LengthMismatch {
99            expected: 9,
100            got: v.len(),
101        });
102    }
103    Ok(RobotState {
104        position: [v[0], v[1], v[2]],
105        velocity: [v[3], v[4], v[5]],
106        acceleration: [v[6], v[7], v[8]],
107        timestamp_us: timestamp,
108    })
109}
110
111/// Flatten a [`Pose`] into `[px, py, pz, qx, qy, qz, qw]`.
112pub fn pose_to_vector(pose: &Pose) -> Vec<f64> {
113    vec![
114        pose.position[0],
115        pose.position[1],
116        pose.position[2],
117        pose.orientation.x,
118        pose.orientation.y,
119        pose.orientation.z,
120        pose.orientation.w,
121    ]
122}
123
124/// Extract occupied cells (value > 0.5) as `[world_x, world_y, value]` vectors.
125pub fn occupancy_grid_to_vectors(grid: &OccupancyGrid) -> Vec<Vec<f32>> {
126    let mut result = Vec::new();
127    for y in 0..grid.height {
128        for x in 0..grid.width {
129            let val = grid.get(x, y).unwrap_or(0.0);
130            if val > 0.5 {
131                let wx = grid.origin[0] as f32 + x as f32 * grid.resolution as f32;
132                let wy = grid.origin[1] as f32 + y as f32 * grid.resolution as f32;
133                result.push(vec![wx, wy, val]);
134            }
135        }
136    }
137    result
138}
139
140/// Convert a [`SceneGraph`] into node feature vectors and an edge list.
141///
142/// Each node vector is `[cx, cy, cz, ex, ey, ez, confidence]`.
143/// Each edge tuple is `(from_index, to_index, distance)`.
144type NodeFeatures = Vec<Vec<f64>>;
145type EdgeList = Vec<(usize, usize, f64)>;
146
147pub fn scene_graph_to_adjacency(
148    scene: &SceneGraph,
149) -> (NodeFeatures, EdgeList) {
150    let nodes: Vec<Vec<f64>> = scene
151        .objects
152        .iter()
153        .map(|o| {
154            vec![
155                o.center[0],
156                o.center[1],
157                o.center[2],
158                o.extent[0],
159                o.extent[1],
160                o.extent[2],
161                o.confidence as f64,
162            ]
163        })
164        .collect();
165
166    let edges: Vec<(usize, usize, f64)> = scene
167        .edges
168        .iter()
169        .map(|e| (e.from, e.to, e.distance))
170        .collect();
171
172    (nodes, edges)
173}
174
175#[cfg(test)]
176mod tests {
177    use super::*;
178    use crate::bridge::{OccupancyGrid, SceneEdge, SceneObject};
179
180    #[test]
181    fn test_point_cloud_to_vectors_basic() {
182        let cloud = PointCloud::new(
183            vec![Point3D::new(1.0, 2.0, 3.0), Point3D::new(4.0, 5.0, 6.0)],
184            100,
185        );
186        let vecs = point_cloud_to_vectors(&cloud);
187        assert_eq!(vecs.len(), 2);
188        assert_eq!(vecs[0], vec![1.0, 2.0, 3.0]);
189        assert_eq!(vecs[1], vec![4.0, 5.0, 6.0]);
190    }
191
192    #[test]
193    fn test_point_cloud_to_vectors_empty() {
194        let cloud = PointCloud::default();
195        let vecs = point_cloud_to_vectors(&cloud);
196        assert!(vecs.is_empty());
197    }
198
199    #[test]
200    fn test_point_cloud_with_intensity_ok() {
201        let cloud = PointCloud::new(vec![Point3D::new(1.0, 2.0, 3.0)], 0);
202        let vecs = point_cloud_to_vectors_with_intensity(&cloud).unwrap();
203        assert_eq!(vecs[0], vec![1.0, 2.0, 3.0, 1.0]);
204    }
205
206    #[test]
207    fn test_point_cloud_with_intensity_mismatch() {
208        let mut cloud = PointCloud::new(vec![Point3D::new(1.0, 2.0, 3.0)], 0);
209        cloud.intensities = vec![];
210        let err = point_cloud_to_vectors_with_intensity(&cloud).unwrap_err();
211        assert_eq!(err, ConversionError::LengthMismatch { expected: 1, got: 0 });
212    }
213
214    #[test]
215    fn test_vectors_to_point_cloud_ok() {
216        let vecs = vec![vec![1.0, 2.0, 3.0], vec![4.0, 5.0, 6.0]];
217        let cloud = vectors_to_point_cloud(&vecs, 42).unwrap();
218        assert_eq!(cloud.len(), 2);
219        assert_eq!(cloud.timestamp(), 42);
220        assert_eq!(cloud.points[0].x, 1.0);
221    }
222
223    #[test]
224    fn test_vectors_to_point_cloud_empty() {
225        let vecs: Vec<Vec<f32>> = vec![];
226        let err = vectors_to_point_cloud(&vecs, 0).unwrap_err();
227        assert_eq!(err, ConversionError::EmptyInput);
228    }
229
230    #[test]
231    fn test_vectors_to_point_cloud_wrong_dim() {
232        let vecs = vec![vec![1.0, 2.0]];
233        let err = vectors_to_point_cloud(&vecs, 0).unwrap_err();
234        assert_eq!(err, ConversionError::LengthMismatch { expected: 3, got: 2 });
235    }
236
237    #[test]
238    fn test_point_cloud_roundtrip() {
239        let pts = vec![Point3D::new(1.0, 2.0, 3.0), Point3D::new(-1.0, 0.0, 5.5)];
240        let original = PointCloud::new(pts, 999);
241        let vecs = point_cloud_to_vectors(&original);
242        let restored = vectors_to_point_cloud(&vecs, 999).unwrap();
243        assert_eq!(restored.len(), original.len());
244    }
245
246    #[test]
247    fn test_robot_state_to_vector() {
248        let state = RobotState {
249            position: [1.0, 2.0, 3.0],
250            velocity: [4.0, 5.0, 6.0],
251            acceleration: [7.0, 8.0, 9.0],
252            timestamp_us: 0,
253        };
254        let v = robot_state_to_vector(&state);
255        assert_eq!(v, vec![1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0]);
256    }
257
258    #[test]
259    fn test_vector_to_robot_state_ok() {
260        let v = vec![1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0];
261        let state = vector_to_robot_state(&v, 123).unwrap();
262        assert_eq!(state.position, [1.0, 2.0, 3.0]);
263        assert_eq!(state.velocity, [4.0, 5.0, 6.0]);
264        assert_eq!(state.acceleration, [7.0, 8.0, 9.0]);
265        assert_eq!(state.timestamp_us, 123);
266    }
267
268    #[test]
269    fn test_vector_to_robot_state_wrong_len() {
270        let v = vec![1.0, 2.0, 3.0];
271        let err = vector_to_robot_state(&v, 0).unwrap_err();
272        assert_eq!(err, ConversionError::LengthMismatch { expected: 9, got: 3 });
273    }
274
275    #[test]
276    fn test_robot_state_roundtrip() {
277        let original = RobotState {
278            position: [10.0, 20.0, 30.0],
279            velocity: [-1.0, -2.0, -3.0],
280            acceleration: [0.1, 0.2, 0.3],
281            timestamp_us: 555,
282        };
283        let v = robot_state_to_vector(&original);
284        let restored = vector_to_robot_state(&v, 555).unwrap();
285        assert_eq!(original, restored);
286    }
287
288    #[test]
289    fn test_pose_to_vector() {
290        let pose = Pose {
291            position: [1.0, 2.0, 3.0],
292            orientation: Quaternion::new(0.1, 0.2, 0.3, 0.9),
293            frame_id: "map".into(),
294        };
295        let v = pose_to_vector(&pose);
296        assert_eq!(v.len(), 7);
297        assert!((v[0] - 1.0).abs() < f64::EPSILON);
298        assert!((v[3] - 0.1).abs() < f64::EPSILON);
299        assert!((v[6] - 0.9).abs() < f64::EPSILON);
300    }
301
302    #[test]
303    fn test_pose_to_vector_identity() {
304        let pose = Pose::default();
305        let v = pose_to_vector(&pose);
306        assert_eq!(v, vec![0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]);
307    }
308
309    #[test]
310    fn test_occupancy_grid_to_vectors_no_occupied() {
311        let grid = OccupancyGrid::new(5, 5, 0.1);
312        let vecs = occupancy_grid_to_vectors(&grid);
313        assert!(vecs.is_empty());
314    }
315
316    #[test]
317    fn test_occupancy_grid_to_vectors_some_occupied() {
318        let mut grid = OccupancyGrid::new(3, 3, 0.5);
319        grid.set(1, 2, 0.9);
320        grid.set(0, 0, 0.3); // below threshold
321        let vecs = occupancy_grid_to_vectors(&grid);
322        assert_eq!(vecs.len(), 1);
323        let v = &vecs[0];
324        assert!((v[2] - 0.9).abs() < f32::EPSILON);
325    }
326
327    #[test]
328    fn test_occupancy_grid_with_origin() {
329        let mut grid = OccupancyGrid::new(2, 2, 1.0);
330        grid.origin = [10.0, 20.0, 0.0];
331        grid.set(1, 0, 0.8);
332        let vecs = occupancy_grid_to_vectors(&grid);
333        assert_eq!(vecs.len(), 1);
334        assert!((vecs[0][0] - 11.0).abs() < f32::EPSILON); // wx = 10 + 1*1
335        assert!((vecs[0][1] - 20.0).abs() < f32::EPSILON); // wy = 20 + 0*1
336    }
337
338    #[test]
339    fn test_scene_graph_to_adjacency_empty() {
340        let scene = SceneGraph::default();
341        let (nodes, edges) = scene_graph_to_adjacency(&scene);
342        assert!(nodes.is_empty());
343        assert!(edges.is_empty());
344    }
345
346    #[test]
347    fn test_scene_graph_to_adjacency() {
348        let o1 = SceneObject::new(0, [1.0, 2.0, 3.0], [0.5, 0.5, 0.5]);
349        let o2 = SceneObject::new(1, [4.0, 5.0, 6.0], [1.0, 1.0, 1.0]);
350        let edge = SceneEdge {
351            from: 0,
352            to: 1,
353            distance: 5.196,
354            relation: "near".into(),
355        };
356        let scene = SceneGraph::new(vec![o1, o2], vec![edge], 0);
357        let (nodes, edges) = scene_graph_to_adjacency(&scene);
358
359        assert_eq!(nodes.len(), 2);
360        assert_eq!(nodes[0].len(), 7);
361        assert!((nodes[0][0] - 1.0).abs() < f64::EPSILON);
362        assert!((nodes[0][6] - 1.0).abs() < f64::EPSILON); // confidence
363
364        assert_eq!(edges.len(), 1);
365        assert_eq!(edges[0].0, 0);
366        assert_eq!(edges[0].1, 1);
367    }
368
369    #[test]
370    fn test_point_cloud_10k_roundtrip() {
371        let points: Vec<Point3D> = (0..10_000)
372            .map(|i| {
373                let f = i as f32;
374                Point3D::new(f * 0.1, f * 0.2, f * 0.3)
375            })
376            .collect();
377        let cloud = PointCloud::new(points, 1_000_000);
378        let vecs = point_cloud_to_vectors(&cloud);
379        assert_eq!(vecs.len(), 10_000);
380        let restored = vectors_to_point_cloud(&vecs, 1_000_000).unwrap();
381        assert_eq!(restored.len(), 10_000);
382    }
383
384    #[test]
385    fn test_conversion_error_display() {
386        let e1 = ConversionError::LengthMismatch { expected: 3, got: 5 };
387        assert!(format!("{e1}").contains("3") && format!("{e1}").contains("5"));
388        assert!(format!("{}", ConversionError::EmptyInput).contains("empty"));
389    }
390}
ruvector_robotics/bridge/converters.rs

ruvector_robotics/bridge/
converters.rs
