use crate::Result;
use crate::common::points::dist;
use crate::geom3::Align3;
use crate::geom3::align3::jacobian::{point_plane_jacobian, point_plane_jacobian_rev};
use crate::geom3::align3::multi_param::ParamHandler;
use crate::geom3::align3::{GAPParams, distance_weight, normal_weight};
use crate::na::{DMatrix, Dyn, Matrix, Owned, U1, Vector};
use faer::prelude::default;
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
use rayon::prelude::*;
use crate::geom3::align3::mesh::{AlignmentMesh, generate_alignment_points};
use crate::geom3::mesh::MeshSurfPoint;
pub fn multi_mesh_adjustment_with_points(
meshes: &[AlignmentMesh],
points: Vec<MulMeshAlignPoint>,
static_i: usize,
opts: MMOpts,
) -> Result<Vec<Align3>> {
let problem = MultiMeshProblem::new(meshes, points, static_i, opts);
let (result, report) = LevenbergMarquardt::new().minimize(problem);
if report.termination.was_successful() {
let alignments = (0..meshes.len())
.map(|i| result.params.get_transform(i))
.collect::<Vec<_>>();
let mut grouped = (0..meshes.len()).map(|_| Vec::new()).collect::<Vec<_>>();
let residuals = result.residuals().unwrap();
for (i, p) in result.point_handles.iter().enumerate() {
grouped[p.mesh_i].push(residuals[i]);
}
Ok(alignments
.iter()
.zip(grouped)
.map(|(a, g)| Align3::new(*a, g))
.collect())
} else {
Err("Failed to converge".into())
}
}
#[derive(Debug, Clone, Copy)]
pub struct MMOpts {
pub search_radius: f64,
pub respect_normals: bool,
}
impl MMOpts {
pub fn new(search_radius: f64, respect_normals: bool) -> Self {
Self {
search_radius,
respect_normals,
}
}
}
pub fn multi_mesh_adjustment(
meshes: &[AlignmentMesh],
opts: MMOpts,
sample_opts: GAPParams,
) -> Result<Vec<Align3>> {
let matrix = correspondence_matrix(meshes, &sample_opts);
let mut corr = &matrix / matrix.max();
corr.apply(|x| *x = x.sqrt());
let mut corr_pairs = corr
.column_sum()
.iter()
.enumerate()
.map(|(i, x)| (i, *x))
.collect::<Vec<_>>();
corr_pairs.sort_by(|a, b| b.1.partial_cmp(&a.1).unwrap());
let reference_order = corr_pairs.iter().map(|(i, _)| *i).collect::<Vec<_>>();
let static_i = reference_order[0];
let mut work_list = Vec::new();
let mut meshes_to_test = (0..meshes.len()).collect::<Vec<_>>();
for ref_i in reference_order {
meshes_to_test.retain(|j| *j != ref_i);
for &mesh_i in meshes_to_test.iter() {
work_list.push((mesh_i, ref_i));
}
}
let handles = work_list
.par_iter()
.map(|(mesh_i, ref_i)| {
let t = meshes[*ref_i]
.transform()
.inv_mul(&meshes[*mesh_i].transform());
let mut to_test = Vec::new();
let samples = generate_alignment_points(
meshes[*mesh_i].mesh,
meshes[*ref_i].mesh,
&t,
&sample_opts,
);
for mp in samples {
let weight = if let Some(providers) = &meshes[*mesh_i].weights {
let mut w = 1.0;
for item in providers.iter() {
w *= item.weight(&mp);
}
w
} else {
1.0
};
to_test.push(MulMeshAlignPoint::new(*mesh_i, mp, *ref_i, weight, 0.0));
}
to_test
})
.flatten()
.collect::<Vec<_>>();
let problem = MultiMeshProblem::new(meshes, handles, static_i, opts);
let (result, report) = LevenbergMarquardt::new().minimize(problem);
if report.termination.was_successful() {
let alignments = (0..meshes.len())
.map(|i| result.params.get_transform(i))
.collect::<Vec<_>>();
let mut grouped = (0..meshes.len()).map(|_| Vec::new()).collect::<Vec<_>>();
let residuals = result.residuals().unwrap();
for (i, p) in result.point_handles.iter().enumerate() {
grouped[p.mesh_i].push(residuals[i]);
}
Ok(alignments
.iter()
.zip(grouped)
.map(|(a, g)| Align3::new(*a, g))
.collect())
} else {
Err("Failed to converge".into())
}
}
pub struct MulMeshAlignPoint {
pub mesh_i: usize,
pub mp: MeshSurfPoint,
pub ref_i: usize,
pub weight: f64,
#[allow(dead_code)]
pub uncert: f64,
}
impl MulMeshAlignPoint {
pub fn new(mesh_i: usize, mp: MeshSurfPoint, ref_i: usize, weight: f64, uncert: f64) -> Self {
Self {
mesh_i,
mp,
ref_i,
weight,
uncert,
}
}
}
struct MultiMeshProblem<'a> {
meshes: &'a [AlignmentMesh<'a>],
point_handles: Vec<MulMeshAlignPoint>,
moved: Vec<MeshSurfPoint>,
closest: Vec<MeshSurfPoint>,
weight: Vec<f64>,
params: ParamHandler,
options: MMOpts,
}
impl<'a> MultiMeshProblem<'a> {
fn new(
meshes: &'a [AlignmentMesh],
point_handles: Vec<MulMeshAlignPoint>,
static_i: usize,
options: MMOpts,
) -> Self {
let mean_points = meshes
.iter()
.map(|m| m.mesh.aabb().center())
.collect::<Vec<_>>();
let initial = meshes.iter().map(|m| m.transform()).collect::<Vec<_>>();
let params = ParamHandler::new(static_i, mean_points, Some(&initial));
let count: usize = point_handles.len();
let mut item = Self {
meshes,
point_handles,
moved: vec![default(); count],
closest: vec![default(); count],
weight: vec![0.0; count],
params,
options,
};
item.move_points();
item
}
fn move_points(&mut self) {
let indices = (0..self.point_handles.len()).collect::<Vec<_>>();
let collected = indices
.par_iter()
.map(|i| {
let h = &self.point_handles[*i];
let ref_mesh = &self.meshes[h.ref_i];
let t = self.params.relative_transform(h.mesh_i, h.ref_i);
let moved = h.mp.new_transformed_by(&t);
let closest = ref_mesh.mesh.surf_closest_to(&moved.point());
let weight_d = distance_weight(dist(&moved, &closest), self.options.search_radius);
let weight_n = if self.options.respect_normals {
normal_weight(&moved.normal(), &closest.normal())
} else {
1.0
};
let weight = h.weight * weight_d * weight_n;
(*i, moved, closest, weight)
})
.collect::<Vec<_>>();
for (i, moved, closest, w) in collected {
self.moved[i] = moved;
self.closest[i] = closest;
self.weight[i] = w;
}
}
}
impl<'a> LeastSquaresProblem<f64, Dyn, Dyn> for MultiMeshProblem<'a> {
type ResidualStorage = Owned<f64, Dyn, U1>;
type JacobianStorage = Owned<f64, Dyn, Dyn>;
type ParameterStorage = Owned<f64, Dyn>;
fn set_params(&mut self, x: &Vector<f64, Dyn, Self::ParameterStorage>) {
self.params.set_param(x);
self.move_points();
}
fn params(&self) -> Vector<f64, Dyn, Self::ParameterStorage> {
(*self.params.params()).clone()
}
fn residuals(&self) -> Option<Vector<f64, Dyn, Self::ResidualStorage>> {
let mut res =
Matrix::<f64, Dyn, U1, Self::ResidualStorage>::zeros(self.point_handles.len());
for (i, (p, c)) in self.moved.iter().zip(self.closest.iter()).enumerate() {
let v = p.point() - c.point();
let d = v.dot(&c.normal());
res[i] = self.weight[i] * d.abs();
}
Some(res)
}
fn jacobian(&self) -> Option<Matrix<f64, Dyn, Dyn, Self::JacobianStorage>> {
let mut jac = Matrix::<f64, Dyn, Dyn, Self::JacobianStorage>::zeros(
self.point_handles.len(),
self.params.params().len(),
);
for (i, (p, c)) in self.moved.iter().zip(self.closest.iter()).enumerate() {
let handle = &self.point_handles[i];
let test_i = handle.mesh_i;
let ref_i = handle.ref_i;
let values0 = point_plane_jacobian(&p.point(), &c.sp, &self.params.params[test_i]);
self.params
.set_jacobian(&mut jac, i, test_i, &(values0 * self.weight[i]));
let values1 = point_plane_jacobian_rev(&p.point(), &c.sp, &self.params.params[ref_i]);
self.params
.set_jacobian(&mut jac, i, ref_i, &(values1 * self.weight[i]));
}
Some(jac)
}
}
fn correspondence_matrix(meshes: &[AlignmentMesh], params: &GAPParams) -> DMatrix<f64> {
let mut matrix = DMatrix::<f64>::zeros(meshes.len(), meshes.len());
let mut work_list = Vec::new();
for i in 0..meshes.len() {
for j in i..meshes.len() {
if i != j {
work_list.push((i, j));
}
}
}
let collected = work_list
.par_iter()
.map(|&(i, j)| {
let t = meshes[i].transform().inv_mul(&meshes[j].transform());
let samples = generate_alignment_points(meshes[j].mesh, meshes[i].mesh, &t, params);
(i, j, samples.len() as f64)
})
.collect::<Vec<_>>();
for (i, j, count) in collected {
matrix[(i, j)] = count;
matrix[(j, i)] = count; }
matrix
}