uvt_plot/
lib.rs

1//! # uvt-plot
2//!
3//! This crate provides utilities for visualizing the content of an _Uncrewed Vehicle Trajectory_ (UVT) file
4//! by plotting a bird-eye view of the recorded trajectory. The UVT format is an extension of the LTR file
5//! format introduced in [_Kilometer-Scale Autonomous Navigation in Subarctic Forests: Challenges and Lessons Learned_](https://doi.org/10.55417/fr.2022050).
6//!
7//! ## Features
8//!
9//! A UVT file contains:
10//!
11//! - A LiDAR map of the environment, stored in the [`VTK` format](https://vtk.org).
12//! - A trajectory recorded by an uncrewed vehicle.
13//!
14//! This crate provides a function to plot the trajectory in a UVT file.
15//!
16//! ## Example
17//!
18//! ```rust
19//! use uvt;
20//! use uvt_plot;
21//!
22//! // Open a UVT file
23//! let my_uvt = uvt::Uvt::read_file("example.uvt").unwrap();
24//!
25//! // Plot trajectory
26//! uvt_plot::plot_trajectory(my_uvt);
27//! ```
28use std::{path::PathBuf, str::FromStr};
29
30use plotters::{
31    chart::{self, ChartBuilder},
32    prelude::{BitMapBackend, Circle, IntoDrawingArea},
33    series::LineSeries,
34    style::{RED, WHITE},
35};
36use uvt;
37
38/// Plots the trajectory from a UVT file.
39///
40/// This function generates a bird-eye view of the trajectory recorded in the UVT file
41/// and saves it as a PNG image (`traj.png`) in the current working directory.
42///
43/// # Arguments
44///
45/// * `uvt_file` - A `uvt::Uvt` object containing the trajectory data.
46///
47/// # Example
48///
49/// ```rust
50/// use uvt;
51/// use uvt_plot;
52///
53/// let my_uvt = uvt::Uvt::read_file("example.uvt").unwrap();
54/// uvt_plot::plot_trajectory(my_uvt);
55/// ```
56///
57/// The resulting plot will be saved as `traj.png`.
58pub fn plot_trajectory(uvt_file: uvt::Uvt) {
59    let figpath: PathBuf = PathBuf::from_str("traj.png").unwrap();
60
61    let positions: Vec<(f64, f64, f64)> = uvt_file
62        .trajectory
63        .iter()
64        .map(|pose| pose.pose.position)
65        .map(|pt| (pt.x, pt.y, pt.z))
66        .collect();
67
68    let (xs, ys): (Vec<f64>, Vec<f64>) = positions.iter().map(|pt| (pt.0, pt.1)).unzip();
69    let zs: Vec<f64> = positions.iter().map(|pt| pt.2).collect();
70
71    let x_max = xs
72        .iter()
73        .max_by(|&a, &b| a.partial_cmp(b).unwrap())
74        .unwrap()
75        .clone();
76    let x_min = xs
77        .iter()
78        .min_by(|&a, &b| a.partial_cmp(b).unwrap())
79        .unwrap()
80        .clone();
81    let y_max = ys
82        .iter()
83        .max_by(|&a, &b| a.partial_cmp(b).unwrap())
84        .unwrap()
85        .clone();
86    let y_min = ys
87        .iter()
88        .min_by(|&a, &b| a.partial_cmp(b).unwrap())
89        .unwrap()
90        .clone();
91    let z_max = zs
92        .iter()
93        .max_by(|&a, &b| a.partial_cmp(b).unwrap())
94        .unwrap()
95        .clone();
96    let z_min = zs
97        .iter()
98        .min_by(|&a, &b| a.partial_cmp(b).unwrap())
99        .unwrap()
100        .clone();
101
102    let x_span = (x_min, x_max);
103    let y_span = (y_min, y_max);
104    let z_span = (z_min, z_max);
105
106    let fig = BitMapBackend::new(figpath.as_os_str(), (800, 600)).into_drawing_area();
107    fig.fill(&WHITE).unwrap();
108
109    let mut ctx = ChartBuilder::on(&fig)
110        .margin(20)
111        .set_label_area_size(chart::LabelAreaPosition::Left, 40)
112        .set_label_area_size(chart::LabelAreaPosition::Bottom, 40)
113        .build_cartesian_2d(
114            (x_span.0 - 10.0)..(x_span.1 + 10.0),
115            (y_span.0 - 10.0)..(y_span.1 + 10.0),
116        )
117        .unwrap();
118    ctx.configure_mesh().draw().unwrap();
119
120    ctx.draw_series(LineSeries::new(
121        (-100..100)
122            .map(|y| y as f64 / 100.0)
123            .map(|y| ((y * 10.0).sin(), y)),
124        &RED,
125    ))
126    .unwrap();
127
128    ctx.draw_series(
129        positions
130            .into_iter()
131            .map(|(x, y, _)| (x, y))
132            .map(|pt| Circle::new(pt, 1, &RED)),
133    )
134    .unwrap();
135
136    println!("Saved trajectory plot in {}", figpath.as_os_str().display());
137}
uvt_plot/lib.rs

uvt_plot/
lib.rs
