use cv_core::nalgebra::{IsometryMatrix3, Rotation3, Vector2, Vector3};
use cv_core::sample_consensus::{Estimator, Model};
use cv_core::{CameraPoint, CameraToCamera, FeatureMatch, Pose};
use cv_pinhole::NormalizedKeyPoint;
const SAMPLE_POINTS: usize = 16;
const RESIDUAL_THRESHOLD: f64 = 1e-4;
const ROT_MAGNITUDE: f64 = 0.2;
const POINT_BOX_SIZE: f64 = 2.0;
const POINT_DISTANCE: f64 = 3.0;
#[test]
fn randomized() {
let successes = (0..1000).filter(|_| run_round()).count();
eprintln!("successes: {}", successes);
assert!(successes > 950);
}
fn run_round() -> bool {
let mut success = true;
let (real_pose, aps, bps) = some_test_data();
let matches = aps.iter().zip(&bps).map(|(&a, &b)| FeatureMatch(a, b));
let eight_point = eight_point::EightPoint::new();
let essential = eight_point
.estimate(matches.clone())
.expect("didn't get any essential matrix");
for m in matches.clone() {
if essential.residual(&m).abs() > RESIDUAL_THRESHOLD {
success = false;
eprintln!("failed residual check: {}", essential.residual(&m).abs());
}
}
let estimate_pose = match essential.pose_solver().solve_unscaled(matches) {
Some(pose) => pose,
None => {
return false;
}
};
let rot_axis_residual = 1.0
- estimate_pose
.0
.rotation
.axis()
.unwrap()
.dot(&real_pose.0.rotation.axis().unwrap());
let rot_angle_residual =
(estimate_pose.0.rotation.angle() - real_pose.0.rotation.angle()).abs();
let translation_residual = 1.0
- real_pose
.0
.translation
.vector
.normalize()
.dot(&estimate_pose.0.translation.vector.normalize());
success &= rot_axis_residual < RESIDUAL_THRESHOLD;
success &= rot_angle_residual < RESIDUAL_THRESHOLD;
success &= translation_residual < RESIDUAL_THRESHOLD;
if !success {
eprintln!("rot angle residual({})", rot_angle_residual);
eprintln!("rot axis residual({})", rot_axis_residual);
eprintln!("translation residual({})", translation_residual);
eprintln!("real pose: {:?}", real_pose);
eprintln!("estimate pose: {:?}", estimate_pose);
}
success
}
fn some_test_data() -> (
CameraToCamera,
[NormalizedKeyPoint; SAMPLE_POINTS],
[NormalizedKeyPoint; SAMPLE_POINTS],
) {
let relative_pose = CameraToCamera(IsometryMatrix3::from_parts(
Vector3::new_random().into(),
Rotation3::new(Vector3::new_random() * std::f64::consts::PI * 2.0 * ROT_MAGNITUDE),
));
let cams_a = (0..SAMPLE_POINTS)
.map(|_| {
let mut a = Vector3::new_random() * POINT_BOX_SIZE;
a.x -= 0.5 * POINT_BOX_SIZE;
a.y -= 0.5 * POINT_BOX_SIZE;
a.z += POINT_DISTANCE;
CameraPoint(a.push(1.0))
})
.collect::<Vec<_>>()
.into_iter();
let cams_b = cams_a.clone().map(|a| relative_pose.transform(a));
let mut kps_a = [NormalizedKeyPoint(Vector2::zeros().into()); SAMPLE_POINTS];
for (keypoint, camera) in kps_a.iter_mut().zip(cams_a) {
*keypoint = NormalizedKeyPoint::from_camera_point(camera).unwrap();
}
let mut kps_b = [NormalizedKeyPoint(Vector2::zeros().into()); SAMPLE_POINTS];
for (keypoint, camera) in kps_b.iter_mut().zip(cams_b.clone()) {
*keypoint = NormalizedKeyPoint::from_camera_point(camera).unwrap();
}
(relative_pose, kps_a, kps_b)
}