use std::fmt;
use nalgebra::Point3;
use crate::ba;
use crate::calib::Intrinsics;
use crate::error::Error;
use crate::event::{emit, report, silent, Event, Flow, Stage};
use crate::input::MIN_VIEWS;
use crate::pnp;
use crate::pose::Pose;
use crate::rng::Rng;
use crate::tracks::Track;
use crate::triangulate;
use crate::twoview::{RelativePose, Verdict};
use crate::types::{Norm, PairId, ViewId, World};
#[derive(Clone, Debug)]
pub struct Params {
pub max_reprojection: f64,
pub min_point_angle_deg: f64,
pub min_pnp_inlier_ratio: f64,
pub enough_pnp_inliers: usize,
pub min_pnp_inlier_ratio_when_enough: f64,
pub min_registration_points: usize,
pub max_iterations: usize,
pub confidence: f64,
pub min_camera_spread: f64,
pub adjust: ba::Params,
}
impl Default for Params {
fn default() -> Self {
Params {
max_reprojection: 3.0,
min_point_angle_deg: 1.5,
min_pnp_inlier_ratio: 0.6,
enough_pnp_inliers: 150,
min_pnp_inlier_ratio_when_enough: 0.35,
min_registration_points: 30,
max_iterations: 1000,
confidence: 0.999,
min_camera_spread: 0.02,
adjust: ba::Params {
max_iterations: 15,
rounds: 1,
max_median_error: f64::INFINITY,
..ba::Params::default()
},
}
}
}
#[derive(Clone, Debug, PartialEq)]
#[non_exhaustive]
pub struct SparsePoint {
pub position: World,
pub track: usize,
pub observations: usize,
pub error_px: f64,
pub angle_deg: f64,
}
#[derive(Clone, Debug, PartialEq)]
#[non_exhaustive]
pub struct Registration {
pub view: ViewId,
pub candidates: usize,
pub inliers: usize,
pub inlier_ratio: f64,
}
#[derive(Clone, Debug, PartialEq)]
#[non_exhaustive]
pub enum Warning {
Unregistered {
view: ViewId,
reason: DropReason,
},
CollinearCameras {
deviation: f64,
},
}
#[derive(Clone, Debug, PartialEq)]
#[non_exhaustive]
pub enum DropReason {
NotLinked {
min_coverage: f32,
},
TooFewPoints {
seen: usize,
required: usize,
},
NoPose,
TooFewInliers {
inliers: usize,
points: usize,
required_ratio: f64,
enough_inliers: usize,
ratio_when_enough: f64,
},
}
impl fmt::Display for DropReason {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match *self {
DropReason::NotLinked { min_coverage } => write!(
f,
"no pair with at least {:.0}% coverage links it to the others",
min_coverage * 100.0
),
DropReason::TooFewPoints { seen, required } => write!(
f,
"it sees only {seen} of the points placed so far (needs {required})"
),
DropReason::NoPose => f.write_str("no camera position fits the points it sees"),
DropReason::TooFewInliers {
inliers,
points,
required_ratio,
enough_inliers,
ratio_when_enough,
} => {
let ratio = inliers as f64 / points as f64;
if inliers >= enough_inliers {
write!(
f,
"only {:.0}% of the {points} points it sees agree on where it was (needs {:.0}%)",
ratio * 100.0,
required_ratio * 100.0
)
} else {
write!(
f,
"only {inliers} ({:.0}%) of the {points} points it sees agree on where it was (needs {:.0}%, or {enough_inliers} points and {:.0}%)",
ratio * 100.0,
required_ratio * 100.0,
ratio_when_enough * 100.0
)
}
}
}
}
}
impl fmt::Display for Warning {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Warning::Unregistered { view, reason } => {
write!(f, "{view} was left out: {reason}")
}
Warning::CollinearCameras { deviation } => write!(
f,
"the camera positions lie almost on one line ({:.1}% off it); vary the height or distance between shots",
deviation * 100.0
),
}
}
}
#[derive(Clone, Debug)]
#[non_exhaustive]
pub struct SparseModel {
pub cameras: Vec<Option<Pose>>,
pub points: Vec<SparsePoint>,
pub seed: PairId,
pub intrinsics: Intrinsics,
pub registrations: Vec<Registration>,
pub warnings: Vec<Warning>,
}
impl SparseModel {
pub fn registered(&self) -> Vec<ViewId> {
self.cameras
.iter()
.enumerate()
.filter(|(_, c)| c.is_some())
.map(|(v, _)| ViewId(v as u32))
.collect()
}
}
pub fn reconstruct(
relative: &[RelativePose],
tracks: &[Track],
intrinsics: &Intrinsics,
views: usize,
precision_px: f64,
params: &Params,
rng: &mut Rng,
) -> Result<SparseModel, Error> {
reconstruct_with_progress(
relative,
tracks,
intrinsics,
views,
precision_px,
params,
rng,
&mut silent,
)
}
#[allow(clippy::too_many_arguments)]
pub fn reconstruct_with_progress(
relative: &[RelativePose],
tracks: &[Track],
intrinsics: &Intrinsics,
views: usize,
precision_px: f64,
params: &Params,
rng: &mut Rng,
on_event: &mut dyn FnMut(Event) -> Flow,
) -> Result<SparseModel, Error> {
let mut intrinsics = *intrinsics;
let mut threshold = params.max_reprojection * precision_px / mean_focal(&intrinsics);
let min_angle = params.min_point_angle_deg.to_radians();
let seed = relative
.iter()
.filter(|r| r.is_usable() && r.pair.b().index() < views)
.max_by(|a, b| seed_score(a).total_cmp(&seed_score(b)))
.ok_or_else(|| Error::NoUsablePair {
reasons: relative
.iter()
.filter_map(|r| match &r.verdict {
Verdict::Degenerate(reason) => Some((r.pair, reason.clone())),
Verdict::Usable => None,
})
.collect(),
})?;
let mut cameras: Vec<Option<Pose>> = vec![None; views];
cameras[seed.pair.a().index()] = Some(Pose::identity());
cameras[seed.pair.b().index()] = Some(seed.pose);
report(
on_event,
Stage::Registration,
2.0 / views as f32,
format!("{} defines the frame; 2 of {views} views placed", seed.pair),
)?;
let mut normalized = normalize(tracks, &intrinsics);
let mut points: Vec<Option<World>> = vec![None; tracks.len()];
triangulate_missing(&normalized, &cameras, &mut points, threshold, min_angle);
let mut registrations = Vec::new();
let mut warnings = Vec::new();
let mut version = 0usize;
let mut tried_at: Vec<Option<usize>> = vec![None; views];
let mut reasons: Vec<Option<DropReason>> = vec![None; views];
let mut adjusted_at = version;
let mut placed_at_adjustment = 2usize;
loop {
let candidate = (0..views)
.filter(|&v| cameras[v].is_none() && tried_at[v] != Some(version))
.map(|v| (v, observed(&normalized, &points, ViewId(v as u32))))
.max_by_key(|&(v, count)| (count, std::cmp::Reverse(v)));
let Some((v, count)) = candidate else {
let stuck = cameras.iter().any(Option::is_none);
if stuck
&& adjusted_at != version
&& adjust_placed(
tracks,
seed.pair,
precision_px,
params,
&mut cameras,
&mut points,
&mut intrinsics,
on_event,
)?
{
normalized = normalize(tracks, &intrinsics);
threshold = params.max_reprojection * precision_px / mean_focal(&intrinsics);
triangulate_missing(&normalized, &cameras, &mut points, threshold, min_angle);
version += 1;
adjusted_at = version;
continue;
}
break;
};
let view = ViewId(v as u32);
tried_at[v] = Some(version);
let placed = cameras.iter().flatten().count();
let attempt = if count < params.min_registration_points {
Err(DropReason::TooFewPoints {
seen: count,
required: params.min_registration_points,
})
} else {
let (world, image): (Vec<Point3<f64>>, Vec<Norm>) = normalized
.iter()
.zip(&points)
.filter_map(|(obs, point)| {
let point = (*point)?;
let &(_, n) = obs.iter().find(|(o, _)| *o == view)?;
Some((point.0, n))
})
.unzip();
match pnp::ransac(
&world,
&image,
threshold,
params.max_iterations,
params.confidence,
rng,
) {
None => Err(DropReason::NoPose),
Some(solution) => {
let inliers = solution.inliers.len();
let ratio = inliers as f64 / count as f64;
let needed = if inliers >= params.enough_pnp_inliers {
params.min_pnp_inlier_ratio_when_enough
} else {
params.min_pnp_inlier_ratio
};
if ratio >= needed {
Ok((solution, ratio))
} else {
Err(DropReason::TooFewInliers {
inliers,
points: count,
required_ratio: needed,
enough_inliers: params.enough_pnp_inliers,
ratio_when_enough: params.min_pnp_inlier_ratio_when_enough,
})
}
}
}
};
let (solution, inlier_ratio) = match attempt {
Ok(found) => found,
Err(reason) => {
report(
on_event,
Stage::Registration,
placed as f32 / views as f32,
format!("{view} not placed yet: {reason}"),
)?;
reasons[v] = Some(reason);
continue;
}
};
cameras[v] = Some(solution.pose);
reasons[v] = None;
registrations.push(Registration {
view,
candidates: count,
inliers: solution.inliers.len(),
inlier_ratio,
});
let placed = cameras.iter().flatten().count();
report(
on_event,
Stage::Registration,
placed as f32 / views as f32,
format!(
"placed {view} on {} of the {count} points it sees; {placed} of {views} views placed",
solution.inliers.len()
),
)?;
version += 1;
triangulate_missing(&normalized, &cameras, &mut points, threshold, min_angle);
if 2 * placed >= 3 * placed_at_adjustment {
placed_at_adjustment = placed;
adjusted_at = version;
if adjust_placed(
tracks,
seed.pair,
precision_px,
params,
&mut cameras,
&mut points,
&mut intrinsics,
on_event,
)? {
normalized = normalize(tracks, &intrinsics);
threshold = params.max_reprojection * precision_px / mean_focal(&intrinsics);
triangulate_missing(&normalized, &cameras, &mut points, threshold, min_angle);
version += 1;
adjusted_at = version;
}
}
}
for (v, reason) in reasons.into_iter().enumerate() {
let (None, Some(reason)) = (cameras[v], reason) else {
continue;
};
let view = ViewId(v as u32);
emit(
on_event,
Event::ViewDropped {
stage: Stage::Registration,
view,
reason: reason.clone(),
},
)?;
warnings.push(Warning::Unregistered { view, reason });
}
let registered: Vec<ViewId> = (0..views)
.filter(|&v| cameras[v].is_some())
.map(|v| ViewId(v as u32))
.collect();
if registered.len() < MIN_VIEWS {
return Err(Error::RegistrationFailed {
registered,
minimum: MIN_VIEWS,
left_out: warnings
.into_iter()
.filter_map(|warning| match warning {
Warning::Unregistered { view, reason } => Some((view, reason)),
Warning::CollinearCameras { .. } => None,
})
.collect(),
});
}
let focal = mean_focal(&intrinsics);
let points = normalized
.iter()
.enumerate()
.filter_map(|(track, obs)| {
let t = triangulate_track(obs, &cameras, threshold, min_angle)?;
Some(SparsePoint {
position: t.point,
track,
observations: t.observations,
error_px: t.mean_error * focal,
angle_deg: t.angle.to_degrees(),
})
})
.collect();
let centres: Vec<Point3<f64>> = cameras.iter().flatten().map(Pose::centre).collect();
if let Some(deviation) = collinearity(¢res) {
if deviation < params.min_camera_spread {
warnings.push(Warning::CollinearCameras { deviation });
}
}
Ok(SparseModel {
cameras,
points,
seed: seed.pair,
intrinsics,
registrations,
warnings,
})
}
fn mean_focal(intrinsics: &Intrinsics) -> f64 {
(intrinsics.fx + intrinsics.fy) / 2.0
}
fn normalize(tracks: &[Track], intrinsics: &Intrinsics) -> Vec<Vec<(ViewId, Norm)>> {
tracks
.iter()
.map(|t| {
t.observations
.iter()
.map(|&(v, p)| (v, intrinsics.normalize(p)))
.collect()
})
.collect()
}
fn observed(normalized: &[Vec<(ViewId, Norm)>], points: &[Option<World>], view: ViewId) -> usize {
normalized
.iter()
.zip(points)
.filter(|(obs, point)| point.is_some() && obs.iter().any(|(o, _)| *o == view))
.count()
}
#[allow(clippy::too_many_arguments)]
fn adjust_placed(
tracks: &[Track],
seed: PairId,
precision_px: f64,
params: &Params,
cameras: &mut [Option<Pose>],
points: &mut [Option<World>],
intrinsics: &mut Intrinsics,
on_event: &mut dyn FnMut(Event) -> Flow,
) -> Result<bool, Error> {
let model = SparseModel {
cameras: cameras.to_vec(),
points: points
.iter()
.enumerate()
.filter_map(|(track, point)| {
Some(SparsePoint {
position: (*point)?,
track,
observations: 0,
error_px: 0.0,
angle_deg: 0.0,
})
})
.collect(),
seed,
intrinsics: *intrinsics,
registrations: Vec::new(),
warnings: Vec::new(),
};
let Ok(adjusted) = ba::adjust(&model, tracks, intrinsics, precision_px, ¶ms.adjust) else {
return Ok(false);
};
let placed = cameras.iter().flatten().count();
let mut message = format!(
"adjusted the {placed} views placed so far: median reprojection error {:.2} px → {:.2} px",
adjusted.report.initial_median_px, adjusted.report.final_median_px
);
if params.adjust.refine_focal {
message += &format!(
"; focal length {:.1} px → {:.1} px",
intrinsics.fx, adjusted.intrinsics.fx
);
}
report(
on_event,
Stage::Registration,
placed as f32 / cameras.len() as f32,
message,
)?;
cameras.copy_from_slice(&adjusted.model.cameras);
points.fill(None);
for point in &adjusted.model.points {
points[point.track] = Some(point.position);
}
*intrinsics = Intrinsics {
source: intrinsics.source,
..adjusted.intrinsics
};
Ok(true)
}
fn seed_score(relative: &RelativePose) -> f64 {
relative.coverage as f64 * relative.median_angle_deg
}
struct Triangulated {
point: World,
observations: usize,
mean_error: f64,
angle: f64,
}
fn triangulate_track(
observations: &[(ViewId, Norm)],
cameras: &[Option<Pose>],
threshold: f64,
min_angle: f64,
) -> Option<Triangulated> {
let observations: Vec<(Pose, Norm)> = observations
.iter()
.filter_map(|&(v, n)| Some((cameras.get(v.index()).copied().flatten()?, n)))
.collect();
if observations.len() < 2 {
return None;
}
let initial = triangulate::dlt(&observations)?;
let point = triangulate::refine(&observations, initial, 10);
if !triangulate::in_front(&observations, &point) {
return None;
}
let errors: Vec<f64> = triangulate::reprojection_errors(&observations, &point).collect();
if errors.iter().any(|&e| e.is_nan() || e > threshold) {
return None;
}
let centres: Vec<Point3<f64>> = observations.iter().map(|(pose, _)| pose.centre()).collect();
let mut angle = 0.0f64;
for (i, a) in centres.iter().enumerate() {
for b in ¢res[i + 1..] {
angle = angle.max(triangulate::triangulation_angle(a, b, &point));
}
}
if angle < min_angle {
return None;
}
Some(Triangulated {
point,
observations: observations.len(),
mean_error: errors.iter().sum::<f64>() / errors.len() as f64,
angle,
})
}
fn triangulate_missing(
normalized: &[Vec<(ViewId, Norm)>],
cameras: &[Option<Pose>],
points: &mut [Option<World>],
threshold: f64,
min_angle: f64,
) {
for (obs, slot) in normalized.iter().zip(points.iter_mut()) {
if slot.is_none() {
*slot = triangulate_track(obs, cameras, threshold, min_angle).map(|t| t.point);
}
}
}
fn collinearity(centres: &[Point3<f64>]) -> Option<f64> {
if centres.len() < 3 {
return None;
}
let mut farthest = (0, 1, 0.0);
for (i, a) in centres.iter().enumerate() {
for (j, b) in centres.iter().enumerate().skip(i + 1) {
let d = (a - b).norm();
if d > farthest.2 {
farthest = (i, j, d);
}
}
}
let (i, j, length) = farthest;
if length <= 0.0 {
return None;
}
let direction = (centres[j] - centres[i]) / length;
let deviation = centres
.iter()
.map(|c| {
let offset = c - centres[i];
(offset - direction * offset.dot(&direction)).norm()
})
.fold(0.0, f64::max);
Some(deviation / length)
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn measures_how_far_cameras_stray_from_a_line() {
let line = [
Point3::new(0.0, 0.0, 0.0),
Point3::new(1.0, 0.0, 0.0),
Point3::new(2.0, 0.01, 0.0),
];
assert!(collinearity(&line).unwrap() < 0.01);
let arc = [
Point3::new(0.0, 0.0, 0.0),
Point3::new(1.0, 0.3, 0.0),
Point3::new(2.0, 0.0, 0.0),
];
assert!((collinearity(&arc).unwrap() - 0.15).abs() < 1e-9);
assert!(collinearity(&arc[..2]).is_none());
}
}