spatialrust_voxelize/
range_image.rs

1//! Spherical range-image projection for rotating-LiDAR scans.
2//!
3//! Projects a 3D scan into the dense 2D range image that learned LiDAR models
4//! consume (RangeNet++, SqueezeSeg): rows span the sensor's vertical field of
5//! view, columns span azimuth, and each pixel holds the nearest point's range.
6
7use spatialrust_core::{HasPositions3, PointCloud, SpatialError, SpatialResult};
8
9/// Configuration for [`range_image`].
10#[derive(Clone, Copy, Debug, PartialEq)]
11pub struct RangeImageConfig {
12    /// Image width in pixels (azimuth resolution).
13    pub width: usize,
14    /// Image height in pixels (one row per laser beam).
15    pub height: usize,
16    /// Upper bound of the vertical field of view, in degrees.
17    pub fov_up_deg: f32,
18    /// Lower bound of the vertical field of view, in degrees (usually negative).
19    pub fov_down_deg: f32,
20}
21
22impl Default for RangeImageConfig {
23    fn default() -> Self {
24        // Velodyne HDL-64E-like vertical FOV.
25        Self { width: 1024, height: 64, fov_up_deg: 3.0, fov_down_deg: -25.0 }
26    }
27}
28
29impl RangeImageConfig {
30    /// Creates a config from the image dimensions (default vertical FOV).
31    #[must_use]
32    pub fn new(width: usize, height: usize) -> Self {
33        Self { width, height, ..Self::default() }
34    }
35}
36
37/// A dense range image in row-major order (`row * width + col`).
38#[derive(Clone, Debug, PartialEq)]
39pub struct RangeImage {
40    /// Image width (columns).
41    pub width: usize,
42    /// Image height (rows).
43    pub height: usize,
44    /// Range per pixel in meters; `0.0` marks an empty pixel.
45    pub data: Vec<f32>,
46}
47
48impl RangeImage {
49    /// Total number of pixels.
50    #[must_use]
51    pub fn len(&self) -> usize {
52        self.data.len()
53    }
54
55    /// Whether the image has no pixels.
56    #[must_use]
57    pub fn is_empty(&self) -> bool {
58        self.data.is_empty()
59    }
60
61    /// Number of pixels that received a point.
62    #[must_use]
63    pub fn filled_count(&self) -> usize {
64        self.data.iter().filter(|&&r| r > 0.0).count()
65    }
66
67    /// Range at `(row, col)`, or `None` if out of range.
68    #[must_use]
69    pub fn get(&self, row: usize, col: usize) -> Option<f32> {
70        if row >= self.height || col >= self.width {
71            return None;
72        }
73        Some(self.data[row * self.width + col])
74    }
75}
76
77/// Projects a cloud into a spherical range image, keeping the nearest range per
78/// pixel. Points outside the vertical field of view are dropped.
79pub fn range_image(cloud: &PointCloud, config: RangeImageConfig) -> SpatialResult<RangeImage> {
80    if config.width == 0 || config.height == 0 {
81        return Err(SpatialError::InvalidArgument(
82            "range image dimensions must be non-zero".to_owned(),
83        ));
84    }
85    if config.fov_up_deg <= config.fov_down_deg {
86        return Err(SpatialError::InvalidArgument(
87            "fov_up must be greater than fov_down".to_owned(),
88        ));
89    }
90
91    let (x, y, z) = cloud.positions3()?;
92    let fov_up = config.fov_up_deg.to_radians();
93    let fov_down = config.fov_down_deg.to_radians();
94    let fov = fov_up - fov_down;
95
96    let mut data = vec![0.0_f32; config.width * config.height];
97    for i in 0..cloud.len() {
98        let (px, py, pz) = (x[i], y[i], z[i]);
99        let range = (px * px + py * py + pz * pz).sqrt();
100        if range < 1e-6 {
101            continue;
102        }
103        let azimuth = py.atan2(px); // [-pi, pi]
104        let elevation = (pz / range).asin(); // [-pi/2, pi/2]
105
106        // Azimuth -> column, elevation -> row (top row = fov_up).
107        let u = 0.5 * (azimuth / std::f32::consts::PI + 1.0);
108        let v = 1.0 - (elevation - fov_down) / fov;
109        let col = (u * config.width as f32).floor();
110        let row = (v * config.height as f32).floor();
111        if row < 0.0 || col < 0.0 {
112            continue;
113        }
114        let (row, col) = (row as usize, col as usize);
115        if row >= config.height || col >= config.width {
116            continue;
117        }
118        let idx = row * config.width + col;
119        // Keep the nearest return (or fill an empty pixel).
120        if data[idx] == 0.0 || range < data[idx] {
121            data[idx] = range;
122        }
123    }
124
125    Ok(RangeImage { width: config.width, height: config.height, data })
126}
127
128#[cfg(test)]
129mod tests {
130    use super::*;
131    use spatialrust_core::{PointCloudBuilder, StandardSchemas};
132
133    fn cloud(points: &[[f32; 3]]) -> PointCloud {
134        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
135        for p in points {
136            builder.push_point(*p).unwrap();
137        }
138        builder.build().unwrap()
139    }
140
141    #[test]
142    fn projects_points_into_pixels() {
143        // A point straight ahead (+x) lands near the middle column; one to the
144        // left (+y) lands a quarter of the way across.
145        let c = cloud(&[[5.0, 0.0, 0.0], [0.0, 5.0, 0.0]]);
146        let img = range_image(&c, RangeImageConfig::new(360, 64)).unwrap();
147        assert_eq!(img.filled_count(), 2);
148        // Both points are at range 5.
149        let total: f32 = img.data.iter().filter(|&&r| r > 0.0).sum();
150        assert!((total - 10.0).abs() < 1e-3);
151    }
152
153    #[test]
154    fn keeps_nearest_range_per_pixel() {
155        // Two points in the same direction at different ranges share a pixel.
156        let c = cloud(&[[10.0, 0.0, 0.0], [3.0, 0.0, 0.0]]);
157        let img = range_image(&c, RangeImageConfig::new(16, 8)).unwrap();
158        assert_eq!(img.filled_count(), 1);
159        let nearest = img.data.iter().copied().find(|&r| r > 0.0).unwrap();
160        assert!((nearest - 3.0).abs() < 1e-3, "did not keep nearest range: {nearest}");
161    }
162
163    #[test]
164    fn drops_points_outside_vertical_fov() {
165        // A point straight up is well above a narrow forward FOV.
166        let c = cloud(&[[0.0, 0.0, 5.0]]);
167        let config = RangeImageConfig { fov_up_deg: 2.0, fov_down_deg: -2.0, width: 64, height: 8 };
168        let img = range_image(&c, config).unwrap();
169        assert_eq!(img.filled_count(), 0);
170    }
171
172    #[test]
173    fn rejects_bad_config() {
174        let c = cloud(&[[1.0, 0.0, 0.0]]);
175        assert!(range_image(&c, RangeImageConfig::new(0, 8)).is_err());
176        let bad_fov = RangeImageConfig { fov_up_deg: -5.0, fov_down_deg: 5.0, width: 8, height: 8 };
177        assert!(range_image(&c, bad_fov).is_err());
178    }
179}
spatialrust_voxelize/range_image.rs

spatialrust_voxelize/
range_image.rs
