use nalgebra::{Point3, Vector3};
use crate::calib::Intrinsics;
use crate::error::Error;
use crate::event::{report, silent, Event, Flow, Stage};
use crate::lookup::PairLookup;
use crate::pairs::PairGraph;
use crate::pose::Pose;
use crate::triangulate;
use crate::types::{Norm, PhotoPx, ViewId, World};
pub const MIN_VALID_FRACTION: f64 = 0.15;
#[derive(Clone, Debug)]
pub struct Params {
pub stride: Option<usize>,
pub max_reprojection: f64,
pub min_angle_deg: f64,
pub max_depth_difference: f64,
pub depth_agreement_sigmas: f64,
pub min_consistent_views: usize,
pub max_depth_step: f64,
pub min_component: usize,
pub smooth: bool,
}
impl Default for Params {
fn default() -> Self {
Params {
stride: None,
max_reprojection: 10.0,
min_angle_deg: 7.0,
max_depth_difference: 0.5,
depth_agreement_sigmas: 5.0,
min_consistent_views: 1,
max_depth_step: 0.2,
min_component: 64,
smooth: true,
}
}
}
#[derive(Clone, Debug, PartialEq)]
#[non_exhaustive]
pub struct DepthMap {
pub view: ViewId,
pub columns: usize,
pub rows: usize,
pub stride: usize,
pub depth: Vec<f32>,
pub uncertainty: Vec<f32>,
}
impl DepthMap {
pub fn pixel(&self, column: usize, row: usize) -> PhotoPx {
PhotoPx::new((column * self.stride) as f32, (row * self.stride) as f32)
}
pub fn get(&self, column: usize, row: usize) -> Option<f32> {
let d = self.depth[row * self.columns + column];
d.is_finite().then_some(d)
}
pub fn sample(&self, p: PhotoPx) -> Option<f32> {
let (column, row) = self.nearest(p)?;
self.get(column, row)
}
pub fn get_with_uncertainty(&self, column: usize, row: usize) -> Option<(f32, f32)> {
let i = row * self.columns + column;
let d = self.depth[i];
d.is_finite().then(|| (d, self.uncertainty[i]))
}
pub fn sample_with_uncertainty(&self, p: PhotoPx) -> Option<(f32, f32)> {
let (column, row) = self.nearest(p)?;
self.get_with_uncertainty(column, row)
}
pub fn index_of(&self, p: PhotoPx) -> Option<usize> {
self.nearest(p)
.map(|(column, row)| row * self.columns + column)
}
fn nearest(&self, p: PhotoPx) -> Option<(usize, usize)> {
let column = (p.x() / self.stride as f32).round();
let row = (p.y() / self.stride as f32).round();
if column < 0.0 || row < 0.0 {
return None;
}
let (column, row) = (column as usize, row as usize);
(column < self.columns && row < self.rows).then_some((column, row))
}
pub fn valid(&self) -> usize {
self.depth.iter().filter(|d| d.is_finite()).count()
}
pub fn valid_fraction(&self) -> f64 {
self.valid() as f64 / self.depth.len().max(1) as f64
}
}
#[derive(Clone, Debug, PartialEq)]
#[non_exhaustive]
pub struct DepthStats {
pub view: ViewId,
pub unmapped: usize,
pub single: Counts,
pub multiple: Counts,
pub fates: Vec<Fate>,
pub matched: Vec<u8>,
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum Fate {
Unmapped,
RejectedByFit,
RejectedByAngle,
RejectedByConsistency,
RemovedAsSpeckle,
Kept,
}
#[derive(Clone, Copy, Debug, Default, PartialEq)]
#[non_exhaustive]
pub struct Counts {
pub samples: usize,
pub rejected_by_fit: usize,
pub rejected_by_angle: usize,
pub rejected_by_consistency: usize,
pub removed_as_speckle: usize,
pub kept: usize,
}
pub fn estimate<L: PairLookup>(
graph: &PairGraph<L>,
cameras: &[Option<Pose>],
intrinsics: &Intrinsics,
size: (usize, usize),
params: &Params,
) -> Vec<DepthMap> {
estimate_with_stats(graph, cameras, intrinsics, size, params).0
}
pub fn estimate_with_stats<L: PairLookup>(
graph: &PairGraph<L>,
cameras: &[Option<Pose>],
intrinsics: &Intrinsics,
size: (usize, usize),
params: &Params,
) -> (Vec<DepthMap>, Vec<DepthStats>) {
estimate_with_progress(graph, cameras, intrinsics, size, params, &mut silent)
.expect("a callback that never breaks cannot cancel")
}
pub fn estimate_with_progress<L: PairLookup>(
graph: &PairGraph<L>,
cameras: &[Option<Pose>],
intrinsics: &Intrinsics,
(width, height): (usize, usize),
params: &Params,
on_event: &mut dyn FnMut(Event) -> Flow,
) -> Result<(Vec<DepthMap>, Vec<DepthStats>), Error> {
let precision = graph.precision_px() as f64;
let native = graph
.pairs()
.map(|(_, lookup)| lookup.native_stride())
.fold(0.0f32, f32::max);
let stride = params
.stride
.unwrap_or_else(|| (native / 3.0).round() as usize)
.max(1);
let focal = (intrinsics.fx + intrinsics.fy) / 2.0;
let threshold = params.max_reprojection * precision / focal;
let min_angle = params.min_angle_deg.to_radians();
let columns = (width - 1) / stride + 1;
let rows = (height - 1) / stride + 1;
let registered: Vec<(ViewId, Pose)> = cameras
.iter()
.enumerate()
.filter_map(|(v, c)| Some((ViewId(v as u32), (*c)?)))
.filter(|(v, _)| v.index() < graph.views())
.collect();
let mut results: Vec<(DepthMap, Vec<u8>, Vec<bool>)> = Vec::with_capacity(registered.len());
for (index, &(view, pose)) in registered.iter().enumerate() {
let to_world = pose.rotation.inverse();
let centre = pose.centre();
let mut depth = vec![f32::NAN; columns * rows];
let mut uncertainty = vec![f32::NAN; columns * rows];
let mut narrow = vec![false; columns * rows];
let mut mapped = vec![0u8; columns * rows];
let mut rays = Vec::with_capacity(registered.len());
for row in 0..rows {
for column in 0..columns {
let p = PhotoPx::new((column * stride) as f32, (row * stride) as f32);
rays.clear();
for &(other, other_pose) in ®istered {
if other == view {
continue;
}
if let Some(q) = graph.directed(view, other).and_then(|m| m.map(p)) {
rays.push((other_pose, intrinsics.normalize(q)));
}
}
let i = row * columns + column;
mapped[i] = rays.len().min(u8::MAX as usize) as u8;
let n = intrinsics.normalize(p);
let direction = to_world * Vector3::new(n.x(), n.y(), 1.0);
let fit = depth_along_ray(¢re, &direction, &mut rays, threshold, min_angle);
narrow[i] = fit == Err(Rejection::Angle);
if let Ok((z, angle)) = fit {
depth[i] = z as f32;
let count = rays.len().max(1) as f64;
uncertainty[i] = (precision / (focal * angle.sin() * count.sqrt())) as f32;
}
}
}
let map = DepthMap {
view,
columns,
rows,
stride,
depth,
uncertainty,
};
let (valid, total) = (map.valid(), map.depth.len());
results.push((map, mapped, narrow));
report(
on_event,
Stage::Depth,
0.6 * (index + 1) as f32 / registered.len() as f32,
format!("{view}: {valid} of {total} samples have a depth"),
)?;
}
let mut raw = Vec::with_capacity(results.len());
let mut mapped = Vec::with_capacity(results.len());
let mut narrow = Vec::with_capacity(results.len());
for (map, counts, angles) in results {
raw.push(map);
mapped.push(counts);
narrow.push(angles);
}
let consistent = consistency_filter(&raw, cameras, intrinsics, precision, params, on_event)?;
let view_count = raw.len();
let mut maps = Vec::with_capacity(raw.len());
let mut stats = Vec::with_capacity(raw.len());
for (index, (((raw, consistent), mapped), narrow)) in raw
.iter()
.zip(consistent)
.zip(&mapped)
.zip(&narrow)
.enumerate()
{
let cleaned = remove_speckles(consistent.clone(), params);
let mut view_stats = DepthStats {
view: raw.view,
unmapped: 0,
single: Counts::default(),
multiple: Counts::default(),
fates: Vec::with_capacity(mapped.len()),
matched: mapped.clone(),
};
for (i, &rays) in mapped.iter().enumerate() {
let fate = if rays == 0 {
Fate::Unmapped
} else if !raw.depth[i].is_finite() && narrow[i] {
Fate::RejectedByAngle
} else if !raw.depth[i].is_finite() {
Fate::RejectedByFit
} else if !consistent.depth[i].is_finite() {
Fate::RejectedByConsistency
} else if !cleaned.depth[i].is_finite() {
Fate::RemovedAsSpeckle
} else {
Fate::Kept
};
view_stats.fates.push(fate);
let counts = match rays {
0 => {
view_stats.unmapped += 1;
continue;
}
1 => &mut view_stats.single,
_ => &mut view_stats.multiple,
};
counts.samples += 1;
match fate {
Fate::RejectedByFit => counts.rejected_by_fit += 1,
Fate::RejectedByAngle => counts.rejected_by_angle += 1,
Fate::RejectedByConsistency => counts.rejected_by_consistency += 1,
Fate::RemovedAsSpeckle => counts.removed_as_speckle += 1,
Fate::Kept => counts.kept += 1,
Fate::Unmapped => {}
}
}
stats.push(view_stats);
maps.push(if params.smooth {
smooth(cleaned, params)
} else {
cleaned
});
report(
on_event,
Stage::Depth,
0.9 + 0.1 * (index + 1) as f32 / view_count as f32,
format!("{}: speckles removed", raw.view),
)?;
}
Ok((maps, stats))
}
pub fn require_coverage(maps: &[DepthMap]) -> Result<f64, Error> {
let total: usize = maps.iter().map(|m| m.depth.len()).sum();
let valid: usize = maps.iter().map(DepthMap::valid).sum();
let fraction = valid as f64 / total.max(1) as f64;
if fraction < MIN_VALID_FRACTION {
return Err(Error::InsufficientDepth {
valid_fraction: fraction,
required: MIN_VALID_FRACTION,
});
}
Ok(fraction)
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
enum Rejection {
Fit,
Angle,
}
fn depth_along_ray(
centre: &Point3<f64>,
direction: &Vector3<f64>,
rays: &mut Vec<(Pose, Norm)>,
threshold: f64,
min_angle: f64,
) -> Result<(f64, f64), Rejection> {
while !rays.is_empty() {
let z = solve_depth(centre, direction, rays).ok_or(Rejection::Fit)?;
if z <= 0.0 {
return Err(Rejection::Fit);
}
let point = World(centre + direction * z);
let (worst, error) = rays
.iter()
.enumerate()
.map(|(i, (pose, n))| {
let error = pose
.project(&point)
.map_or(f64::INFINITY, |p| (p.x() - n.x()).hypot(p.y() - n.y()));
(i, error)
})
.max_by(|a, b| a.1.total_cmp(&b.1))
.ok_or(Rejection::Fit)?;
if error <= threshold {
let angle = rays
.iter()
.map(|(pose, _)| triangulate::triangulation_angle(centre, &pose.centre(), &point))
.fold(0.0, f64::max);
return if angle >= min_angle {
Ok((z, angle))
} else {
Err(Rejection::Angle)
};
}
rays.swap_remove(worst);
}
Err(Rejection::Fit)
}
fn solve_depth(
centre: &Point3<f64>,
direction: &Vector3<f64>,
rays: &[(Pose, Norm)],
) -> Option<f64> {
let (mut num, mut den) = (0.0, 0.0);
for (pose, n) in rays {
let a = pose.to_camera(centre).coords;
let b = pose.rotation * direction;
for (u, ai, bi) in [(n.x(), a.x, b.x), (n.y(), a.y, b.y)] {
let alpha = u * a.z - ai;
let beta = u * b.z - bi;
num += alpha * beta;
den += beta * beta;
}
}
if den <= 1e-18 {
return None;
}
let mut z = -num / den;
for _ in 0..5 {
let (mut g, mut h) = (0.0, 0.0);
for (pose, n) in rays {
let c = pose.to_camera(&(centre + direction * z));
if c.z <= 1e-9 {
continue;
}
let b = pose.rotation * direction;
let (px, py) = (c.x / c.z, c.y / c.z);
let (jx, jy) = ((b.x - px * b.z) / c.z, (b.y - py * b.z) / c.z);
g += jx * (px - n.x()) + jy * (py - n.y());
h += jx * jx + jy * jy;
}
if h <= 1e-18 {
break;
}
let step = g / h;
z -= step;
if step.abs() <= 1e-9 * z.abs() {
break;
}
}
z.is_finite().then_some(z)
}
fn consistency_filter(
maps: &[DepthMap],
cameras: &[Option<Pose>],
intrinsics: &Intrinsics,
precision: f64,
params: &Params,
on_event: &mut dyn FnMut(Event) -> Flow,
) -> Result<Vec<DepthMap>, Error> {
let tolerance_px = params.max_reprojection * precision;
let mut filtered_maps = Vec::with_capacity(maps.len());
for (index, map) in maps.iter().enumerate() {
let pose = cameras[map.view.index()].expect("depth maps are of registered views");
let to_world = pose.inverse();
let mut filtered = map.clone();
for row in 0..map.rows {
for column in 0..map.columns {
let Some((d, sigma)) = map.get_with_uncertainty(column, row) else {
continue;
};
let d = d as f64;
let p = map.pixel(column, row);
let n = intrinsics.normalize(p);
let world = to_world.to_camera(&Point3::new(n.x() * d, n.y() * d, d));
let agreeing = maps
.iter()
.filter(|other| other.view != map.view)
.filter(|other| {
let other_pose = cameras[other.view.index()]
.expect("depth maps are of registered views");
let c = other_pose.to_camera(&world);
if c.z <= 0.0 {
return false;
}
let q = intrinsics.denormalize(Norm::new(c.x / c.z, c.y / c.z));
let Some((ds, other_sigma)) = other.sample_with_uncertainty(q) else {
return false;
};
let ds = ds as f64;
let nq = intrinsics.normalize(q);
let back = other_pose.inverse().to_camera(&Point3::new(
nq.x() * ds,
nq.y() * ds,
ds,
));
let in_reference = pose.to_camera(&back);
if in_reference.z <= 0.0 {
return false;
}
let reprojected = intrinsics.denormalize(Norm::new(
in_reference.x / in_reference.z,
in_reference.y / in_reference.z,
));
let pixels = (reprojected.0 - p.0).norm() as f64;
let difference = ((in_reference.z - d) / d).abs();
let allowed = params.max_depth_difference.max(
params.depth_agreement_sigmas
* (sigma as f64).hypot(other_sigma as f64),
);
let relaxation = allowed / params.max_depth_difference;
pixels <= tolerance_px * relaxation && difference <= allowed
})
.count();
if agreeing < params.min_consistent_views {
filtered.depth[row * map.columns + column] = f32::NAN;
}
}
}
filtered_maps.push(filtered);
report(
on_event,
Stage::Depth,
0.6 + 0.3 * (index + 1) as f32 / maps.len() as f32,
format!("{}: cross-checked against the other views", map.view),
)?;
}
Ok(filtered_maps)
}
fn continuous(a: (f32, f32), b: (f32, f32), step: f64) -> bool {
let allowed = step.max(3.0 * (a.1 + b.1) as f64);
((a.0 - b.0).abs() as f64) <= allowed * a.0.max(b.0) as f64
}
fn remove_speckles(mut map: DepthMap, params: &Params) -> DepthMap {
let (columns, rows) = (map.columns, map.rows);
let mut visited = vec![false; map.depth.len()];
let mut component = Vec::new();
let mut stack = Vec::new();
for start in 0..map.depth.len() {
if visited[start] || !map.depth[start].is_finite() {
continue;
}
visited[start] = true;
component.clear();
stack.push(start);
while let Some(i) = stack.pop() {
component.push(i);
let (column, row) = (i % columns, i / columns);
let neighbours = [
(column > 0).then(|| i - 1),
(column + 1 < columns).then(|| i + 1),
(row > 0).then(|| i - columns),
(row + 1 < rows).then(|| i + columns),
];
for j in neighbours.into_iter().flatten() {
if !visited[j]
&& map.depth[j].is_finite()
&& continuous(
(map.depth[i], map.uncertainty[i]),
(map.depth[j], map.uncertainty[j]),
params.max_depth_step,
)
{
visited[j] = true;
stack.push(j);
}
}
}
if component.len() < params.min_component {
for &i in &component {
map.depth[i] = f32::NAN;
}
}
}
map
}
fn smooth(map: DepthMap, params: &Params) -> DepthMap {
let mut smoothed = map.clone();
let mut window = Vec::with_capacity(9);
for row in 0..map.rows {
for column in 0..map.columns {
let Some(d) = map.get_with_uncertainty(column, row) else {
continue;
};
window.clear();
for r in row.saturating_sub(1)..(row + 2).min(map.rows) {
for c in column.saturating_sub(1)..(column + 2).min(map.columns) {
if let Some(n) = map.get_with_uncertainty(c, r) {
if continuous(d, n, params.max_depth_step) {
window.push(n.0);
}
}
}
}
window.sort_by(f32::total_cmp);
smoothed.depth[row * map.columns + column] = window[window.len() / 2];
}
}
smoothed
}
#[cfg(test)]
mod tests {
use super::*;
fn map(depth: Vec<f32>, columns: usize) -> DepthMap {
DepthMap {
view: ViewId(0),
columns,
rows: depth.len() / columns,
stride: 4,
uncertainty: vec![0.001; depth.len()],
depth,
}
}
#[test]
fn samples_the_nearest_grid_point() {
let m = map(vec![1.0, 2.0, 3.0, f32::NAN], 2);
assert_eq!(m.sample(PhotoPx::new(1.9, 0.0)), Some(1.0));
assert_eq!(m.sample(PhotoPx::new(2.1, 0.0)), Some(2.0));
assert_eq!(m.sample(PhotoPx::new(0.0, 4.0)), Some(3.0));
assert_eq!(m.sample(PhotoPx::new(4.0, 4.0)), None);
assert_eq!(m.sample(PhotoPx::new(8.0, 0.0)), None);
assert_eq!(m.valid(), 3);
}
#[test]
fn removes_small_patches_and_keeps_edges_sharp() {
let params = Params {
min_component: 5,
..Params::default()
};
let mut depth = Vec::new();
for _ in 0..4 {
depth.extend([1.0, 1.0, 1.0, 2.0]);
}
let cleaned = remove_speckles(map(depth, 4), ¶ms);
assert_eq!(cleaned.valid(), 12);
assert!(cleaned.get(3, 0).is_none());
let mut step = Vec::new();
for _ in 0..3 {
step.extend([1.0, 1.0, 3.0, 3.0]);
}
let smoothed = smooth(map(step.clone(), 4), ¶ms);
assert_eq!(
smoothed.depth, step,
"smoothing must not blur across the edge"
);
}
#[test]
fn solves_depth_along_a_ray() {
let target = Point3::new(0.3, -0.2, 4.0);
let reference = Pose::identity();
let others = [
Pose::look_at(&Point3::new(1.0, 0.0, 0.0), &target, &Vector3::y()),
Pose::look_at(&Point3::new(-0.8, 0.5, 0.2), &target, &Vector3::y()),
];
let mut rays: Vec<(Pose, Norm)> = others
.iter()
.map(|pose| (*pose, pose.project(&World(target)).unwrap()))
.collect();
rays.push((others[0], Norm::new(0.4, 0.4)));
let n = reference.project(&World(target)).unwrap();
let direction = Vector3::new(n.x(), n.y(), 1.0);
let (z, _angle) = depth_along_ray(
&reference.centre(),
&direction,
&mut rays,
1e-3,
1f64.to_radians(),
)
.expect("the honest views agree");
assert!((z - 4.0).abs() < 1e-6, "depth {z}");
assert_eq!(rays.len(), 2, "the lying view was dropped");
}
}