uvt_viz3d/
lib.rs

1//! # uvt-viz3d
2//!
3//! This crate provides utilities to visualize the content of an _Uncrewed Vehicle Trajectory_ (UVT) file in 3D, using [rerun](https://rerun.io).
4//! The UVT format is an extension of the LTR file format introduced in [_Kilometer-Scale Autonomous Navigation in Subarctic Forests: Challenges and Lessons Learned_](https://doi.org/10.55417/fr.2022050).
5//!
6//! ## Features
7//!
8//! A UVT file contains:
9//!
10//! - A LiDAR map of the environment, stored in the [`VTK` format](https://vtk.org).
11//! - A trajectory recorded by an uncrewed vehicle.
12//!
13//! This crate provides a function to visualize the content of a UVT file in 3D.
14//!
15//! ## Example
16//!
17//! ```rust
18//! use uvt;
19//! use uvt_viz3d;
20//!
21//! // Open a UVT file
22//! let my_uvt = uvt::Uvt::read_file("example.uvt").unwrap();
23//!
24//! // Visualize UVT with rerun
25//! uvt_viz3d::show_uvt(my_uvt);
26//! ```
27use rerun::external::glam;
28use vtkio::model::{DataSet, Piece};
29
30use uvt;
31use vtkio::IOBuffer;
32
33/// Visualizes the content of a UVT file in 3D using rerun.
34///
35/// This function displays the LiDAR map and trajectory data from the UVT file in a 3D viewer.
36///
37/// # Arguments
38///
39/// * `uvt_file` - A `uvt::Uvt` object containing the map and trajectory data.
40///
41/// # Example
42///
43/// ```rust
44/// use uvt;
45/// use uvt_viz3d;
46///
47/// let my_uvt = uvt::Uvt::read_file("example.uvt").unwrap();
48/// uvt_viz3d::show_uvt(my_uvt);
49/// ```
50pub fn show_uvt(uvt_file: uvt::Uvt) {
51    let map = uvt_file.map;
52    let point_cloud = map.data.clone();
53
54    let pieces = match point_cloud {
55        DataSet::PolyData { pieces, .. } => pieces,
56        _ => {
57            panic!("Wrong vtk data type");
58        }
59    };
60
61    let points: Vec<uvt::Point> = pieces
62        .iter()
63        .map(|inline_piece| match inline_piece {
64            Piece::Inline(piece) => {
65                let piece_buf_points = piece.points.clone();
66                let buf_values = match piece_buf_points {
67                    IOBuffer::F64(buf_points) => buf_points,
68                    IOBuffer::F32(buf_points) => buf_points.iter().map(|&v| v as f64).collect(),
69                    _ => panic!("Unimplemented IO Buffer"),
70                };
71
72                let n_points = piece.num_points();
73                let piece_points: Vec<uvt::Point> = (0..n_points)
74                    .into_iter()
75                    .map(|i| (3 * i + 0, 3 * i + 1, 3 * i + 2))
76                    .map(|(idx_x, idx_y, idx_z)| {
77                        uvt::Point::new(buf_values[idx_x], buf_values[idx_y], buf_values[idx_z])
78                    })
79                    .collect();
80                piece_points
81            }
82            _ => panic!("Unknown piece type"),
83        })
84        .flatten()
85        .collect();
86
87    // Limits of Z
88    let zs: Vec<f64> = points.iter().map(|&pt| pt.z).collect();
89    let z_min = zs
90        .iter()
91        .min_by(|&a, &b| a.partial_cmp(b).unwrap())
92        .unwrap()
93        .clone();
94    let z_max = zs
95        .iter()
96        .max_by(|&a, &b| a.partial_cmp(b).unwrap())
97        .unwrap()
98        .clone();
99
100    // Colors
101    let colors: Vec<[u8; 4]> = points
102        .iter()
103        .map(|pt| colormap_turbo_srgb(((pt.z - z_min) / (z_max - z_min)) as f32))
104        .collect();
105
106    // Init rerun
107    rerun::external::re_log::setup_logging();
108
109    let rec = rerun::RecordingStreamBuilder::new("point-cloud-viewer")
110        .spawn()
111        .unwrap();
112    rec.set_duration_secs("stable-time", 0f64);
113
114    // Log map
115    rec.log_static(
116        "/map",
117        &rerun::Points3D::new(
118            points
119                .iter()
120                .map(|&pt| {
121                    let coords: [f32; 3] = pt.into();
122                    let vec_pt: glam::Vec3 = coords.into();
123                    vec_pt
124                })
125                .into_iter(),
126        )
127        .with_colors(colors)
128        .with_radii([0.08]),
129    )
130    .unwrap();
131
132    // Log trajectory
133    let n_points = uvt_file.trajectory.len();
134    let red: Vec<[u8; 4]> = (0..n_points)
135        .into_iter()
136        .map(|_| [255, 255, 255, 255])
137        .collect();
138
139    rec.log_static(
140        "/trajectory",
141        &rerun::Points3D::new(
142            uvt_file
143                .trajectory
144                .iter()
145                .map(|pt| {
146                    let coords: [f32; 3] = pt.pose.position.into();
147                    let vec_pt: glam::Vec3 = coords.into();
148                    vec_pt
149                })
150                .into_iter(),
151        )
152        .with_colors(red)
153        .with_radii([0.25]),
154    )
155    .unwrap();
156}
157
158// Returns sRGB polynomial approximation from Turbo color map, assuming `t` is normalized. Copied from rerun DNA demo.
159fn colormap_turbo_srgb(t: f32) -> [u8; 4] {
160    #![allow(clippy::excessive_precision)]
161    use glam::{Vec2, Vec4, Vec4Swizzles as _};
162
163    const R4: Vec4 = Vec4::new(0.13572138, 4.61539260, -42.66032258, 132.13108234);
164    const G4: Vec4 = Vec4::new(0.09140261, 2.19418839, 4.84296658, -14.18503333);
165    const B4: Vec4 = Vec4::new(0.10667330, 12.64194608, -60.58204836, 110.36276771);
166
167    const R2: Vec2 = Vec2::new(-152.94239396, 59.28637943);
168    const G2: Vec2 = Vec2::new(4.27729857, 2.82956604);
169    const B2: Vec2 = Vec2::new(-89.90310912, 27.34824973);
170
171    debug_assert!((0.0..=1.0).contains(&t));
172
173    let v4 = glam::vec4(1.0, t, t * t, t * t * t);
174    let v2 = v4.zw() * v4.z;
175
176    [
177        ((v4.dot(R4) + v2.dot(R2)) * 255.0) as u8,
178        ((v4.dot(G4) + v2.dot(G2)) * 255.0) as u8,
179        ((v4.dot(B4) + v2.dot(B2)) * 255.0) as u8,
180        255,
181    ]
182}
uvt_viz3d/lib.rs

uvt_viz3d/
lib.rs
