use super::Cylinder3;
use crate::common::PCoords;
use crate::common::consensus::{ConsensusModel, Magsac};
use crate::geom3::IsoExtensions3;
use crate::{Iso3, Point3, Result, SurfacePoint3, UnitVec3, Vector3};
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
use parry3d_f64::na::{Dyn, Matrix, Matrix3, Owned, U1, U5, UnitQuaternion, Vector, Vector5};
fn point_cylinder_distance(
center: &Point3,
direction: &UnitVec3,
radius: f64,
point: &Point3,
) -> f64 {
let v = point - center;
let axial = v.dot(&direction.into_inner());
let radial = v - direction.into_inner() * axial;
radial.norm() - radius
}
fn axis_from_normals(points: &[SurfacePoint3], weights: &[f64]) -> Option<UnitVec3> {
let mut m = Matrix3::<f64>::zeros();
for (sp, &w) in points.iter().zip(weights) {
let n = sp.normal.into_inner();
m += w * n * n.transpose();
}
let eigen = m.symmetric_eigen();
let ev = eigen.eigenvalues;
let mut order = [0usize, 1, 2];
order.sort_by(|&a, &b| ev[a].partial_cmp(&ev[b]).unwrap());
let (smallest, second, largest) = (order[0], order[1], order[2]);
if ev[largest] <= 0.0 || ev[second] / ev[largest] < 1e-6 {
return None;
}
let axis = eigen.eigenvectors.column(smallest).into_owned();
UnitVec3::try_new(axis, 1e-9)
}
fn axis_point_and_radius(
direction: &UnitVec3,
points: &[Point3],
weights: &[f64],
) -> Option<(Point3, f64)> {
let arbitrary = Iso3::from_z_arbitrary_xy(direction, None);
let u = arbitrary.x().into_inner();
let v = arbitrary.y().into_inner();
let mut wsum = 0.0;
let mut acc = Vector3::zeros();
for (p, &w) in points.iter().zip(weights) {
acc += w * p.coords;
wsum += w;
}
if wsum <= 0.0 {
return None;
}
let origin = Point3::from(acc / wsum);
let mut m = Matrix3::<f64>::zeros();
let mut b = Vector3::zeros();
for (p, &w) in points.iter().zip(weights) {
let d = p - origin;
let a = d.dot(&u);
let bb = d.dot(&v);
let row = Vector3::new(a, bb, 1.0);
let target = a * a + bb * bb;
m += w * row * row.transpose();
b += w * target * row;
}
let s = m.lu().solve(&b)?;
let cu = 0.5 * s[0];
let cv = 0.5 * s[1];
let r_squared = s[2] + cu * cu + cv * cv;
if r_squared <= 0.0 {
return None;
}
let axis_point = origin + u * cu + v * cv;
Some((axis_point, r_squared.sqrt()))
}
fn bootstrap_cylinder(points: &[SurfacePoint3], weights: &[f64]) -> Option<Cylinder3> {
let direction = axis_from_normals(points, weights)?;
let pts: Vec<Point3> = points.iter().map(|sp| sp.point).collect();
let (axis_point, radius) = axis_point_and_radius(&direction, &pts, weights)?;
Some(Cylinder3::new(axis_point, direction, radius, 0.0))
}
fn bound_axially(cyl: &Cylinder3, points: &[Point3]) -> Cylinder3 {
let axis = cyl.axis();
let (mut t_min, mut t_max) = (f64::INFINITY, f64::NEG_INFINITY);
for p in points {
let t = axis.scalar_project(p);
t_min = t_min.min(t);
t_max = t_max.max(t);
}
let center = axis.at(t_min);
Cylinder3::new(center, cyl.direction, cyl.radius, (t_max - t_min).max(0.0))
}
impl Cylinder3 {
pub fn from_fit(
points: &[impl PCoords<3>],
guess: &Cylinder3,
weights: Option<&[f64]>,
) -> Result<Self> {
let pts: Vec<Point3> = points.iter().map(|p| Point3::from(p.coords())).collect();
if pts.len() < 2 {
return Err("At least two points are required to fit a cylinder".into());
}
let ones;
let weights = match weights {
Some(w) => w,
None => {
ones = vec![1.0; pts.len()];
&ones
}
};
let refined =
CylinderFit::refine(&pts, weights, guess).ok_or("Failed to refine cylinder fit")?;
Ok(bound_axially(&refined, &pts))
}
pub fn from_fit_oriented(points: &[SurfacePoint3], weights: Option<&[f64]>) -> Result<Self> {
if points.len() < 3 {
return Err("At least three oriented points are required to fit a cylinder".into());
}
let ones;
let weights = match weights {
Some(w) => w,
None => {
ones = vec![1.0; points.len()];
&ones
}
};
let guess = bootstrap_cylinder(points, weights).ok_or(
"Failed to estimate an initial cylinder from the oriented points \
(parallel normals or degenerate sample)",
)?;
let pts: Vec<Point3> = points.iter().map(|sp| sp.point).collect();
let refined =
CylinderFit::refine(&pts, weights, &guess).ok_or("Failed to refine cylinder fit")?;
Ok(bound_axially(&refined, &pts))
}
pub fn from_consensus(
points: &[SurfacePoint3],
sigma_max: f64,
min_r: Option<f64>,
max_r: Option<f64>,
options: Option<Magsac>,
) -> Result<Cylinder3> {
let min_r = min_r.unwrap_or(0.0);
let max_r = max_r.unwrap_or(f64::INFINITY);
let mut magsac = options.unwrap_or_else(|| Magsac::new(sigma_max));
magsac.sigma_max = sigma_max;
let fit =
magsac.fit_filtered::<3, Cylinder3, _>(points, |c| c.r() >= min_r && c.r() <= max_r)?;
let bound_pts: Vec<Point3> = if fit.inliers.is_empty() {
points.iter().map(|sp| sp.point).collect()
} else {
fit.inliers.iter().map(|&i| points[i].point).collect()
};
Ok(bound_axially(&fit.model, &bound_pts))
}
}
impl ConsensusModel<3> for Cylinder3 {
type Point = SurfacePoint3;
const SAMPLE_SIZE: usize = 3;
fn from_sample(sample: &[SurfacePoint3]) -> Option<Self> {
let weights = vec![1.0; sample.len()];
bootstrap_cylinder(sample, &weights)
}
fn residual(&self, point: &SurfacePoint3) -> f64 {
point_cylinder_distance(&self.center, &self.direction, self.radius, &point.point)
}
fn refine_weighted(
points: &[SurfacePoint3],
weights: &[f64],
initial: &Cylinder3,
) -> Option<Cylinder3> {
let pts: Vec<Point3> = points.iter().map(|sp| sp.point).collect();
CylinderFit::refine(&pts, weights, initial)
}
}
type Residuals = Matrix<f64, Dyn, U1, Owned<f64, Dyn, U1>>;
struct CylinderFit<'a> {
points: &'a [Point3],
weights: &'a [f64],
base_center: Point3,
base_direction: UnitVec3,
base_r: f64,
axis_x: UnitVec3,
axis_y: UnitVec3,
params: Vector5<f64>,
residuals: Residuals,
}
impl<'a> CylinderFit<'a> {
fn new(points: &'a [Point3], weights: &'a [f64], base: &Cylinder3) -> Self {
let arbitrary = Iso3::from_z_arbitrary_xy(&base.direction, None);
let axis_x = arbitrary.x();
let axis_y = arbitrary.y();
let mut problem = Self {
points,
weights,
base_center: base.center,
base_direction: base.direction,
base_r: base.r(),
axis_x,
axis_y,
params: Vector5::zeros(),
residuals: Residuals::zeros(points.len()),
};
problem.set_params(&Vector5::zeros());
problem
}
fn refine(points: &[Point3], weights: &[f64], initial: &Cylinder3) -> Option<Cylinder3> {
let problem = CylinderFit::new(points, weights, initial);
let (result, report) = LevenbergMarquardt::new().minimize(problem);
report.termination.was_successful().then(|| {
let (center, direction, radius) = result.cylinder_params(&result.params);
Cylinder3::new(center, direction, radius, 0.0)
})
}
fn cylinder_params(&self, p: &Vector5<f64>) -> (Point3, UnitVec3, f64) {
let center =
self.base_center + self.axis_x.into_inner() * p[0] + self.axis_y.into_inner() * p[1];
let rot_x = UnitQuaternion::from_axis_angle(&self.axis_x, p[2]);
let rot_y = UnitQuaternion::from_axis_angle(&self.axis_y, p[3]);
let direction = UnitVec3::new_normalize(rot_y * (rot_x * self.base_direction.into_inner()));
let radius = (self.base_r + p[4]).max(1e-9);
(center, direction, radius)
}
}
impl LeastSquaresProblem<f64, Dyn, U5> for CylinderFit<'_> {
type ResidualStorage = Owned<f64, Dyn, U1>;
type JacobianStorage = Owned<f64, Dyn, U5>;
type ParameterStorage = Owned<f64, U5>;
fn set_params(&mut self, x: &Vector<f64, U5, Self::ParameterStorage>) {
self.params = *x;
let (center, direction, radius) = self.cylinder_params(&self.params);
for i in 0..self.points.len() {
self.residuals[i] = self.weights[i]
* point_cylinder_distance(¢er, &direction, radius, &self.points[i]);
}
}
fn params(&self) -> Vector<f64, U5, Self::ParameterStorage> {
self.params
}
fn residuals(&self) -> Option<Vector<f64, Dyn, Self::ResidualStorage>> {
Some(self.residuals.clone())
}
fn jacobian(&self) -> Option<Matrix<f64, Dyn, U5, Self::JacobianStorage>> {
let (center, direction, _radius) = self.cylinder_params(&self.params);
let n = direction.into_inner();
let axis_x = self.axis_x.into_inner();
let axis_y = self.axis_y.into_inner();
let rot_x = UnitQuaternion::from_axis_angle(&self.axis_x, self.params[2]);
let n1 = rot_x * self.base_direction.into_inner();
let rot_y = UnitQuaternion::from_axis_angle(&self.axis_y, self.params[3]);
let dn_dp2 = rot_y * axis_x.cross(&n1);
let dn_dp3 = axis_y.cross(&n);
let mut jac = Matrix::<f64, Dyn, U5, Self::JacobianStorage>::zeros(self.points.len());
for (i, point) in self.points.iter().enumerate() {
let w = self.weights[i];
let v = point - center;
let a = v.dot(&n);
let radial = v - n * a;
let rho = radial.norm();
let u = if rho > 1e-12 {
radial / rho
} else {
Vector3::zeros()
};
let grad_n = -u * a;
jac[(i, 0)] = -w * u.dot(&axis_x);
jac[(i, 1)] = -w * u.dot(&axis_y);
jac[(i, 2)] = w * grad_n.dot(&dn_dp2);
jac[(i, 3)] = w * grad_n.dot(&dn_dp3);
jac[(i, 4)] = -w;
}
Some(jac)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::random_geometry::RandomGeometry3;
use crate::geom3::Cylinder3;
use crate::{Point3, UnitVec3, Vector3};
use approx::assert_relative_eq;
use levenberg_marquardt::LeastSquaresProblem;
use parry3d_f64::na::Vector5;
use std::f64::consts::TAU;
fn tilted() -> Cylinder3 {
let center = Point3::new(1.0, 2.0, 3.0);
let direction = UnitVec3::new_normalize(Vector3::new(0.3, -0.5, 1.0));
Cylinder3::new(center, direction, 1.5, 6.0)
}
fn sample_surface(cyl: &Cylinder3, n: usize, outward: bool) -> Vec<SurfacePoint3> {
let arbitrary = Iso3::from_z_arbitrary_xy(&cyl.direction, None);
let u = arbitrary.x().into_inner();
let v = arbitrary.y().into_inner();
let axis = cyl.axis();
(0..n)
.map(|i| {
let f = i as f64 / (n - 1) as f64;
let t = cyl.length * f;
let ang = TAU * i as f64 * 0.6180339887;
let radial = u * ang.cos() + v * ang.sin();
let point = axis.at(t) + radial * cyl.r();
let normal = if outward { radial } else { -radial };
SurfacePoint3::new_normalize(point, normal)
})
.collect()
}
fn with_noise(
rg: &mut RandomGeometry3,
points: &[SurfacePoint3],
sigma: f64,
) -> Vec<SurfacePoint3> {
points
.iter()
.map(|sp| SurfacePoint3::new(sp.point + rg.gaussian_vector::<3>(sigma), sp.normal))
.collect()
}
#[test]
fn jacobian_matches_finite_differences() {
let cyl = tilted();
let surface = sample_surface(&cyl, 30, true);
let points: Vec<Point3> = surface.iter().map(|sp| sp.point).collect();
let weights = vec![1.0; points.len()];
let mut problem = CylinderFit::new(&points, &weights, &cyl);
let probe = Vector5::new(0.05, -0.03, 0.2, -0.15, 0.1);
problem.set_params(&probe);
let analytic = problem.jacobian().expect("analytic jacobian");
let base_residuals = problem.residuals().expect("residuals");
const DELTA: f64 = 1e-6;
for k in 0..5 {
let mut perturbed = probe;
perturbed[k] += DELTA;
problem.set_params(&perturbed);
let perturbed_residuals = problem.residuals().expect("residuals");
problem.set_params(&probe);
for i in 0..points.len() {
let numeric = (perturbed_residuals[i] - base_residuals[i]) / DELTA;
assert_relative_eq!(analytic[(i, k)], numeric, epsilon = 1e-4);
}
}
}
fn assert_cyl_matches(result: &Cylinder3, expected: &Cylinder3, eps: f64) {
assert_relative_eq!(result.r(), expected.r(), epsilon = eps);
assert_relative_eq!(result.length, expected.length, epsilon = eps);
let dot = result
.direction
.into_inner()
.dot(&expected.direction.into_inner())
.abs();
assert_relative_eq!(dot, 1.0, epsilon = eps);
assert!(expected.axis().distance_to(&result.center) < eps);
assert!(result.axis().distance_to(&expected.center) < eps);
}
#[test]
fn from_fit_oriented_recovers_clean_cylinder() {
let expected = tilted();
let points = sample_surface(&expected, 60, true);
let fit = Cylinder3::from_fit_oriented(&points, None).unwrap();
assert_cyl_matches(&fit, &expected, 1e-6);
}
#[test]
fn from_fit_oriented_inner_facing_matches_outer_facing() {
let expected = tilted();
let outer =
Cylinder3::from_fit_oriented(&sample_surface(&expected, 60, true), None).unwrap();
let inner =
Cylinder3::from_fit_oriented(&sample_surface(&expected, 60, false), None).unwrap();
assert_cyl_matches(&inner, &expected, 1e-6);
assert_relative_eq!(inner.center, outer.center, epsilon = 1e-9);
assert_relative_eq!(
inner.direction.into_inner(),
outer.direction.into_inner(),
epsilon = 1e-9
);
assert_relative_eq!(inner.r(), outer.r(), epsilon = 1e-9);
assert_relative_eq!(inner.length, outer.length, epsilon = 1e-9);
}
#[test]
fn from_fit_oriented_with_noise_is_close() {
let mut rg = RandomGeometry3::from_seed(8);
let expected = tilted();
let clean = sample_surface(&expected, 400, true);
let points = with_noise(&mut rg, &clean, 0.01);
let fit = Cylinder3::from_fit_oriented(&points, None).unwrap();
assert_cyl_matches(&fit, &expected, 0.05);
}
#[test]
fn from_fit_oriented_uniform_weights_match_unweighted() {
let mut rg = RandomGeometry3::from_seed(21);
let expected = tilted();
let points = with_noise(&mut rg, &sample_surface(&expected, 40, true), 0.02);
let unweighted = Cylinder3::from_fit_oriented(&points, None).unwrap();
let weights = vec![1.0; points.len()];
let weighted = Cylinder3::from_fit_oriented(&points, Some(&weights)).unwrap();
assert_relative_eq!(weighted.center, unweighted.center, epsilon = 1e-9);
assert_relative_eq!(
weighted.direction.into_inner(),
unweighted.direction.into_inner(),
epsilon = 1e-9
);
assert_relative_eq!(weighted.r(), unweighted.r(), epsilon = 1e-9);
}
#[test]
fn from_fit_oriented_too_few_points_is_error() {
let expected = tilted();
let points = sample_surface(&expected, 2, true);
assert!(Cylinder3::from_fit_oriented(&points, None).is_err());
}
#[test]
fn from_fit_oriented_parallel_normals_is_error() {
let expected = tilted();
let arbitrary = Iso3::from_z_arbitrary_xy(&expected.direction, None);
let u = arbitrary.x().into_inner();
let axis = expected.axis();
let points: Vec<SurfacePoint3> = (0..30)
.map(|i| {
let t = expected.length * i as f64 / 29.0;
SurfacePoint3::new_normalize(axis.at(t) + u * expected.r(), u)
})
.collect();
assert!(Cylinder3::from_fit_oriented(&points, None).is_err());
}
#[test]
fn from_fit_recovers_clean_cylinder_from_guess() {
let expected = tilted();
let surface = sample_surface(&expected, 60, true);
let points: Vec<Point3> = surface.iter().map(|sp| sp.point).collect();
let guess = Cylinder3::new(
expected.center + Vector3::new(0.2, -0.15, 0.1),
UnitVec3::new_normalize(
expected.direction.into_inner() + Vector3::new(0.05, 0.05, 0.0),
),
expected.r() + 0.3,
expected.length,
);
let fit = Cylinder3::from_fit(&points, &guess, None).unwrap();
assert_cyl_matches(&fit, &expected, 1e-6);
}
#[test]
fn from_fit_with_noise_is_close() {
let mut rg = RandomGeometry3::from_seed(13);
let expected = tilted();
let surface = with_noise(&mut rg, &sample_surface(&expected, 400, true), 0.01);
let points: Vec<Point3> = surface.iter().map(|sp| sp.point).collect();
let guess = Cylinder3::new(
expected.center + Vector3::new(0.1, 0.1, -0.1),
expected.direction,
expected.r() + 0.2,
expected.length,
);
let fit = Cylinder3::from_fit(&points, &guess, None).unwrap();
assert_cyl_matches(&fit, &expected, 0.05);
}
#[test]
fn from_fit_too_few_points_is_error() {
let guess = tilted();
let points = [Point3::new(0.0, 0.0, 0.0)];
assert!(Cylinder3::from_fit(&points, &guess, None).is_err());
}
#[test]
fn from_consensus_recovers_clean_cylinder() {
let expected = tilted();
let points = sample_surface(&expected, 120, true);
let options = Magsac {
sigma_max: 0.01,
max_iterations: Some(500),
refinement_steps: 5,
confidence: 0.99,
seed: Some(17),
};
let fit = Cylinder3::from_consensus(&points, 0.01, None, None, Some(options)).unwrap();
assert_cyl_matches(&fit, &expected, 5e-3);
}
#[test]
fn from_consensus_rejects_outliers() {
let mut rg = RandomGeometry3::from_seed(29);
let expected = tilted();
let inliers = with_noise(&mut rg, &sample_surface(&expected, 150, true), 0.01);
let inlier_count = inliers.len();
let mut points = inliers.clone();
let center = expected.center + Vector3::new(6.0, -5.0, 4.0);
for _ in 0..50 {
let p = center + rg.gaussian_vector::<3>(1.0);
points.push(SurfacePoint3::new_normalize(p, rg.unit_vec().into_inner()));
}
let magsac = Magsac {
sigma_max: 0.03,
max_iterations: Some(500),
refinement_steps: 5,
confidence: 0.99,
seed: Some(17),
};
let fit = magsac
.fit_filtered::<3, Cylinder3, _>(&points, |_| true)
.unwrap();
assert_relative_eq!(fit.model.r(), expected.r(), epsilon = 0.05);
let dot = fit
.model
.direction
.into_inner()
.dot(&expected.direction.into_inner())
.abs();
assert_relative_eq!(dot, 1.0, epsilon = 1e-2);
assert!(expected.axis().distance_to(&fit.model.center) < 0.05);
assert!(fit.inliers.iter().all(|&i| i < inlier_count));
assert!(fit.inliers.len() > inlier_count * 9 / 10);
}
#[test]
fn from_consensus_too_few_points_is_error() {
let expected = tilted();
let points = sample_surface(&expected, 2, true);
assert!(Cylinder3::from_consensus(&points, 0.01, None, None, None).is_err());
}
}