ringgrid 0.5.6

Pure-Rust detector for coded ring calibration targets
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151

use crate::DetectedMarker;
use crate::board_layout::BoardLayout;
use crate::conic::{fit_ellipse_direct, rms_sampson_distance};
use crate::homography;

use super::super::{DivisionModel, PixelMapper};

/// Minimum number of edge points per ring required for conic refit.
const MIN_EDGE_POINTS: usize = 6;

/// Edge point data for a single marker: (outer_points, inner_points).
pub(super) type MarkerEdgeData = (Vec<[f64; 2]>, Vec<[f64; 2]>);

#[derive(Debug, Clone, Copy)]
pub(super) struct HomographySelfError {
    pub mean_error_px: f64,
    pub n_inliers: usize,
}

pub(super) fn collect_marker_edge_data(markers: &[DetectedMarker]) -> Vec<MarkerEdgeData> {
    markers
        .iter()
        .filter_map(|m| {
            let outer = m.edge_points_outer.as_ref()?;
            let inner = m.edge_points_inner.as_ref()?;
            if outer.len() >= MIN_EDGE_POINTS && inner.len() >= MIN_EDGE_POINTS {
                Some((outer.clone(), inner.clone()))
            } else {
                None
            }
        })
        .collect()
}

fn trimmed_mean(values: &mut [f64], trim_fraction: f64) -> Option<f64> {
    if values.is_empty() {
        return None;
    }
    values.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
    let n = values.len();
    let trim = ((n as f64) * trim_fraction.clamp(0.0, 0.49)).floor() as usize;
    if 2 * trim >= n {
        return None;
    }
    let slice = &values[trim..(n - trim)];
    if slice.is_empty() {
        return None;
    }
    Some(slice.iter().sum::<f64>() / slice.len() as f64)
}

/// Compute a robust projective-conic objective across markers when edge points
/// are undistorted with the given lambda.
pub(super) fn conic_consistency_objective(
    lambda: f64,
    marker_edge_data: &[MarkerEdgeData],
    image_center: [f64; 2],
    trim_fraction: f64,
) -> f64 {
    let model = DivisionModel::new(lambda, image_center[0], image_center[1]);
    let mut marker_objective = Vec::with_capacity(marker_edge_data.len());

    for (outer_pts, inner_pts) in marker_edge_data {
        let outer_ud = model.undistort_points(outer_pts);
        let inner_ud = model.undistort_points(inner_pts);

        let Some(outer_ellipse) = fit_ellipse_direct(&outer_ud) else {
            continue;
        };
        let Some(inner_ellipse) = fit_ellipse_direct(&inner_ud) else {
            continue;
        };

        let rms_outer = rms_sampson_distance(&outer_ellipse, &outer_ud);
        let rms_inner = rms_sampson_distance(&inner_ellipse, &inner_ud);
        if !rms_outer.is_finite() || !rms_inner.is_finite() {
            continue;
        }

        let value = 0.5 * (rms_outer + rms_inner);
        if value.is_finite() {
            marker_objective.push(value);
        }
    }

    if marker_objective.is_empty() {
        return f64::MAX;
    }

    let Some(base) = trimmed_mean(&mut marker_objective, trim_fraction) else {
        return f64::MAX;
    };

    // Mild regularization to avoid unstable large-|lambda| solutions when
    // objective curvature is flat.
    let lambda_reg = 1e-6 * (lambda * 1.0e6).powi(2);
    base + lambda_reg
}

pub(super) fn homography_self_error_px(
    markers: &[DetectedMarker],
    board: &BoardLayout,
    mapper: &dyn PixelMapper,
) -> Option<HomographySelfError> {
    let correspondences = homography::collect_marker_correspondences(
        markers,
        board,
        homography::CorrespondenceDestinationFrame::Working,
        homography::DuplicateIdPolicy::KeepHighestConfidencePerIdSorted,
        |m| {
            mapper
                .image_to_working_pixel(m.center)
                .and_then(|center_w| {
                    (center_w[0].is_finite() && center_w[1].is_finite()).then_some(center_w)
                })
        },
    );
    debug_assert_eq!(
        correspondences.dst_frame,
        homography::CorrespondenceDestinationFrame::Working
    );
    if correspondences.len() < 4 {
        return None;
    }

    let ransac_cfg = homography::RansacHomographyConfig {
        max_iters: 1000,
        inlier_threshold: 5.0,
        min_inliers: 8,
        seed: 0,
    };

    let Ok(res) = homography::fit_homography_ransac(
        &correspondences.src_board_mm,
        &correspondences.dst_points,
        &ransac_cfg,
    ) else {
        return None;
    };

    if res.n_inliers == 0 {
        return None;
    }

    let (mean_error_px, n_inliers) =
        homography::mean_finite_masked_inlier_error(&res.errors, &res.inlier_mask)?;
    Some(HomographySelfError {
        mean_error_px,
        n_inliers,
    })
}