148 lines
4.4 KiB
Rust
148 lines
4.4 KiB
Rust
//! Point cloud types + PLY export + Gaussian splat conversion.
|
|
#![allow(dead_code)]
|
|
|
|
use serde::{Deserialize, Serialize};
|
|
use std::io::Write;
|
|
|
|
#[derive(Clone, Debug, Serialize, Deserialize)]
|
|
pub struct Point3D {
|
|
pub x: f32,
|
|
pub y: f32,
|
|
pub z: f32,
|
|
}
|
|
|
|
#[derive(Clone, Debug, Serialize, Deserialize)]
|
|
pub struct ColorPoint {
|
|
pub x: f32,
|
|
pub y: f32,
|
|
pub z: f32,
|
|
pub r: u8,
|
|
pub g: u8,
|
|
pub b: u8,
|
|
pub intensity: f32,
|
|
}
|
|
|
|
#[derive(Clone, Debug, Serialize, Deserialize)]
|
|
pub struct PointCloud {
|
|
pub points: Vec<ColorPoint>,
|
|
pub timestamp_ms: i64,
|
|
pub source: String,
|
|
}
|
|
|
|
impl PointCloud {
|
|
pub fn new(source: &str) -> Self {
|
|
Self {
|
|
points: Vec::new(),
|
|
timestamp_ms: chrono::Utc::now().timestamp_millis(),
|
|
source: source.to_string(),
|
|
}
|
|
}
|
|
|
|
#[allow(clippy::too_many_arguments)]
|
|
pub fn add(&mut self, x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, intensity: f32) {
|
|
self.points.push(ColorPoint {
|
|
x,
|
|
y,
|
|
z,
|
|
r,
|
|
g,
|
|
b,
|
|
intensity,
|
|
});
|
|
}
|
|
|
|
pub fn bounds(&self) -> ([f32; 3], [f32; 3]) {
|
|
if self.points.is_empty() {
|
|
return ([0.0; 3], [0.0; 3]);
|
|
}
|
|
let mut min = [f32::MAX; 3];
|
|
let mut max = [f32::MIN; 3];
|
|
for p in &self.points {
|
|
min[0] = min[0].min(p.x);
|
|
min[1] = min[1].min(p.y);
|
|
min[2] = min[2].min(p.z);
|
|
max[0] = max[0].max(p.x);
|
|
max[1] = max[1].max(p.y);
|
|
max[2] = max[2].max(p.z);
|
|
}
|
|
(min, max)
|
|
}
|
|
}
|
|
|
|
/// Write point cloud to PLY format (ASCII).
|
|
pub fn write_ply(cloud: &PointCloud, path: &str) -> anyhow::Result<()> {
|
|
let mut f = std::fs::File::create(path)?;
|
|
writeln!(f, "ply")?;
|
|
writeln!(f, "format ascii 1.0")?;
|
|
writeln!(f, "comment Generated by RuView Dense Point Cloud")?;
|
|
writeln!(f, "comment Source: {}", cloud.source)?;
|
|
writeln!(f, "comment Timestamp: {}", cloud.timestamp_ms)?;
|
|
writeln!(f, "element vertex {}", cloud.points.len())?;
|
|
writeln!(f, "property float x")?;
|
|
writeln!(f, "property float y")?;
|
|
writeln!(f, "property float z")?;
|
|
writeln!(f, "property uchar red")?;
|
|
writeln!(f, "property uchar green")?;
|
|
writeln!(f, "property uchar blue")?;
|
|
writeln!(f, "property float intensity")?;
|
|
writeln!(f, "end_header")?;
|
|
for p in &cloud.points {
|
|
writeln!(
|
|
f,
|
|
"{:.4} {:.4} {:.4} {} {} {} {:.4}",
|
|
p.x, p.y, p.z, p.r, p.g, p.b, p.intensity
|
|
)?;
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
/// Convert point cloud to Gaussian splats for 3D rendering.
|
|
#[derive(Serialize, Deserialize)]
|
|
pub struct GaussianSplat {
|
|
pub center: [f32; 3],
|
|
pub color: [f32; 3],
|
|
pub opacity: f32,
|
|
pub scale: [f32; 3],
|
|
}
|
|
|
|
pub fn to_gaussian_splats(cloud: &PointCloud) -> Vec<GaussianSplat> {
|
|
// Cluster points into voxels and create one Gaussian per cluster
|
|
let voxel_size = 0.08; // smaller voxels = more detail = visible movement
|
|
let mut cells: std::collections::HashMap<(i32, i32, i32), Vec<&ColorPoint>> =
|
|
std::collections::HashMap::new();
|
|
|
|
for p in &cloud.points {
|
|
let key = (
|
|
(p.x / voxel_size).floor() as i32,
|
|
(p.y / voxel_size).floor() as i32,
|
|
(p.z / voxel_size).floor() as i32,
|
|
);
|
|
cells.entry(key).or_default().push(p);
|
|
}
|
|
|
|
cells
|
|
.values()
|
|
.map(|pts| {
|
|
let n = pts.len() as f32;
|
|
let cx = pts.iter().map(|p| p.x).sum::<f32>() / n;
|
|
let cy = pts.iter().map(|p| p.y).sum::<f32>() / n;
|
|
let cz = pts.iter().map(|p| p.z).sum::<f32>() / n;
|
|
let cr = pts.iter().map(|p| p.r as f32).sum::<f32>() / n / 255.0;
|
|
let cg = pts.iter().map(|p| p.g as f32).sum::<f32>() / n / 255.0;
|
|
let cb = pts.iter().map(|p| p.b as f32).sum::<f32>() / n / 255.0;
|
|
|
|
// Scale based on point spread
|
|
let sx = pts.iter().map(|p| (p.x - cx).abs()).sum::<f32>() / n + 0.01;
|
|
let sy = pts.iter().map(|p| (p.y - cy).abs()).sum::<f32>() / n + 0.01;
|
|
let sz = pts.iter().map(|p| (p.z - cz).abs()).sum::<f32>() / n + 0.01;
|
|
|
|
GaussianSplat {
|
|
center: [cx, cy, cz],
|
|
color: [cr, cg, cb],
|
|
opacity: (n / 10.0).min(1.0),
|
|
scale: [sx, sy, sz],
|
|
}
|
|
})
|
|
.collect()
|
|
}
|