lidar_utils/ouster/
pcd_converter.rs

1//! Provides the converter type that converts packets to points.
2
3use super::{
4    config::Config,
5    consts::PIXELS_PER_COLUMN,
6    packet::{Column, Packet},
7};
8use crate::common::*;
9
10fn spherical_to_xyz(range: Length, azimuth_angle: Angle, altitude_angle: Angle) -> [Length; 3] {
11    let x = range * altitude_angle.sin() * azimuth_angle.cos();
12    let y = range * altitude_angle.sin() * azimuth_angle.sin();
13    let z = range * altitude_angle.cos();
14    [x, y, z]
15}
16
17#[derive(Clone, Debug)]
18pub struct Point {
19    pub timestamp: Time,
20    pub azimuth_angle: Angle,
21    pub distance: Length,
22    pub reflectivity: u16,
23    pub signal_photons: u16,
24    pub noise_photons: u16,
25    pub laser_id: u32,
26    pub point: [Length; 3],
27}
28
29/// A conversion tool that transforms [Column](Column) raw sensor data
30/// into point clouds.
31#[derive(Debug, Clone)]
32pub struct PointCloudConverter {
33    altitude_angles: [Angle; PIXELS_PER_COLUMN],
34    azimuth_angle_corrections: [Angle; PIXELS_PER_COLUMN],
35    columns_per_revolution: u16,
36}
37
38impl PointCloudConverter {
39    /// Create a converter from config.
40    pub fn from_config(config: Config) -> Self {
41        let Config {
42            beam_azimuth_angle_corrections,
43            beam_altitude_angles,
44            lidar_mode,
45        } = config;
46
47        let altitude_angles = {
48            let mut array = [Angle::new::<radian>(0.0); PIXELS_PER_COLUMN];
49            debug_assert_eq!(array.len(), beam_altitude_angles.len());
50
51            for idx in 0..(array.len()) {
52                let angle =
53                    std::f64::consts::FRAC_PI_2 - beam_altitude_angles[idx].to_radians().raw();
54                array[idx] = Angle::new::<radian>(angle);
55            }
56            array
57        };
58
59        let azimuth_angle_corrections = {
60            let mut array = [Angle::new::<radian>(0.0); PIXELS_PER_COLUMN];
61            debug_assert_eq!(array.len(), beam_azimuth_angle_corrections.len());
62
63            for idx in 0..(array.len()) {
64                let angle = beam_azimuth_angle_corrections[idx].to_radians();
65                array[idx] = Angle::new::<radian>(angle.raw());
66            }
67            array
68        };
69
70        let columns_per_revolution = lidar_mode.columns_per_revolution();
71
72        Self {
73            altitude_angles,
74            azimuth_angle_corrections,
75            columns_per_revolution,
76        }
77    }
78
79    /// Get lidar scene width by its mode. For example,
80    /// [LidarMode](super::enums::LidarMode) mode results
81    /// in 1024.
82    pub fn columns_per_revolution(&self) -> u16 {
83        self.columns_per_revolution
84    }
85
86    /// Compute point locations from column returned from lidar.
87    ///
88    /// The method takes [Column.measurement_id](Column.measurement_id) as column index.
89    /// It returns error if the index is out of bound.
90    pub(crate) fn column_to_points(&self, column: &Column) -> Result<Vec<Point>> {
91        // sanity check
92        let col_index = column.measurement_id;
93        ensure!(
94            col_index < self.columns_per_revolution,
95            "measurement_id {} is exceeds the upper bound {}. Is the lidar_mode configured correctly?",
96            col_index,
97            self.columns_per_revolution,
98        );
99
100        // return empty list if the column is not valid
101        if !column.valid() {
102            return Ok(vec![]);
103        }
104
105        let pixels_iter = column.pixels.iter();
106
107        let points = izip!(
108            pixels_iter,
109            self.altitude_angles.iter(),
110            self.azimuth_angle_corrections.iter(),
111            0..
112        )
113        .map(
114            |(pixel, altitude_angle, azimuth_angle_correction, laser_id)| {
115                // add correction according to manual
116                let clockwise_azimuth_angle = column.azimuth_angle() + *azimuth_angle_correction;
117                let counter_clockwise_azimuth_angle =
118                    Angle::new::<radian>(std::f64::consts::PI * 2.0) - clockwise_azimuth_angle;
119                let distance = pixel.distance();
120                let timestamp = column.time();
121                let point =
122                    spherical_to_xyz(distance, counter_clockwise_azimuth_angle, *altitude_angle);
123
124                Point {
125                    timestamp,
126                    reflectivity: pixel.reflectivity,
127                    signal_photons: pixel.signal_photons,
128                    noise_photons: pixel.noise_photons,
129                    azimuth_angle: clockwise_azimuth_angle,
130                    distance,
131                    laser_id,
132                    point,
133                }
134            },
135        )
136        .collect::<Vec<_>>();
137        Ok(points)
138    }
139
140    /// Compute point positions from a packet.
141    pub fn convert<P>(&self, packet: P) -> Result<Vec<Point>>
142    where
143        P: AsRef<Packet>,
144    {
145        let points: Vec<_> = packet
146            .as_ref()
147            .columns
148            .iter()
149            .map(|col| self.column_to_points(col))
150            .collect::<Result<Vec<_>>>()?
151            .into_iter()
152            .flatten()
153            .collect();
154        Ok(points)
155    }
156}
lidar_utils/ouster/pcd_converter.rs

lidar_utils/ouster/
pcd_converter.rs
