#![no_std]
#![warn(missing_docs)]
use arrayvec::ArrayVec;
use num_traits::Float;
use cv_core::nalgebra::{Matrix3, Rotation3, Vector3};
use cv_core::sample_consensus::Estimator;
use cv_core::{Bearing, FeatureWorldMatch, Pose, Projective, WorldToCamera};
type Mat3 = Matrix3<f64>;
type Vec3 = Vector3<f64>;
#[derive(Copy, Clone, Debug, PartialEq)]
#[non_exhaustive]
pub struct LambdaTwist {
pub gauss_newton_iterations: usize,
pub rotation_convergence_iterations: usize,
pub rotation_convergence_epsilon: f64,
}
impl LambdaTwist {
pub fn new() -> Self {
Self::default()
}
pub fn gauss_newton_iterations(self, gauss_newton_iterations: usize) -> Self {
Self {
gauss_newton_iterations,
..self
}
}
pub fn rotation_convergence_iterations(self, rotation_convergence_iterations: usize) -> Self {
Self {
rotation_convergence_iterations,
..self
}
}
pub fn rotation_convergence_epsilon(self, rotation_convergence_epsilon: f64) -> Self {
Self {
rotation_convergence_epsilon,
..self
}
}
fn compute_poses_nordberg<P: Bearing>(
&self,
samples: [FeatureWorldMatch<P>; 3],
) -> ArrayVec<[WorldToCamera; 4]> {
let to_wp = |&FeatureWorldMatch(_, point)| point.point();
let wps = [
if let Some(p) = to_wp(&samples[0]) {
p
} else {
return ArrayVec::new();
},
if let Some(p) = to_wp(&samples[1]) {
p
} else {
return ArrayVec::new();
},
if let Some(p) = to_wp(&samples[2]) {
p
} else {
return ArrayVec::new();
},
];
let to_bearing = |FeatureWorldMatch(point, _): &FeatureWorldMatch<P>| point.bearing();
let bearings = [
to_bearing(&samples[0]),
to_bearing(&samples[1]),
to_bearing(&samples[2]),
];
let d12 = wps[0] - wps[1];
let d13 = wps[0] - wps[2];
let d23 = wps[1] - wps[2];
let d12xd13 = d12.cross(&d13);
let a12 = d12.norm_squared();
let a13 = d13.norm_squared();
let a23 = d23.norm_squared();
let c12 = bearings[0].dot(&bearings[1]);
let c23 = bearings[1].dot(&bearings[2]);
let c31 = bearings[2].dot(&bearings[0]);
let blob = c12 * c23 * c31 - 1.0;
let s12_sqr = 1.0 - c12 * c12;
let s23_sqr = 1.0 - c23 * c23;
let s31_sqr = 1.0 - c31 * c31;
let b12 = -2.0 * c12;
let b13 = -2.0 * c31;
let b23 = -2.0 * c23;
let p3 = a13 * (a23 * s31_sqr - a13 * s23_sqr);
let p2 = 2.0 * blob * a23 * a13
+ a13 * (2.0 * a12 + a13) * s23_sqr
+ a23 * (a23 - a12) * s31_sqr;
let p1 = a23 * (a13 - a23) * s12_sqr
- a12 * a12 * s23_sqr
- 2.0 * a12 * (blob * a23 + a13 * s23_sqr);
let p0 = a12 * (a12 * s23_sqr - a23 * s12_sqr);
let g = cube_root(p2 / p3, p1 / p3, p0 / p3);
let d0_00 = a23 * (1.0 - g);
let d0_01 = -(a23 * c12);
let d0_02 = a23 * c31 * g;
let d0_11 = a23 - a12 + a13 * g;
let d0_12 = -c23 * (a13 * g - a12);
let d0_22 = g * (a13 - a23) - a12;
#[rustfmt::skip]
let d0_mat = Mat3::new(
d0_00, d0_01, d0_02,
d0_01, d0_11, d0_12,
d0_02, d0_12, d0_22,
);
let (eig_vectors, eig_values) = eigen_decomposition_singular(d0_mat);
let mut lambdas: ArrayVec<[Vec3; 4]> = ArrayVec::new();
let eigen_ratio = (0.0_f64.max(-eig_values[1] / eig_values[0])).sqrt();
let quadratic_coefficients = |ratio: f64| {
let w2 = 1.0 / (ratio * eig_vectors.m12 - eig_vectors.m11);
let w0 = w2 * (eig_vectors.m21 - ratio * eig_vectors.m22);
let w1 = w2 * (eig_vectors.m31 - ratio * eig_vectors.m32);
let a = 1.0 / ((a13 - a12) * w1 * w1 - a12 * b13 * w1 - a12);
let b = a * (a13 * b12 * w1 - a12 * b13 * w0 - 2.0 * w0 * w1 * (a12 - a13));
let c = a * ((a13 - a12) * w0 * w0 + a13 * b12 * w0 + a13);
(w0, w1, b, c)
};
let possible_depths = |tau: f64, w0: f64, w1: f64| {
let d = a23 / (tau * (b23 + tau) + 1.0);
if d > 0.0 {
let l2 = d.sqrt();
let l3 = tau * l2;
let l1 = w0 * l2 + w1 * l3;
(true, l1, l2, l3)
} else {
(false, 0.0, 0.0, 0.0)
}
};
let mut push_solution = |tau: f64, w0: f64, w1: f64| {
if tau > 0.0 {
let (valid, l1, l2, l3) = possible_depths(tau, w0, w1);
if valid && l1 >= 0.0 {
lambdas.push(Vec3::new(l1, l2, l3));
}
}
};
let mut push_solutions_to_lambdas = |ratio: f64| {
let (w0, w1, b, c) = quadratic_coefficients(ratio);
if b * b - 4.0 * c >= 0.0 {
let (_, tau1, tau2) = root2real(b, c);
push_solution(tau1, w0, w1);
push_solution(tau2, w0, w1);
}
};
push_solutions_to_lambdas(eigen_ratio);
push_solutions_to_lambdas(-eigen_ratio);
#[rustfmt::skip]
let x_mat = Mat3::new(
d12[0], d13[0], d12xd13[0],
d12[1], d13[1], d12xd13[1],
d12[2], d13[2], d12xd13[2],
);
let x_mat = if let Some(x_mat) = x_mat.try_inverse() {
x_mat
} else {
return ArrayVec::new();
};
lambdas
.iter()
.map(|&lambda| {
let lambda_refined = gauss_newton_refine_lambda(
lambda,
self.gauss_newton_iterations,
a12,
a13,
a23,
b12,
b13,
b23,
);
let ry1 = lambda_refined[0] * *bearings[0];
let ry2 = lambda_refined[1] * *bearings[1];
let ry3 = lambda_refined[2] * *bearings[2];
let yd1 = ry1 - ry2;
let yd2 = ry1 - ry3;
let yd1xd2 = yd1.cross(&yd2);
#[rustfmt::skip]
let y_mat = Mat3::new(
yd1[0], yd2[0], yd1xd2[0],
yd1[1], yd2[1], yd1xd2[1],
yd1[2], yd2[2], yd1xd2[2],
);
let rot = y_mat * x_mat;
(rot, ry1 - rot * wps[0].coords)
})
.map(|(rot, trans)| {
WorldToCamera::from_parts(
trans,
Rotation3::from_matrix_eps(
&rot,
self.rotation_convergence_epsilon,
self.rotation_convergence_iterations,
Rotation3::identity(),
),
)
})
.collect()
}
}
impl Default for LambdaTwist {
fn default() -> Self {
Self {
gauss_newton_iterations: 5,
rotation_convergence_iterations: 100,
rotation_convergence_epsilon: 1e-12,
}
}
}
impl<P> Estimator<FeatureWorldMatch<P>> for LambdaTwist
where
P: Bearing,
{
type Model = WorldToCamera;
type ModelIter = ArrayVec<[WorldToCamera; 4]>;
const MIN_SAMPLES: usize = 3;
fn estimate<I>(&self, mut data: I) -> Self::ModelIter
where
I: Iterator<Item = FeatureWorldMatch<P>> + Clone,
{
self.compute_poses_nordberg([
data.next()
.expect("must provide 3 samples at minimum to LambdaTwist"),
data.next()
.expect("must provide 3 samples at minimum to LambdaTwist"),
data.next()
.expect("must provide 3 samples at minimum to LambdaTwist"),
])
}
}
#[allow(clippy::similar_names)]
#[allow(clippy::too_many_arguments)]
fn gauss_newton_refine_lambda(
lambda: Vec3,
iterations: usize,
a12: f64,
a13: f64,
a23: f64,
b12: f64,
b13: f64,
b23: f64,
) -> Vec3 {
let compute_residual = |l: &Vec3| {
let l1 = l.x;
let l2 = l.y;
let l3 = l.z;
let r1 = l1 * l1 + l2 * l2 + b12 * l1 * l2 - a12;
let r2 = l1 * l1 + l3 * l3 + b13 * l1 * l3 - a13;
let r3 = l2 * l2 + l3 * l3 + b23 * l2 * l3 - a23;
(l1, l2, l3, Vec3::new(r1, r2, r3))
};
let (mut l1, mut l2, mut l3, mut res) = compute_residual(&lambda);
for _ in 0..iterations {
if l1_norm(res) < 1e-10 {
break;
}
let dr1dl1 = 2.0 * l1 + b12 * l2;
let dr1dl2 = 2.0 * l2 + b12 * l1;
let dr2dl1 = 2.0 * l1 + b13 * l3;
let dr2dl3 = 2.0 * l3 + b13 * l1;
let dr3dl2 = 2.0 * l2 + b23 * l3;
let dr3dl3 = 2.0 * l3 + b23 * l2;
let det = 1.0 / (-dr1dl1 * dr2dl3 * dr3dl2 - dr1dl2 * dr2dl1 * dr3dl3);
#[rustfmt::skip]
let jacobian = Mat3::new(
-dr2dl3 * dr3dl2, -dr1dl2 * dr3dl3, dr1dl2 * dr2dl3,
-dr2dl1 * dr3dl3, dr1dl1 * dr3dl3, -dr1dl1 * dr2dl3,
dr2dl1 * dr3dl2, -dr1dl1 * dr3dl2, -dr1dl2 * dr2dl1,
);
let lambda_new = Vec3::new(l1, l2, l3) - det * (jacobian * res);
let (l1_new, l2_new, l3_new, res_new) = compute_residual(&lambda_new);
if l1_norm(res_new) > l1_norm(res) {
break;
} else {
l1 = l1_new;
l2 = l2_new;
l3 = l3_new;
res = res_new;
}
}
Vec3::new(l1, l2, l3)
}
#[inline]
fn l1_norm(v: Vec3) -> f64 {
v.x.abs() + v.y.abs() + v.z.abs()
}
fn root2real(b: f64, c: f64) -> (bool, f64, f64) {
let discriminant = b * b - 4.0 * c;
if discriminant < 0.0 {
let root = 0.5 * b;
(false, root, root)
} else if b < 0.0 {
let y = Float::sqrt(discriminant);
(true, 0.5 * (-b + y), 0.5 * (-b - y))
} else {
let y = Float::sqrt(discriminant);
(true, 2.0 * c / (-b + y), 2.0 * c / (-b - y))
}
}
#[allow(clippy::many_single_char_names)]
fn cube_root(b: f64, c: f64, d: f64) -> f64 {
let mut r0;
if b * b >= 3.0 * c {
let v = (b * b - 3.0 * c).sqrt();
let t1 = (-b - v) / 3.0;
let mut k = ((t1 + b) * t1 + c) * t1 + d;
if k > 0.0 {
r0 = t1 - (-k / (3.0 * t1 + b)).sqrt();
} else {
let t2 = (-b + v) / 3.0;
k = ((t2 + b) * t2 + c) * t2 + d;
r0 = t2 + (-k / (3.0 * t2 + b)).sqrt();
}
} else {
r0 = -b / 3.0;
if ((3.0 * r0 + 2.0 * b) * r0 + c).abs() < 1e-4 {
r0 += 1.0;
}
}
for _ in 0..7 {
let fx = ((r0 + b) * r0 + c) * r0 + d;
let fpx = (3.0 * r0 + 2.0 * b) * r0 + c;
r0 -= fx / fpx;
}
for _ in 0..43 {
let fx = ((r0 + b) * r0 + c) * r0 + d;
if fx.abs() > 1e-13 {
let fpx = (3.0 * r0 + 2.0 * b) * r0 + c;
r0 -= fx / fpx;
} else {
break;
}
}
r0
}
fn eigen_decomposition_singular(x: Mat3) -> (Mat3, Vec3) {
let mut eigenvalues = Vec3::zeros();
#[rustfmt::skip]
let mut v3 = Vec3::new(
x[1] * x[5] - x[2] * x[4],
x[2] * x[3] - x[5] * x[0],
x[4] * x[0] - x[1] * x[3],
);
v3.normalize_mut();
let x12_sqr = x.m12 * x.m12;
let b = -x.m11 - x.m22 - x.m33;
let c = -x12_sqr - x.m13 * x.m13 - x.m23 * x.m23 + x.m11 * (x.m22 + x.m33) + x.m22 * x.m33;
let (_, mut e1, mut e2) = root2real(b, c);
if e1.abs() < e2.abs() {
core::mem::swap(&mut e1, &mut e2);
}
eigenvalues[0] = e1;
eigenvalues[1] = e2;
let mx0011 = -x.m11 * x.m22;
let prec_0 = x.m12 * x.m23 - x.m13 * x.m22;
let prec_1 = x.m12 * x.m13 - x.m11 * x.m23;
let compute_eigen_vector = |e: f64| {
let tmp = 1.0 / (e * (x.m11 + x.m22) + mx0011 - e * e + x12_sqr);
let mut a1 = -(e * x.m13 + prec_0) * tmp;
let mut a2 = -(e * x.m23 + prec_1) * tmp;
let rnorm = 1.0 / (a1 * a1 + a2 * a2 + 1.0).sqrt();
a1 *= rnorm;
a2 *= rnorm;
Vec3::new(a1, a2, rnorm)
};
let v1 = compute_eigen_vector(e1);
let v2 = compute_eigen_vector(e2);
#[rustfmt::skip]
let eigenvectors = Mat3::new(
v1[0], v2[0], v3[0],
v1[1], v2[1], v3[1],
v1[2], v2[2], v3[2],
);
(eigenvectors, eigenvalues)
}