Skip to content
This repository has been archived by the owner on Jul 19, 2020. It is now read-only.

Commit

Permalink
closes #7; switch everything over to homogeneous projective coordinates
Browse files Browse the repository at this point in the history
  • Loading branch information
@vadixidav
vadixidav committed Jun 21, 2020
1 parent ddce0bc commit 8ba71df
Show file tree
Hide file tree
Showing 6 changed files with 264 additions and 527 deletions.
290 changes: 66 additions & 224 deletions src/essential.rs
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,4 @@
use crate::{
Bearing, CameraPoint, CameraToCamera, FeatureMatch, Pose, TriangulatorProject,
TriangulatorRelative, UnscaledCameraToCamera,
};
use crate::{Bearing, CameraToCamera, FeatureMatch, Pose, Projective, TriangulatorRelative};
use derive_more::{AsMut, AsRef, Deref, DerefMut, From, Into};
use nalgebra::{Matrix3, Rotation3, Vector3, SVD};
use num_traits::Float;
Expand Down Expand Up @@ -123,14 +120,14 @@ impl EssentialMatrix {
/// // Get the possible poses for the essential matrix created from `pose`.
/// let (rot_a, rot_b, t) = pose.essential_matrix().possible_rotations_unscaled_translation(1e-6, 50).unwrap();
/// // Compute residual rotations.
/// let a_res = rot_a.rotation_to(&pose.rotation).angle();
/// let b_res = rot_b.rotation_to(&pose.rotation).angle();
/// let a_res = rot_a.rotation_to(&pose.0.rotation).angle();
/// let b_res = rot_b.rotation_to(&pose.0.rotation).angle();
/// let a_close = a_res < 1e-4;
/// let b_close = b_res < 1e-4;
/// // At least one rotation is correct.
/// assert!(a_close || b_close);
/// // The translation points in the same (or reverse) direction
/// let t_res = 1.0 - t.normalize().dot(&pose.translation.vector.normalize()).abs();
/// let t_res = 1.0 - t.normalize().dot(&pose.0.translation.vector.normalize()).abs();
/// assert!(t_res < 1e-4);
/// ```
pub fn possible_rotations_unscaled_translation(
Expand Down Expand Up @@ -214,7 +211,7 @@ impl EssentialMatrix {
/// // Get the possible rotations for the essential matrix created from `pose`.
/// let rbs = pose.essential_matrix().possible_rotations(1e-6, 50).unwrap();
/// let one_correct = rbs.iter().any(|&rot| {
/// let angle_residual = rot.rotation_to(&pose.rotation).angle();
/// let angle_residual = rot.rotation_to(&pose.0.rotation).angle();
/// angle_residual < 1e-4
/// });
/// assert!(one_correct);
Expand Down Expand Up @@ -243,10 +240,10 @@ impl EssentialMatrix {
/// let rbs = pose.essential_matrix().possible_unscaled_poses(1e-6, 50).unwrap();
/// let one_correct = rbs.iter().any(|&upose| {
/// let angle_residual =
/// upose.rotation.rotation_to(&pose.rotation).angle();
/// upose.0.rotation.rotation_to(&pose.0.rotation).angle();
/// let translation_residual =
/// 1.0 - upose.translation.vector.normalize()
/// .dot(&pose.translation.vector.normalize());
/// 1.0 - upose.0.translation.vector.normalize()
/// .dot(&pose.0.translation.vector.normalize());
/// angle_residual < 1e-4 && translation_residual < 1e-4
/// });
/// assert!(one_correct);
Expand All @@ -255,14 +252,14 @@ impl EssentialMatrix {
&self,
epsilon: f64,
max_iterations: usize,
) -> Option<[UnscaledCameraToCamera; 4]> {
) -> Option<[CameraToCamera; 4]> {
self.possible_rotations_unscaled_translation(epsilon, max_iterations)
.map(|(rot_a, rot_b, t)| {
[
UnscaledCameraToCamera::from_parts(t, rot_a),
UnscaledCameraToCamera::from_parts(t, rot_b),
UnscaledCameraToCamera::from_parts(-t, rot_a),
UnscaledCameraToCamera::from_parts(-t, rot_b),
CameraToCamera::from_parts(t, rot_a),
CameraToCamera::from_parts(t, rot_b),
CameraToCamera::from_parts(-t, rot_a),
CameraToCamera::from_parts(-t, rot_b),
]
})
}
Expand All @@ -275,12 +272,12 @@ impl EssentialMatrix {
&self,
epsilon: f64,
max_iterations: usize,
) -> Option<[UnscaledCameraToCamera; 2]> {
) -> Option<[CameraToCamera; 2]> {
self.possible_rotations_unscaled_translation(epsilon, max_iterations)
.map(|(rot_a, rot_b, t)| {
[
UnscaledCameraToCamera::from_parts(t, rot_a),
UnscaledCameraToCamera::from_parts(t, rot_b),
CameraToCamera::from_parts(t, rot_a),
CameraToCamera::from_parts(t, rot_b),
]
})
}
Expand Down Expand Up @@ -348,12 +345,53 @@ impl<'a> PoseSolver<'a> {
}
}

/// Solve the pose given correspondences.
/// Return the [`CameraToCamera`] that transforms a [`CameraPoint`] of image
/// A (source of `a`) to the corresponding [`CameraPoint`] of image B (source of `b`).
/// The function takes an iterator over [`FeatureMatch`] from A to B.
/// The translation scale is unknown of the returned pose.
///
/// * `depth` - The actual depth (`z` axis, not distance) of normalized keypoint `a`
/// * `a` - A keypoint from image `A`
/// * `b` - A keypoint from image `B`
///
/// `self` must satisfy the constraint:
///
/// ```text
/// transpose(homogeneous(a)) * E * homogeneous(b) = 0
/// ```
///
/// Also, `a` and `b` must be a correspondence.
///
/// This will take the average translation over the entire iterator. This is done
/// to smooth out noise and outliers (if present).
///
/// `epsilon` is a small value to which SVD must converge to before terminating.
///
/// `max_iterations` is the maximum number of iterations that SVD will run on this
/// matrix. Use this to cap the execution time.
/// A `max_iterations` of `0` may execute indefinitely and is not recommended except
/// for non-production code.
///
/// `consensus_ratio` is the ratio of points which must be in front of the camera for the model
/// to be accepted and return Some. Otherwise, None is returned. Set this to about
/// `0.45` to have approximate majority consensus.
///
/// `bearing_scale` is a function that is passed a translation bearing vector,
/// an untranslated (but rotated) camera point, and a normalized key point
/// where the actual point exists. It must return the scalar which the
/// translation bearing vector must by multiplied by to get the actual translation.
/// It may return `None` if it fails.
///
/// `correspondences` must provide an iterator of tuples containing the matches
/// of a 3d `CameraPoint` `a` from camera A and the matching `NormalizedKeyPoint`
/// `b` from camera B.
///
/// This does not communicate which points were outliers to each model.
pub fn solve_unscaled<P>(
&self,
triangulator: &impl TriangulatorRelative,
correspondences: impl Iterator<Item = FeatureMatch<P>>,
) -> Option<UnscaledCameraToCamera>
) -> Option<CameraToCamera>
where
P: Bearing + Copy,
{
Expand All @@ -373,15 +411,15 @@ impl<'a> PoseSolver<'a> {
// Transform the point to camera B to get `tp`.
let tp = pose.transform(p);
// The points must lie in the same direction as their bearings (positive dot product).
p.coords.normalize().dot(&a.bearing()) > 0.0
&& tp.coords.normalize().dot(&b.bearing()) > 0.0
p.bearing().dot(&a.bearing()) > 0.0
&& tp.bearing().dot(&b.bearing()) > 0.0
})
.unwrap_or(false)
};

// Do it for all poses.
for (tn, &pose) in ts.iter_mut().zip(&poses) {
if trans_and_agree(pose.assume_scaled()) {
if trans_and_agree(pose) {
*tn += 1;
}
}
Expand Down Expand Up @@ -418,7 +456,7 @@ impl<'a> PoseSolver<'a> {
&self,
triangulator: &impl TriangulatorRelative,
correspondences: impl Iterator<Item = FeatureMatch<P>>,
) -> Option<(UnscaledCameraToCamera, alloc::vec::Vec<usize>)>
) -> Option<(CameraToCamera, alloc::vec::Vec<usize>)>
where
P: Bearing + Copy,
{
Expand Down Expand Up @@ -446,15 +484,15 @@ impl<'a> PoseSolver<'a> {
// Transform the point to camera B to get `tp`.
let tp = pose.transform(p);
// The points must lie in the same direction as their bearings (positive dot product).
p.coords.normalize().dot(&a.bearing()) > 0.0
&& tp.coords.normalize().dot(&b.bearing()) > 0.0
p.bearing().dot(&a.bearing()) > 0.0
&& tp.bearing().dot(&b.bearing()) > 0.0
})
.unwrap_or(false)
};

// Do it for all poses.
for ((tn, ti), &pose) in ts.iter_mut().zip(&poses) {
if trans_and_agree(pose.assume_scaled()) {
if trans_and_agree(pose) {
*tn += 1;
ti.push(ix);
}
Expand Down Expand Up @@ -485,200 +523,4 @@ impl<'a> PoseSolver<'a> {
Some((poses[ix], inliers))
})
}

/// Return the [`CameraToCamera`] that transforms a [`CameraPoint`] of image
/// `A` (source of `a`) to the corresponding [`CameraPoint`] of image B (source of `b`).
/// This determines the average expected translation from the points themselves and
/// if the points agree with the rotation (points must be in front of the camera).
/// The function takes an iterator containing tuples in the form `(depth, a, b)`:
///
/// * `depth` - The actual depth (`z` axis, not distance) of normalized keypoint `a`
/// * `a` - A keypoint from image `A`
/// * `b` - A keypoint from image `B`
///
/// `self` must satisfy the constraint:
///
/// ```text
/// transpose(homogeneous(a)) * E * homogeneous(b) = 0
/// ```
///
/// Also, `a` and `b` must be a correspondence.
///
/// This will take the average translation over the entire iterator. This is done
/// to smooth out noise and outliers (if present).
///
/// `epsilon` is a small value to which SVD must converge to before terminating.
///
/// `max_iterations` is the maximum number of iterations that SVD will run on this
/// matrix. Use this to cap the execution time.
/// A `max_iterations` of `0` may execute indefinitely and is not recommended except
/// for non-production code.
///
/// `consensus_ratio` is the ratio of points which must be in front of the camera for the model
/// to be accepted and return Some. Otherwise, None is returned. Set this to about
/// `0.45` to have approximate majority consensus.
///
/// `bearing_scale` is a function that is passed a translation bearing vector,
/// an untranslated (but rotated) camera point, and a normalized key point
/// where the actual point exists. It must return the scalar which the
/// translation bearing vector must by multiplied by to get the actual translation.
/// It may return `None` if it fails.
///
/// `correspondences` must provide an iterator of tuples containing the matches
/// of a 3d `CameraPoint` `a` from camera A and the matching `NormalizedKeyPoint`
/// `b` from camera B.
///
/// This does not communicate which points were outliers to each model.
pub fn solve<P>(
&self,
triangulator: &impl TriangulatorProject,
correspondences: impl Iterator<Item = (CameraPoint, P)> + Clone,
) -> Option<CameraToCamera>
where
P: Bearing + Copy,
{
// Get the possible rotations and the translation
self.essential
.possible_unscaled_poses_bearing(self.epsilon, self.max_iterations)
.and_then(|poses| {
// Get the net translation scale of points that agree with a and b
// in addition to the number of points that agree with a and b.
let (ts, total) = correspondences.fold(
([(0usize, 0.0); 2], 0usize),
|(mut ts, total), (a, b)| {
let trans_and_agree = |pose: UnscaledCameraToCamera| {
let untranslated = pose.rotation * a.0;
let t_scale = triangulator.triangulate_project(
CameraPoint(untranslated),
b,
pose.translation.vector,
)?;

if (untranslated.coords + t_scale * pose.translation.vector)
.dot(&b.bearing())
> 0.0
{
Some(t_scale)
} else {
None
}
};

// Do it for all poses.
for (tn, &pose) in ts.iter_mut().zip(&poses) {
if let Some(scale) = trans_and_agree(pose) {
tn.0 += 1;
tn.1 += scale;
}
}

(ts, total + 1)
},
);

// Ensure that there is at least one point.
if total == 0 {
return None;
}

// Ensure that the best one exceeds the consensus ratio.
let (ix, (best_num, scale_acc)) = ts
.iter()
.copied()
.enumerate()
.max_by_key(|&(_, (t, _))| t)
.unwrap();
if (best_num as f64) < self.consensus_ratio * total as f64 && best_num != 0 {
return None;
}
let scale = scale_acc / best_num as f64;

Some(CameraToCamera::from_parts(
scale * poses[ix].translation.vector,
poses[ix].rotation,
))
})
}

/// Same as [`PoseSolver::solve`], but it also communicates the inliers.
///
/// The `alloc` feature must be enabled to use this method.
#[cfg(feature = "alloc")]
pub fn solve_inliers<P>(
&self,
triangulator: &impl TriangulatorProject,
correspondences: impl Iterator<Item = (CameraPoint, P)> + Clone,
) -> Option<(CameraToCamera, alloc::vec::Vec<usize>)>
where
P: Bearing + Copy,
{
// Get the possible rotations and the translation
self.essential
.possible_unscaled_poses_bearing(self.epsilon, self.max_iterations)
.and_then(|poses| {
// Get the net translation scale of points that agree with a and b
// in addition to the number of points that agree with a and b.
let (mut ts, total) = correspondences.enumerate().fold(
(
[
(0usize, 0.0, alloc::vec::Vec::new()),
(0usize, 0.0, alloc::vec::Vec::new()),
],
0usize,
),
|(mut ts, total), (ix, (a, b))| {
let trans_and_agree = |pose: UnscaledCameraToCamera| {
let untranslated = pose.rotation * a.0;
let t_scale = triangulator.triangulate_project(
CameraPoint(untranslated),
b,
pose.translation.vector,
)?;

if (untranslated.coords + t_scale * pose.translation.vector)
.dot(&b.bearing())
> 0.0
{
Some(t_scale)
} else {
None
}
};

// Do it for all poses.
for (tn, &pose) in ts.iter_mut().zip(&poses) {
if let Some(scale) = trans_and_agree(pose) {
tn.0 += 1;
tn.1 += scale;
tn.2.push(ix);
}
}

(ts, total + 1)
},
);

// Ensure that there is at least one point.
if total == 0 {
return None;
}

// Ensure that the best one exceeds the consensus ratio.
let ix = (0..ts.len()).max_by_key(|&ix| ts[ix].0).unwrap();
let (best_num, scale_acc, best_inliers) = core::mem::take(&mut ts[ix]);
if (best_num as f64) < self.consensus_ratio * total as f64 && best_num != 0 {
return None;
}

let scale = scale_acc / best_num as f64;

Some((
CameraToCamera::from_parts(
scale * poses[ix].translation.vector,
poses[ix].rotation,
),
best_inliers,
))
})
}
}
Loading

0 comments on commit 8ba71df

Please sign in to comment.