nd_icp/
utils.rs

1use crate::types::{Point3D, PointSet};
2use nalgebra::{Dyn, Matrix3, OMatrix, Quaternion, Rotation3, UnitQuaternion};
3use ply_rs::parser::Parser;
4use std::fs::File;
5use three_d::FrameOutput;
6use three_d::{
7    degrees, vec3, Axes, Camera, ClearState, ColorMaterial, CpuMesh, Gm, InstancedMesh, Mat4,
8    OrbitControl, PointCloud, Srgba, Vec3, Window, WindowSettings,
9};
10
11pub fn load_ply_as_point_set(
12    file_path: &str,
13) -> Result<PointSet<Point3D>, Box<dyn std::error::Error>> {
14    // Open the PLY file
15    let file = File::open(file_path)?;
16    let mut f = std::io::BufReader::new(file);
17
18    // Initialize the PLY parser
19    let point_3d_parser = Parser::<Point3D>::new();
20
21    let header = point_3d_parser.read_header(&mut f).unwrap();
22
23    let mut points = vec![];
24    for (_, element) in &header.elements {
25        // we could also just parse them in sequence, but the file format might change
26        match element.name.as_ref() {
27            "vertex" => {
28                points = point_3d_parser
29                    .read_payload_for_element(&mut f, &element, &header)
30                    .unwrap();
31            }
32            _ => {}
33        }
34    }
35    // println!("point list: {:#?}", points);
36    Ok(PointSet { points })
37}
38
39pub fn visualise_points(model_cloud: &PointSet<Point3D>, target_cloud: &PointSet<Point3D>) {
40    // Create a window to render the scene
41    let window = Window::new(WindowSettings {
42        title: "3D Point Cloud Visualization".to_string(),
43        max_size: Some((1280, 720)),
44        ..Default::default()
45    })
46    .unwrap();
47
48    let context = window.gl();
49
50    // Create a camera to view the scene
51    let mut camera = Camera::new_perspective(
52        window.viewport(),
53        vec3(0.125, -0.25, -0.5),
54        vec3(0.0, 0.0, 0.0),
55        vec3(0.0, 1.0, 0.0),
56        degrees(45.0),
57        0.01,
58        100.0,
59    );
60
61    let mut control = OrbitControl::new(*camera.target(), 0.1, 3.0);
62
63    // Convert the PointSets into a format that can be rendered (using CpuMesh)
64    let model_points: Vec<Vec3> = model_cloud
65        .points
66        .iter()
67        .map(|p| vec3(p.x as f32, p.y as f32, p.z as f32))
68        .collect();
69    let target_points: Vec<Vec3> = target_cloud
70        .points
71        .iter()
72        .map(|p| vec3(p.x as f32, p.y as f32, p.z as f32))
73        .collect();
74
75    // Create point clouds
76    let model_cloud = PointCloud {
77        positions: three_d::Positions::F32(model_points.clone()),
78        ..Default::default()
79    };
80
81    let target_cloud = PointCloud {
82        positions: three_d::Positions::F32(target_points.clone()),
83        ..Default::default()
84    };
85
86    let mut point_mesh = CpuMesh::sphere(4);
87    point_mesh.transform(&Mat4::from_scale(0.0001)).unwrap();
88
89    let model_cloud_gm = Gm {
90        geometry: InstancedMesh::new(&context, &model_cloud.into(), &point_mesh),
91        material: ColorMaterial {
92            color: Srgba::new(0, 0, 255, 255), // Blue
93            ..Default::default()
94        },
95    };
96    // let c = -model_cloud_gm.aabb().center();
97    // model_cloud_gm.set_transformation(Mat4::from_translation(c));
98
99    let target_cloud_gm = Gm {
100        geometry: InstancedMesh::new(&context, &target_cloud.into(), &point_mesh),
101        material: ColorMaterial {
102            color: Srgba::new(255, 0, 0, 255), // Red
103            ..Default::default()
104        },
105    };
106
107    // main loop
108    window.render_loop(move |mut frame_input| {
109        let mut redraw = frame_input.first_frame;
110        redraw |= camera.set_viewport(frame_input.viewport);
111        redraw |= control.handle_events(&mut camera, &mut frame_input.events);
112
113        if redraw {
114            frame_input
115                .screen()
116                .clear(ClearState::color_and_depth(1.0, 1.0, 1.0, 1.0, 1.0))
117                .render(
118                    &camera,
119                    model_cloud_gm
120                        .into_iter()
121                        .chain(&Axes::new(&context, 0.01, 0.1))
122                        .chain(target_cloud_gm.into_iter()),
123                    &[],
124                );
125        }
126
127        FrameOutput {
128            swap_buffers: redraw,
129            ..Default::default()
130        }
131    });
132}
133
134pub fn get_quaternion_from_dynamic_rotation(rotation: &OMatrix<f32, Dyn, Dyn>) -> Quaternion<f32> {
135    assert!(
136        rotation.nrows() == 3 && rotation.ncols() == 3,
137        "Matrix must be 3x3"
138    );
139
140    let static_rotation = Matrix3::from_iterator(rotation.iter().cloned());
141    let rot = Rotation3::from_matrix(&static_rotation);
142
143    *UnitQuaternion::from_rotation_matrix(&rot)
144}
nd_icp/utils.rs

nd_icp/
utils.rs
