use super::*;
use crate::geom3::align3::jacobian::{copy_jacobian, point_surf_jacobian};
use crate::geom3::mesh::Mesh;
use crate::geom3::{Alignment3, Point3, SurfacePoint3};
use crate::Result;
use crate::common::kd_tree::{KdTree, KdTreeSearch};
use crate::common::points::dist;
use crate::common::ransac_tools::ransac_indices;
use crate::geom3::align3::params::AlignParams3;
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
use parry3d_f64::na::{Dyn, Matrix, Owned, U1, U6, Vector};
use rayon::prelude::*;
pub fn ransac_points_to_mesh(
points: &[Point3],
mesh: &Mesh,
params: &AlignParams3,
inlier_threshold: f64,
iterations: usize,
) -> Result<Iso3> {
let tree = KdTree::try_new(points)?;
let results = ransac_indices::<3>(iterations, points.len())?
.par_iter()
.map(|i| {
let mut local_indices = vec![i[0], i[1], i[2]];
for (j, _) in tree.nearest(&points[i[0]], 3) {
local_indices.push(j);
}
for (j, _) in tree.nearest(&points[i[1]], 3) {
local_indices.push(j);
}
for (j, _) in tree.nearest(&points[i[2]], 3) {
local_indices.push(j);
}
local_indices.sort();
local_indices.dedup();
let local_points = local_indices.iter().map(|&i| points[i]).collect::<Vec<_>>();
if let Ok(align) = points_to_mesh(&local_points, mesh, params.clone()) {
let mut inliers = 0;
for test_point in points.iter() {
if mesh.distance_closest_to(&(align.full() * test_point)) < inlier_threshold {
inliers += 1;
}
}
Some((*align.full(), inliers))
} else {
None
}
})
.filter_map(|x| x)
.collect::<Vec<_>>();
let (best_transform, _) = results.iter().max_by_key(|(_, inliers)| *inliers).unwrap();
Ok(*best_transform)
}
pub fn points_to_mesh(points: &[Point3], mesh: &Mesh, params: AlignParams3) -> Result<Alignment3> {
let problem = PointsToMesh::new(points, mesh, params);
let (result, report) = LevenbergMarquardt::new().minimize(problem);
if report.termination.was_successful() {
let residuals = result.residuals().unwrap().as_slice().to_vec();
let c = result.params.current_values();
let align = Alignment3::new(
c.transform,
c.align,
result.params.local,
result.params.offset,
residuals,
);
Ok(align)
} else {
Err("Failed to align points to mesh".into())
}
}
struct PointsToMesh<'a> {
points: &'a [Point3],
mesh: &'a Mesh,
params: AlignParams3,
moved: Vec<Point3>,
closest: Vec<SurfacePoint3>,
residuals: Vec<f64>,
weights: Vec<f64>,
parallel: bool,
}
impl<'a> PointsToMesh<'a> {
fn new(points: &'a [Point3], mesh: &'a Mesh, params: AlignParams3) -> Self {
let mut x = Self {
points,
mesh,
params,
moved: vec![Point3::default(); points.len()],
closest: vec![SurfacePoint3::default(); points.len()],
residuals: vec![0.0; points.len()],
weights: vec![1.0; points.len()],
parallel: false,
};
x.move_points();
x
}
fn move_points(&mut self) {
let current = self.params.current_values();
let indices = (0..self.points.len()).collect::<Vec<_>>();
if self.parallel {
let collected = indices
.par_iter()
.map(|&i| {
let m = current.transform * self.points[i];
let c = self.mesh.surf_closest_to(&m);
(i, m, c)
})
.collect::<Vec<_>>();
for (i, m, c) in collected {
self.moved[i] = m;
self.closest[i] = c.sp;
}
} else {
for (i, &j) in indices.iter().enumerate() {
let m = current.transform * self.points[j];
let c = self.mesh.surf_closest_to(&m);
self.moved[i] = m;
self.closest[i] = c.sp;
}
}
for (i, (p, c)) in self.moved.iter().zip(self.closest.iter()).enumerate() {
self.residuals[i] = dist(p, &c.point) * c.scalar_projection(p).signum();
}
}
}
impl LeastSquaresProblem<f64, Dyn, U6> for PointsToMesh<'_> {
type ResidualStorage = Owned<f64, Dyn, U1>;
type JacobianStorage = Owned<f64, Dyn, U6>;
type ParameterStorage = Owned<f64, U6>;
fn set_params(&mut self, x: &Vector<f64, U6, Self::ParameterStorage>) {
self.params.set_storage(*x);
self.move_points();
}
fn params(&self) -> Vector<f64, U6, Self::ParameterStorage> {
self.params.get_storage()
}
fn residuals(&self) -> Option<Vector<f64, Dyn, Self::ResidualStorage>> {
let mut res = Matrix::<f64, Dyn, U1, Self::ResidualStorage>::zeros(self.points.len());
for i in 0..self.points.len() {
res[i] = self.residuals[i] * self.weights[i];
}
Some(res)
}
fn jacobian(&self) -> Option<Matrix<f64, Dyn, U6, Self::JacobianStorage>> {
let current = self.params.current_values();
let mut jac = Matrix::<f64, Dyn, U6, Self::JacobianStorage>::zeros(self.points.len());
for (i, (p, c)) in self.moved.iter().zip(self.closest.iter()).enumerate() {
let values = point_surf_jacobian(p, c, ¤t) * self.weights[i];
copy_jacobian(&values, &mut jac, i);
}
Some(jac)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::points::{clone_points, mean_point, transform_points};
use crate::tests::engine_blade;
use crate::{SelectOp, Selection};
use approx::assert_relative_eq;
use std::f64::consts::PI;
#[test]
fn simple_box_disturbed() -> Result<()> {
let mesh = Mesh::create_box(10.0, 5.0, 2.0, false);
let points = clone_points(&mesh.sample_poisson(0.1, None));
let disturb = Iso3::from_parts(
Translation3::new(3.0, 2.0, 1.0),
UnitQuaternion::from_euler_angles(PI / 8.0, PI / 12.0, PI / 16.0),
);
let params = AlignParams3::new_at_origin(None);
let to_align = transform_points(&points, &disturb);
let result = points_to_mesh(&to_align, &mesh, params)?;
assert_relative_eq!(disturb.inverse(), result.full(), epsilon = 1e-8);
Ok(())
}
#[test]
fn blade_example() -> Result<()> {
let mesh = engine_blade();
let mask = mesh
.face_select(Selection::None)
.facing(&Vector3::y(), PI / 4.0, SelectOp::Add)
.take_mask();
let expected_points = clone_points(&mesh.sample_poisson(2.0, Some(&mask)));
let disturb = Iso3::from_parts(
Translation3::new(-100.0, 150.0, 0.0),
UnitQuaternion::new(Vector3::new(1.0, 1.0, 1.0).normalize() * PI / 6.0),
);
let to_align = transform_points(&expected_points, &disturb);
let params = AlignParams3::new_at_center(mean_point(&to_align), None);
let result = points_to_mesh(&to_align, &mesh, params)?;
let aligned = transform_points(&to_align, result.full());
let max_deviation = aligned
.iter()
.zip(expected_points.iter())
.map(|(a, e)| (a - e).norm())
.max_by(|a, b| a.partial_cmp(b).unwrap())
.unwrap();
assert!(
max_deviation < 1e-6,
"Max deviation is too high: {}",
max_deviation
);
Ok(())
}
}