rigidity_io/
csv.rs

1//! Reading point clouds from CSV with a header row.
2//!
3//! The format of the ETH ASL datasets: the first line holds column names,
4//! the rest are comma-separated numbers. Coordinate names differ between
5//! sets (`x`/`y`/`z`, sometimes prefixed), so columns are located by name
6//! rather than by position.
7
8use std::fs::File;
9use std::io::{BufRead, BufReader};
10use std::path::Path;
11
12use nalgebra::Vector3;
13use rigidity_core::PointCloud;
14
15use crate::IoError;
16
17/// Finds the column whose name matches one of the candidates.
18fn find_column(header: &[String], names: &[&str]) -> Option<usize> {
19    header.iter().position(|column| {
20        let trimmed = column.trim().trim_matches('"').to_ascii_lowercase();
21        names.iter().any(|candidate| trimmed == *candidate)
22    })
23}
24
25/// Reads a cloud from CSV.
26///
27/// The origin is placed at the centre of the bounding box: dataset files
28/// sometimes hold global coordinates with large values, and `f32` storage
29/// cannot take those.
30pub fn read_csv(path: &Path) -> Result<PointCloud, IoError> {
31    let mut reader = BufReader::new(File::open(path)?);
32    let mut line = String::new();
33    if reader.read_line(&mut line)? == 0 {
34        return Err(IoError::BadHeader("the file is empty".into()));
35    }
36    let header: Vec<String> = line.trim().split(',').map(|s| s.to_string()).collect();
37
38    let x = find_column(&header, &["x"]).ok_or_else(|| IoError::BadHeader("no x column".into()))?;
39    let y = find_column(&header, &["y"]).ok_or_else(|| IoError::BadHeader("no y column".into()))?;
40    let z = find_column(&header, &["z"]).ok_or_else(|| IoError::BadHeader("no z column".into()))?;
41    let width = header.len();
42
43    let mut points: Vec<Vector3<f64>> = Vec::new();
44    let mut minimum = Vector3::repeat(f64::INFINITY);
45    let mut maximum = Vector3::repeat(f64::NEG_INFINITY);
46
47    line.clear();
48    while reader.read_line(&mut line)? != 0 {
49        let trimmed = line.trim();
50        if !trimmed.is_empty() {
51            let fields: Vec<&str> = trimmed.split(',').collect();
52            if fields.len() >= width {
53                let parse = |index: usize| -> Result<f64, IoError> {
54                    fields[index]
55                        .trim()
56                        .parse()
57                        .map_err(|_| IoError::BadNumber(fields[index].to_string()))
58                };
59                let point = Vector3::new(parse(x)?, parse(y)?, parse(z)?);
60                if point.iter().all(|value| value.is_finite()) {
61                    minimum = minimum.inf(&point);
62                    maximum = maximum.sup(&point);
63                    points.push(point);
64                }
65            }
66        }
67        line.clear();
68    }
69
70    if points.is_empty() {
71        return Ok(PointCloud::new());
72    }
73    let origin = (minimum + maximum) * 0.5;
74    let mut cloud = PointCloud::with_origin(origin);
75    for point in &points {
76        cloud.push(*point);
77    }
78    Ok(cloud)
79}
80
81/// Reads a `pose_scanner_leica.csv` pose file.
82///
83/// Each line holds a homogeneous 4×4 matrix row by row, plus bookkeeping
84/// columns. Matrices are returned in file order.
85pub fn read_poses(path: &Path) -> Result<Vec<nalgebra::Matrix4<f64>>, IoError> {
86    let text = std::fs::read_to_string(path)?;
87    let mut lines = text.lines();
88    let header: Vec<String> = lines
89        .next()
90        .ok_or_else(|| IoError::BadHeader("the pose file is empty".into()))?
91        .trim()
92        .split(',')
93        .map(|s| s.trim().trim_matches('"').to_ascii_lowercase())
94        .collect();
95
96    // Columns named T00…T33 or m00…m33.
97    let mut indices = [[usize::MAX; 4]; 4];
98    for (row, slots) in indices.iter_mut().enumerate() {
99        for (column, slot) in slots.iter_mut().enumerate() {
100            let names = [
101                format!("t{row}{column}"),
102                format!("m{row}{column}"),
103                format!("t_{row}{column}"),
104            ];
105            let found = header
106                .iter()
107                .position(|name| names.iter().any(|candidate| name == candidate));
108            match found {
109                Some(index) => *slot = index,
110                None => {
111                    return Err(IoError::BadHeader(format!(
112                        "no matrix column T{row}{column}"
113                    )));
114                }
115            }
116        }
117    }
118
119    let mut poses = Vec::new();
120    for line in lines {
121        let trimmed = line.trim();
122        if trimmed.is_empty() {
123            continue;
124        }
125        let fields: Vec<&str> = trimmed.split(',').collect();
126        let mut matrix = nalgebra::Matrix4::zeros();
127        for (row, slots) in indices.iter().enumerate() {
128            for (column, index) in slots.iter().enumerate() {
129                matrix[(row, column)] = fields
130                    .get(*index)
131                    .ok_or_else(|| IoError::BadHeader("row shorter than the header".into()))?
132                    .trim()
133                    .parse()
134                    .map_err(|_| IoError::BadNumber(fields[*index].to_string()))?;
135            }
136        }
137        poses.push(matrix);
138    }
139    Ok(poses)
140}
rigidity_io/csv.rs

rigidity_io/
csv.rs
