use nalgebra::{Isometry3, Matrix3, Quaternion, Translation, UnitQuaternion, Vector3, Vector4};
type Iso3 = Isometry3<f32>;
type Mat3 = Matrix3<f32>;
type Vec3 = Vector3<f32>;
type Vec4 = Vector4<f32>;
#[derive(Debug, Copy, Clone)]
pub struct Pose {
pub rotation: [f32; 4],
pub translation: [f32; 3],
}
pub fn solve(world_3d_points: &[[f32; 3]; 3], bearing_vectors: &[[f32; 3]; 3]) -> Vec<Pose> {
compute_poses_nordberg(world_3d_points, bearing_vectors)
.into_iter()
.map(|(rot, trans)| {
let rotation = UnitQuaternion::from_matrix(&rot);
let translation = Translation::from(trans);
Pose::from_iso3(Iso3::from_parts(translation, rotation))
})
.collect()
}
pub fn error(point_3d: &[f32; 3], bearing_vector: &[f32; 3], pose: &Pose) -> f32 {
let new_bearing = (pose.to_iso3() * Vec3::from(*point_3d)).normalize();
let bearing_vector = Vec3::from(*bearing_vector).normalize();
1.0 - bearing_vector.dot(&new_bearing)
}
impl Pose {
fn from_iso3(iso3: Iso3) -> Self {
Self {
rotation: iso3.rotation.into_inner().coords.into(),
translation: iso3.translation.vector.into(),
}
}
fn to_iso3(&self) -> Iso3 {
let rot_quat = Quaternion::from(Vec4::from(self.rotation));
let rot = UnitQuaternion::from_quaternion(rot_quat);
let trans = Translation::from(Vec3::from(self.translation));
Iso3::from_parts(trans, rot)
}
}
#[allow(clippy::similar_names)]
fn gauss_newton_refine_lambda(
lambda: Vec3,
a12: f32,
a13: f32,
a23: f32,
b12: f32,
b13: f32,
b23: f32,
) -> 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..5 {
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) -> f32 {
v.x.abs() + v.y.abs() + v.z.abs()
}
fn root2real(b: f32, c: f32) -> (bool, f32, f32) {
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 = discriminant.sqrt();
(true, 0.5 * (-b + y), 0.5 * (-b - y))
} else {
let y = discriminant.sqrt();
(true, 2.0 * c / (-b + y), 2.0 * c / (-b - y))
}
}
#[allow(clippy::many_single_char_names)]
fn cube_root(b: f32, c: f32, d: f32) -> f32 {
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() {
std::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: f32| {
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)
}
#[allow(clippy::similar_names)]
fn compute_poses_nordberg(
world_3d_points: &[[f32; 3]; 3],
bearing_vectors: &[[f32; 3]; 3],
) -> Vec<(Mat3, Vec3)> {
let wp1 = Vec3::from(world_3d_points[0]);
let wp2 = Vec3::from(world_3d_points[1]);
let wp3 = Vec3::from(world_3d_points[2]);
let f1 = Vec3::from(bearing_vectors[0]);
let f2 = Vec3::from(bearing_vectors[1]);
let f3 = Vec3::from(bearing_vectors[2]);
let f1 = f1.normalize();
let f2 = f2.normalize();
let f3 = f3.normalize();
let d12 = wp1 - wp2;
let d13 = wp1 - wp3;
let d23 = wp2 - wp3;
let d12xd13 = d12.cross(&d13);
let a12 = d12.norm_squared();
let a13 = d13.norm_squared();
let a23 = d23.norm_squared();
let c12 = f1.dot(&f2);
let c23 = f2.dot(&f3);
let c31 = f3.dot(&f1);
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 = Vec::with_capacity(4);
let eigen_ratio = (0.0_f32.max(-eig_values[1] / eig_values[0])).sqrt();
let quadratic_coefficients = |ratio: f32| {
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: f32, w0: f32, w1: f32| {
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: f32, w0: f32, w1: f32| {
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: f32| {
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 = x_mat.try_inverse().expect("Woops not inversable");
lambdas
.iter()
.map(|&lambda| {
let lambda_refined = gauss_newton_refine_lambda(lambda, a12, a13, a23, b12, b13, b23);
let ry1 = lambda_refined[0] * f1;
let ry2 = lambda_refined[1] * f2;
let ry3 = lambda_refined[2] * f3;
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 * wp1)
})
.collect()
}
#[cfg(test)]
mod tests {
use super::*;
use approx::{assert_relative_eq, relative_eq};
use nalgebra::Point3;
use quickcheck_macros;
type V3 = (f32, f32, f32);
const EPSILON_APPROX: f32 = 1e-2;
#[test]
fn manual_case() {
let p1_cam = [-0.228_125, -0.061_458_334, 1.0];
let p2_cam = [0.418_75, -0.581_25, 2.0];
let p3_cam = [1.128_125, 0.878_125, 3.0];
let rot = UnitQuaternion::from_euler_angles(0.1, 0.2, 0.3);
let trans = Translation::from(Vec3::new(0.1, 0.2, 0.3));
let pose = Iso3::from_parts(trans, rot);
let p1_world = (pose.inverse() * Point3::from(p1_cam)).coords.into();
let p2_world = (pose.inverse() * Point3::from(p2_cam)).coords.into();
let p3_world = (pose.inverse() * Point3::from(p3_cam)).coords.into();
let poses = solve(&[p1_world, p2_world, p3_world], &[p1_cam, p2_cam, p3_cam]);
assert!(!poses.is_empty());
let p3p_iso3 = poses[0].to_iso3();
assert_relative_eq!(rot, p3p_iso3.rotation, epsilon = EPSILON_APPROX);
assert_relative_eq!(trans, p3p_iso3.translation, epsilon = EPSILON_APPROX);
}
#[quickcheck_macros::quickcheck]
fn non_degenerate_case(rot: V3, trans: V3, p1: V3, p2: V3, p3: V3) -> bool {
let p1_cam = [p1.0, p1.1, EPSILON_APPROX + p1.2.abs()];
let p2_cam = [p2.0, p2.1, EPSILON_APPROX + p2.2.abs()];
let p3_cam = [p3.0, p3.1, EPSILON_APPROX + p3.2.abs()];
let d12 = Vec3::from(p1_cam) - Vec3::from(p2_cam);
let d13 = Vec3::from(p1_cam) - Vec3::from(p3_cam);
if d12.norm() < EPSILON_APPROX || d13.norm() < EPSILON_APPROX {
return true;
}
let cosine = d12.normalize().dot(&d13.normalize());
if cosine.abs() > 1.0 - EPSILON_APPROX {
return true;
}
if cosine.abs() < EPSILON_APPROX {
return true;
}
let rotation = UnitQuaternion::from_euler_angles(rot.0, rot.1, rot.2);
let translation = Translation::from(Vec3::new(trans.0, trans.1, trans.2));
let pose = Iso3::from_parts(translation, rotation);
let p1_world = (pose.inverse() * Point3::from(p1_cam)).coords.into();
let p2_world = (pose.inverse() * Point3::from(p2_cam)).coords.into();
let p3_world = (pose.inverse() * Point3::from(p3_cam)).coords.into();
let poses = solve(&[p1_world, p2_world, p3_world], &[p1_cam, p2_cam, p3_cam]);
if poses.is_empty() {
eprintln!("cosine: {}", cosine);
}
assert!(!poses.is_empty());
poses.iter().fold(false, |already_true, pose| {
let p3p_pose = pose.to_iso3();
let same_rot = relative_eq!(rotation, p3p_pose.rotation, epsilon = EPSILON_APPROX);
let same_trans =
relative_eq!(translation, p3p_pose.translation, epsilon = EPSILON_APPROX);
already_true || (same_rot && same_trans)
})
}
}