253 lines
9.4 KiB
Rust
253 lines
9.4 KiB
Rust
//! Point triangulation, mirroring `colmap/geometry/triangulation.h`.
|
|
//!
|
|
//! Provides linear (DLT) two-view triangulation, the mid-point method, a
|
|
//! multi-view DLT, and helpers to compute triangulation angles between observing
|
|
//! rays.
|
|
|
|
use crate::math::{Mat3x4, Vec2, Vec3};
|
|
use nalgebra::{Matrix4, Vector4};
|
|
|
|
/// Triangulates a 3D point from two views using the linear DLT (Direct Linear
|
|
/// Transform) method.
|
|
///
|
|
/// `p1`, `p2` are the `[R | t]` (or full projection) matrices and `x1`, `x2` the
|
|
/// corresponding image points (in the same coordinate system as the projection
|
|
/// matrices, typically normalized camera coordinates). Returns `None` if the
|
|
/// system is degenerate.
|
|
///
|
|
/// # Examples
|
|
/// ```
|
|
/// use colmap::geometry::triangulation::triangulate_point;
|
|
/// use colmap::geometry::Rigid3d;
|
|
/// use colmap::math::{UnitQuat, Vec2, Vec3};
|
|
///
|
|
/// let p1 = Rigid3d::identity().to_matrix();
|
|
/// let pose2 = Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0));
|
|
/// let p2 = pose2.to_matrix();
|
|
/// let point = Vec3::new(0.2, -0.1, 5.0);
|
|
/// let x1 = Vec2::new(point.x / point.z, point.y / point.z);
|
|
/// let pc2 = pose2.transform_point(&point);
|
|
/// let x2 = Vec2::new(pc2.x / pc2.z, pc2.y / pc2.z);
|
|
/// let tri = triangulate_point(&p1, &p2, &x1, &x2).unwrap();
|
|
/// assert!((tri - point).norm() < 1e-9);
|
|
/// ```
|
|
pub fn triangulate_point(p1: &Mat3x4, p2: &Mat3x4, x1: &Vec2, x2: &Vec2) -> Option<Vec3> {
|
|
// Build the 4x4 system A X = 0, two rows per view.
|
|
let mut a = Matrix4::<f64>::zeros();
|
|
a.row_mut(0).copy_from(&(x1.x * p1.row(2) - p1.row(0)));
|
|
a.row_mut(1).copy_from(&(x1.y * p1.row(2) - p1.row(1)));
|
|
a.row_mut(2).copy_from(&(x2.x * p2.row(2) - p2.row(0)));
|
|
a.row_mut(3).copy_from(&(x2.y * p2.row(2) - p2.row(1)));
|
|
|
|
let svd = a.svd(false, true);
|
|
let v_t = svd.v_t?;
|
|
// The solution is the right-singular vector with the smallest singular value,
|
|
// i.e. the last row of V^T.
|
|
let xh: Vector4<f64> = v_t.row(3).transpose();
|
|
if xh.w.abs() < f64::EPSILON {
|
|
return None;
|
|
}
|
|
Some(Vec3::new(xh.x / xh.w, xh.y / xh.w, xh.z / xh.w))
|
|
}
|
|
|
|
/// Triangulates a 3D point (in camera-1 coordinates) from two bearing rays using
|
|
/// the mid-point method.
|
|
///
|
|
/// `cam2_from_cam1` is the relative pose, `ray1`/`ray2` are direction vectors in
|
|
/// each camera frame. Returns the point that minimizes the distance to both
|
|
/// rays, or `None` if the rays are (near) parallel.
|
|
pub fn triangulate_mid_point(
|
|
cam2_from_cam1: &crate::geometry::Rigid3d,
|
|
ray1: &Vec3,
|
|
ray2: &Vec3,
|
|
) -> Option<Vec3> {
|
|
// Camera 1 at the origin, looking along ray1.
|
|
// Camera 2 center (in cam1 coords) and ray2 rotated into cam1 coords.
|
|
let r = cam2_from_cam1.rotation.to_rotation_matrix();
|
|
let c2 = cam2_from_cam1.target_origin_in_source(); // camera-2 center in cam1.
|
|
let d1 = ray1.normalize();
|
|
let d2 = (r.inverse() * ray2).normalize();
|
|
|
|
// Solve for parameters t1, t2 minimizing || (t1 d1) - (c2 + t2 d2) ||.
|
|
let b = c2;
|
|
let d1d1 = d1.dot(&d1);
|
|
let d1d2 = d1.dot(&d2);
|
|
let d2d2 = d2.dot(&d2);
|
|
let denom = d1d1 * d2d2 - d1d2 * d1d2;
|
|
if denom.abs() < 1e-12 {
|
|
return None;
|
|
}
|
|
let d1b = d1.dot(&b);
|
|
let d2b = d2.dot(&b);
|
|
let t1 = (d2d2 * d1b - d1d2 * d2b) / denom;
|
|
let t2 = (d1d2 * d1b - d1d1 * d2b) / denom;
|
|
|
|
let point1 = t1 * d1;
|
|
let point2 = c2 + t2 * d2;
|
|
Some(0.5 * (point1 + point2))
|
|
}
|
|
|
|
/// Triangulates a 3D point from an arbitrary number of views using the linear
|
|
/// DLT method (stacking two rows per observation and solving by SVD).
|
|
///
|
|
/// `proj_matrices` are the `[R | t]`/projection matrices and `points` the
|
|
/// corresponding image points. Requires at least two views; returns `None` for
|
|
/// fewer views or a degenerate system.
|
|
pub fn triangulate_multi_view_point(proj_matrices: &[Mat3x4], points: &[Vec2]) -> Option<Vec3> {
|
|
let n = proj_matrices.len().min(points.len());
|
|
if n < 2 {
|
|
return None;
|
|
}
|
|
// Accumulate the normal-equation matrix A^T A (4x4) from all observations.
|
|
let mut ata = Matrix4::<f64>::zeros();
|
|
for i in 0..n {
|
|
let p = &proj_matrices[i];
|
|
let x = &points[i];
|
|
let row0: Vector4<f64> = (x.x * p.row(2) - p.row(0)).transpose();
|
|
let row1: Vector4<f64> = (x.y * p.row(2) - p.row(1)).transpose();
|
|
ata += row0 * row0.transpose();
|
|
ata += row1 * row1.transpose();
|
|
}
|
|
let eig = ata.symmetric_eigen();
|
|
// Smallest eigenvalue's eigenvector is the solution.
|
|
let mut best_idx = 0;
|
|
let mut best_val = f64::INFINITY;
|
|
for i in 0..4 {
|
|
if eig.eigenvalues[i] < best_val {
|
|
best_val = eig.eigenvalues[i];
|
|
best_idx = i;
|
|
}
|
|
}
|
|
let xh = eig.eigenvectors.column(best_idx);
|
|
if xh[3].abs() < f64::EPSILON {
|
|
return None;
|
|
}
|
|
Some(Vec3::new(xh[0] / xh[3], xh[1] / xh[3], xh[2] / xh[3]))
|
|
}
|
|
|
|
/// Computes the triangulation angle (in radians) at a 3D `point` as seen from two
|
|
/// camera projection centers `c1` and `c2`.
|
|
///
|
|
/// A larger angle means a better-conditioned triangulation; COLMAP uses this to
|
|
/// filter degenerate (near-zero parallax) points.
|
|
pub fn calculate_triangulation_angle(c1: &Vec3, c2: &Vec3, point: &Vec3) -> f64 {
|
|
let ray1 = point - c1;
|
|
let ray2 = point - c2;
|
|
let baseline_sq = (c1 - c2).norm_squared();
|
|
let ray1_sq = ray1.norm_squared();
|
|
let ray2_sq = ray2.norm_squared();
|
|
if ray1_sq < f64::EPSILON || ray2_sq < f64::EPSILON {
|
|
return 0.0;
|
|
}
|
|
// Law of cosines: cos(angle) = (|ray1|^2 + |ray2|^2 - baseline^2) /
|
|
// (2 |ray1| |ray2|).
|
|
let denom = 2.0 * (ray1_sq * ray2_sq).sqrt();
|
|
let cos_angle = ((ray1_sq + ray2_sq - baseline_sq) / denom).clamp(-1.0, 1.0);
|
|
let angle = cos_angle.acos();
|
|
// Return the acute angle, matching COLMAP (triangulation angle in [0, pi/2]).
|
|
angle.min(std::f64::consts::PI - angle)
|
|
}
|
|
|
|
/// Computes the angle (in radians) between two vectors, in `[0, pi]`. Returns 0
|
|
/// if either vector is (near) zero.
|
|
pub fn calculate_angle_between_vectors(a: &Vec3, b: &Vec3) -> f64 {
|
|
let na = a.norm();
|
|
let nb = b.norm();
|
|
if na < f64::EPSILON || nb < f64::EPSILON {
|
|
return 0.0;
|
|
}
|
|
let cos_angle = (a.dot(b) / (na * nb)).clamp(-1.0, 1.0);
|
|
cos_angle.acos()
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
use crate::geometry::Rigid3d;
|
|
use crate::math::UnitQuat;
|
|
use approx::assert_relative_eq;
|
|
use std::f64::consts::FRAC_PI_2;
|
|
|
|
fn project(pose: &Rigid3d, p: &Vec3) -> Vec2 {
|
|
let pc = pose.transform_point(p);
|
|
Vec2::new(pc.x / pc.z, pc.y / pc.z)
|
|
}
|
|
|
|
#[test]
|
|
fn dlt_triangulates_known_point() {
|
|
let pose1 = Rigid3d::identity();
|
|
let pose2 = Rigid3d::new(
|
|
UnitQuat::new(Vec3::new(0.0, 0.05, 0.0)),
|
|
Vec3::new(-1.0, 0.0, 0.0),
|
|
);
|
|
let point = Vec3::new(0.4, -0.7, 6.0);
|
|
let x1 = project(&pose1, &point);
|
|
let x2 = project(&pose2, &point);
|
|
let tri = triangulate_point(&pose1.to_matrix(), &pose2.to_matrix(), &x1, &x2).unwrap();
|
|
assert_relative_eq!(tri, point, epsilon = 1e-9);
|
|
}
|
|
|
|
#[test]
|
|
fn midpoint_triangulates_known_point() {
|
|
let pose2 = Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0));
|
|
let point = Vec3::new(0.2, 0.1, 5.0);
|
|
let ray1 = point; // cam1 == world.
|
|
let ray2 = pose2.transform_point(&point);
|
|
let tri = triangulate_mid_point(&pose2, &ray1, &ray2).unwrap();
|
|
assert_relative_eq!(tri, point, epsilon = 1e-9);
|
|
}
|
|
|
|
#[test]
|
|
fn midpoint_parallel_rays_none() {
|
|
let pose2 = Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0));
|
|
// Both rays point along +z => parallel in cam1 coords => no intersection.
|
|
let ray1 = Vec3::new(0.0, 0.0, 1.0);
|
|
let ray2 = Vec3::new(0.0, 0.0, 1.0);
|
|
assert!(triangulate_mid_point(&pose2, &ray1, &ray2).is_none());
|
|
}
|
|
|
|
#[test]
|
|
fn multi_view_triangulates_known_point() {
|
|
let poses = [
|
|
Rigid3d::identity(),
|
|
Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0)),
|
|
Rigid3d::new(UnitQuat::new(Vec3::new(0.0, 0.1, 0.0)), Vec3::new(-2.0, 0.3, 0.0)),
|
|
];
|
|
let point = Vec3::new(-0.5, 0.8, 7.0);
|
|
let mats: Vec<Mat3x4> = poses.iter().map(|p| p.to_matrix()).collect();
|
|
let pts: Vec<Vec2> = poses.iter().map(|p| project(p, &point)).collect();
|
|
let tri = triangulate_multi_view_point(&mats, &pts).unwrap();
|
|
assert_relative_eq!(tri, point, epsilon = 1e-8);
|
|
}
|
|
|
|
#[test]
|
|
fn multi_view_needs_two_views() {
|
|
let mats = vec![Rigid3d::identity().to_matrix()];
|
|
let pts = vec![Vec2::new(0.0, 0.0)];
|
|
assert!(triangulate_multi_view_point(&mats, &pts).is_none());
|
|
}
|
|
|
|
#[test]
|
|
fn triangulation_angle_right_angle() {
|
|
// Two cameras viewing a point at 90 degrees.
|
|
let c1 = Vec3::new(-1.0, 0.0, 0.0);
|
|
let c2 = Vec3::new(0.0, 0.0, -1.0);
|
|
let point = Vec3::new(0.0, 0.0, 0.0);
|
|
// ray1 = point - c1 = (1,0,0), ray2 = (0,0,1) -> angle 90 deg.
|
|
let angle = calculate_triangulation_angle(&c1, &c2, &point);
|
|
assert_relative_eq!(angle, FRAC_PI_2, epsilon = 1e-9);
|
|
}
|
|
|
|
#[test]
|
|
fn angle_between_vectors_basic() {
|
|
let a = Vec3::new(1.0, 0.0, 0.0);
|
|
let b = Vec3::new(0.0, 1.0, 0.0);
|
|
assert_relative_eq!(
|
|
calculate_angle_between_vectors(&a, &b),
|
|
FRAC_PI_2,
|
|
epsilon = 1e-12
|
|
);
|
|
assert_relative_eq!(calculate_angle_between_vectors(&a, &a), 0.0, epsilon = 1e-12);
|
|
}
|
|
}
|