From cb28a2f1d264ed11fa2f37c88db88bc638907fb7 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 7 May 2026 13:41:03 +0000 Subject: [PATCH 01/11] Initial plan From 2557c96353841928e0fb87fba069b5ca9f8fbba4 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 7 May 2026 13:56:01 +0000 Subject: [PATCH 02/11] chore(core): add calib3d module scaffold Agent-Logs-Url: https://github.com/webarkit/purecv/sessions/38005598-60bc-4ebd-967c-f0aa8ac3425f Co-authored-by: kalwalt <1275858+kalwalt@users.noreply.github.com> --- src/calib3d.rs | 39 +++++++++++++++++++++++++++++++++++++++ src/lib.rs | 1 + 2 files changed, 40 insertions(+) create mode 100644 src/calib3d.rs diff --git a/src/calib3d.rs b/src/calib3d.rs new file mode 100644 index 0000000..cd2bf81 --- /dev/null +++ b/src/calib3d.rs @@ -0,0 +1,39 @@ +/* + * calib3d.rs + * purecv + * + * This file is part of purecv - WebARKit. + * + * purecv is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * purecv is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with purecv. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2026 WebARKit. + * + * Author(s): Walter Perdan @kalwalt https://github.com/kalwalt + * + */ + +//! Camera calibration and 3D geometry module. +//! +//! This module mirrors OpenCV's `calib3d` module structure. diff --git a/src/lib.rs b/src/lib.rs index e3b3bb6..d5d24cb 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -35,6 +35,7 @@ */ // Global modules +pub mod calib3d; pub mod core; pub mod features; pub mod imgproc; From 5322aba528092e4e457ff37602e94b104a672586 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 7 May 2026 13:58:29 +0000 Subject: [PATCH 03/11] chore(core): structure calib3d module with tests and simd stubs Agent-Logs-Url: https://github.com/webarkit/purecv/sessions/38005598-60bc-4ebd-967c-f0aa8ac3425f Co-authored-by: kalwalt <1275858+kalwalt@users.noreply.github.com> --- src/calib3d/mod.rs | 53 +++++++++++++++++++++++++++++ src/{calib3d.rs => calib3d/simd.rs} | 6 ++-- src/calib3d/tests.rs | 44 ++++++++++++++++++++++++ 3 files changed, 99 insertions(+), 4 deletions(-) create mode 100644 src/calib3d/mod.rs rename src/{calib3d.rs => calib3d/simd.rs} (92%) create mode 100644 src/calib3d/tests.rs diff --git a/src/calib3d/mod.rs b/src/calib3d/mod.rs new file mode 100644 index 0000000..4db7616 --- /dev/null +++ b/src/calib3d/mod.rs @@ -0,0 +1,53 @@ +/* + * mod.rs + * purecv + * + * This file is part of purecv - WebARKit. + * + * purecv is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * purecv is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with purecv. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2026 WebARKit. + * + * Author(s): Walter Perdan @kalwalt https://github.com/kalwalt + * + */ + +//! Camera calibration and 3D geometry module. +//! +//! This module mirrors OpenCV's `calib3d` module structure and is the home for +//! Milestone 4 APIs such as: +//! - `find_homography` +//! - `rodrigues` +//! - `solve_pnp` +//! - `solve_pnp_ransac` +//! +//! Implementations in this module follow the project architectural rules: +//! pure Rust, safe memory handling, and feature-gated optimizations. + +#[cfg(feature = "simd")] +pub(crate) mod simd; + +#[cfg(test)] +mod tests; diff --git a/src/calib3d.rs b/src/calib3d/simd.rs similarity index 92% rename from src/calib3d.rs rename to src/calib3d/simd.rs index cd2bf81..52c5d8b 100644 --- a/src/calib3d.rs +++ b/src/calib3d/simd.rs @@ -1,5 +1,5 @@ /* - * calib3d.rs + * simd.rs * purecv * * This file is part of purecv - WebARKit. @@ -34,6 +34,4 @@ * */ -//! Camera calibration and 3D geometry module. -//! -//! This module mirrors OpenCV's `calib3d` module structure. +//! SIMD helpers for `calib3d` algorithms. diff --git a/src/calib3d/tests.rs b/src/calib3d/tests.rs new file mode 100644 index 0000000..2e42e91 --- /dev/null +++ b/src/calib3d/tests.rs @@ -0,0 +1,44 @@ +/* + * tests.rs + * purecv + * + * This file is part of purecv - WebARKit. + * + * purecv is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * purecv is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with purecv. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2026 WebARKit. + * + * Author(s): Walter Perdan @kalwalt https://github.com/kalwalt + * + */ + +#[cfg(test)] +mod calib3d_tests { + #[test] + fn test_calib3d_module_scaffold_present() { + // Placeholder test to keep module test structure in place. + assert!(true); + } +} From 768e1396fd14446c0c309c78ff5074dc5eb4f758 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 7 May 2026 20:01:43 +0000 Subject: [PATCH 04/11] feat(calib3d): implement find_homography, rodrigues, solve_pnp, solve_pnp_ransac Agent-Logs-Url: https://github.com/webarkit/purecv/sessions/5fc1b86e-5d0e-46ea-a26c-d3af094c49f6 Co-authored-by: kalwalt <1275858+kalwalt@users.noreply.github.com> --- src/{calib3d/mod.rs => calib3d.rs} | 41 +- src/calib3d/geometry.rs | 218 +++++++++ src/calib3d/homography.rs | 371 ++++++++++++++ src/calib3d/linalg.rs | 335 +++++++++++++ src/calib3d/pose.rs | 752 +++++++++++++++++++++++++++++ src/calib3d/tests.rs | 373 +++++++++++++- src/lib.rs | 9 +- 7 files changed, 2083 insertions(+), 16 deletions(-) rename src/{calib3d/mod.rs => calib3d.rs} (55%) create mode 100644 src/calib3d/geometry.rs create mode 100644 src/calib3d/homography.rs create mode 100644 src/calib3d/linalg.rs create mode 100644 src/calib3d/pose.rs diff --git a/src/calib3d/mod.rs b/src/calib3d.rs similarity index 55% rename from src/calib3d/mod.rs rename to src/calib3d.rs index 4db7616..8f1775f 100644 --- a/src/calib3d/mod.rs +++ b/src/calib3d.rs @@ -1,5 +1,5 @@ /* - * mod.rs + * calib3d.rs * purecv * * This file is part of purecv - WebARKit. @@ -34,20 +34,41 @@ * */ -//! Camera calibration and 3D geometry module. +//! Camera calibration and 3-D geometry — the `calib3d` module. //! -//! This module mirrors OpenCV's `calib3d` module structure and is the home for -//! Milestone 4 APIs such as: -//! - `find_homography` -//! - `rodrigues` -//! - `solve_pnp` -//! - `solve_pnp_ransac` +//! This module mirrors the structure of [OpenCV's `calib3d` module][ocv] and +//! implements the following **Milestone 4** APIs in pure Rust: //! -//! Implementations in this module follow the project architectural rules: -//! pure Rust, safe memory handling, and feature-gated optimizations. +//! | Function | Description | +//! |----------|-------------| +//! | [`find_homography`] | Compute a perspective homography from point correspondences | +//! | [`rodrigues`] | Convert between rotation vector and rotation matrix | +//! | [`solve_pnp`] | Estimate camera pose from 3-D / 2-D correspondences | +//! | [`solve_pnp_ransac`]| Robust pose estimation with RANSAC | +//! +//! # Conventions +//! +//! All functions follow the standard OpenCV coordinate convention (right-hand, +//! z-axis pointing away from the camera) and accept `f64` matrices +//! (`Matrix`) or point-slice inputs (`&[Point2f]`, `&[Point3f]`). +//! +//! [ocv]: https://docs.opencv.org/4.10.0/d9/d0c/group__calib3d.html + +pub mod geometry; +pub mod homography; +pub(crate) mod linalg; +pub mod pose; #[cfg(feature = "simd")] pub(crate) mod simd; #[cfg(test)] mod tests; + +// --------------------------------------------------------------------------- +// Re-exports +// --------------------------------------------------------------------------- + +pub use geometry::rodrigues; +pub use homography::{find_homography, HomographyMethod}; +pub use pose::{solve_pnp, solve_pnp_ransac, SolvePnPMethod}; diff --git a/src/calib3d/geometry.rs b/src/calib3d/geometry.rs new file mode 100644 index 0000000..adca937 --- /dev/null +++ b/src/calib3d/geometry.rs @@ -0,0 +1,218 @@ +/* + * geometry.rs + * purecv + * + * This file is part of purecv - WebARKit. + * + * purecv is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * purecv is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with purecv. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2026 WebARKit. + * + * Author(s): Walter Perdan @kalwalt https://github.com/kalwalt + * + */ + +//! Rodrigues rotation — `rodrigues`. +//! +//! Converts between a *rotation vector* (compact axis-angle representation, +//! sometimes called an *Rodrigues vector*) and a 3×3 *rotation matrix*. + +use crate::core::error::{PureCvError, Result}; +use crate::core::Matrix; + +// --------------------------------------------------------------------------- +// Public API +// --------------------------------------------------------------------------- + +/// Converts a rotation matrix to a rotation vector, or vice-versa. +/// +/// The function behaves identically to `cv::Rodrigues`: +/// +/// | `src` shape | `dst` shape | Operation | +/// |-------------|-------------|-----------| +/// | 3×1 or 1×3 | 3×3 | rotation vector → rotation matrix | +/// | 3×3 | 3×1 | rotation matrix → rotation vector | +/// +/// The rotation vector `r` encodes the axis of rotation `r/‖r‖` and the +/// angle of rotation `θ = ‖r‖` (in radians) following the right-hand rule. +/// +/// # Errors +/// +/// Returns [`PureCvError::InvalidInput`] when `src` has an unsupported shape. +/// +/// # References +/// +/// Rodrigues, O. (1840). *Des lois géométriques qui régissent les déplacements +/// d'un système solide dans l'espace* — Rodrigues' rotation formula. +pub fn rodrigues(src: &Matrix, dst: &mut Matrix) -> Result<()> { + let elem = src.rows * src.cols * src.channels; + + if src.channels != 1 { + return Err(PureCvError::InvalidInput( + "rodrigues: src must be single-channel".to_string(), + )); + } + + match (src.rows, src.cols, elem) { + // Rotation vector (3×1 or 1×3) → rotation matrix (3×3) + (3, 1, 3) | (1, 3, 3) => { + let rx = src.data[0]; + let ry = src.data[1]; + let rz = src.data[2]; + let r = rvec_to_rmat(rx, ry, rz); + dst.rows = 3; + dst.cols = 3; + dst.channels = 1; + dst.data = r.to_vec(); + Ok(()) + } + // Rotation matrix (3×3) → rotation vector (3×1) + (3, 3, 9) => { + let m: [f64; 9] = src + .data + .as_slice() + .try_into() + .map_err(|_| PureCvError::InternalError("unexpected data layout".into()))?; + let [rx, ry, rz] = rmat_to_rvec(&m); + dst.rows = 3; + dst.cols = 1; + dst.channels = 1; + dst.data = vec![rx, ry, rz]; + Ok(()) + } + _ => Err(PureCvError::InvalidInput( + "rodrigues: src must be 3×1, 1×3, or 3×3 (single-channel f64)".to_string(), + )), + } +} + +// --------------------------------------------------------------------------- +// Internal helpers +// --------------------------------------------------------------------------- + +/// Rotation vector `(rx, ry, rz)` → 3×3 rotation matrix (row-major). +/// +/// Uses the Rodrigues formula: +/// ```text +/// R = cos(θ)·I + (1 − cos(θ))·k·kᵀ + sin(θ)·[k]× +/// ``` +/// where `θ = ‖r‖` and `k = r / θ`. +pub(super) fn rvec_to_rmat(rx: f64, ry: f64, rz: f64) -> [f64; 9] { + let theta = (rx * rx + ry * ry + rz * rz).sqrt(); + + if theta < 1e-10 { + // Identity rotation. + return [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]; + } + + let kx = rx / theta; + let ky = ry / theta; + let kz = rz / theta; + let c = theta.cos(); + let s = theta.sin(); + let t = 1.0 - c; + + // Rodrigues' formula (expanded): + [ + t * kx * kx + c, + t * kx * ky - s * kz, + t * kx * kz + s * ky, + t * kx * ky + s * kz, + t * ky * ky + c, + t * ky * kz - s * kx, + t * kx * kz - s * ky, + t * ky * kz + s * kx, + t * kz * kz + c, + ] +} + +/// 3×3 rotation matrix (row-major) → rotation vector. +/// +/// ```text +/// θ = arccos((trace(R) − 1) / 2) +/// r = θ / (2 sin θ) · [R₃₂ − R₂₃, R₁₃ − R₃₁, R₂₁ − R₁₂] +/// ``` +pub(super) fn rmat_to_rvec(m: &[f64; 9]) -> [f64; 3] { + // Clamp argument of arccos to [−1, 1] to guard against floating-point noise. + let trace_val = ((m[0] + m[4] + m[8] - 1.0) * 0.5).clamp(-1.0, 1.0); + let theta = trace_val.acos(); + + if theta.abs() < 1e-10 { + // Near-identity: zero rotation vector. + return [0.0, 0.0, 0.0]; + } + + // Near π the formula becomes numerically unstable; use an alternative. + if (theta - std::f64::consts::PI).abs() < 1e-4 { + return rmat_to_rvec_near_pi(m, theta); + } + + let factor = theta / (2.0 * theta.sin()); + [ + factor * (m[7] - m[5]), + factor * (m[2] - m[6]), + factor * (m[3] - m[1]), + ] +} + +/// Special-case rotation matrix → rvec near `θ ≈ π`. +/// +/// When `sin(θ) ≈ 0`, the standard formula is ill-conditioned. Instead we +/// extract the axis from the diagonal of `(R + I) / 2 = k·kᵀ`. +fn rmat_to_rvec_near_pi(m: &[f64; 9], theta: f64) -> [f64; 3] { + // (R + Rᵀ) / 2 = cos(θ)·I + (1−cos(θ))·k·kᵀ + // For θ = π: cos(θ) = −1, so (R + I)/2 = k·kᵀ. + // Diagonal: k_i² = (m[i*4] + 1) / 2. + let kx2 = ((m[0] + 1.0) * 0.5).max(0.0); + let ky2 = ((m[4] + 1.0) * 0.5).max(0.0); + let kz2 = ((m[8] + 1.0) * 0.5).max(0.0); + + let kx = kx2.sqrt(); + let ky = ky2.sqrt(); + let kz = kz2.sqrt(); + + // Resolve sign ambiguity from off-diagonal entries. + // m[1] = (1−cosθ)·kx·ky + sinθ·(-kz); for θ≈π sinθ≈0 so sign(m[1]) = sign(kx·ky). + let (kx, ky, kz) = fix_sign(kx, ky, kz, m); + + [theta * kx, theta * ky, theta * kz] +} + +/// Fix the sign of the axis components using off-diagonal matrix entries. +fn fix_sign(kx: f64, mut ky: f64, mut kz: f64, m: &[f64; 9]) -> (f64, f64, f64) { + // m[1] ≈ (1-cosθ)*kx*ky (for θ≈π) — positive iff kx and ky have same sign. + if m[1] < 0.0 { + ky = -ky; + } + // m[2] ≈ (1-cosθ)*kx*kz + if m[2] < 0.0 { + kz = -kz; + } + // Verify: m[5] ≈ (1-cosθ)*ky*kz; if not consistent, flip kz. + if m[5] < 0.0 && (ky * kz > 0.0) { + kz = -kz; + } + (kx, ky, kz) +} diff --git a/src/calib3d/homography.rs b/src/calib3d/homography.rs new file mode 100644 index 0000000..6a037e2 --- /dev/null +++ b/src/calib3d/homography.rs @@ -0,0 +1,371 @@ +/* + * homography.rs + * purecv + * + * This file is part of purecv - WebARKit. + * + * purecv is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * purecv is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with purecv. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2026 WebARKit. + * + * Author(s): Walter Perdan @kalwalt https://github.com/kalwalt + * + */ + +//! Perspective homography estimation — `find_homography`. +//! +//! Implements the *Direct Linear Transform* (DLT) algorithm with isotropic +//! point normalization, plus an optional RANSAC wrapper for robustness +//! against outliers. + +use crate::core::error::{PureCvError, Result}; +use crate::core::types::Point2f; +use crate::core::Matrix; + +use super::linalg::{mat3_inv, mat3_mul, null_space_vector, Lcg}; + +// --------------------------------------------------------------------------- +// Public API +// --------------------------------------------------------------------------- + +/// Method used to compute a homography matrix in [`find_homography`]. +/// +/// Mirrors `cv::HomographyMethod` from OpenCV. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(i32)] +pub enum HomographyMethod { + /// Use all points (no outlier rejection). Equivalent to passing `0` in + /// OpenCV. + None = 0, + /// Least-Median robust method (not yet distinct from RANSAC in this + /// implementation). + LMedS = 4, + /// RANSAC-based robust method. + Ransac = 8, + /// PROSAC-based robust method (currently falls back to RANSAC). + Rho = 16, +} + +/// Finds a perspective transformation between two planes. +/// +/// Given `src_points` and `dst_points`, returns the 3×3 homography matrix +/// **H** (single-channel `f64`) such that for each correspondence *i*: +/// +/// ```text +/// [x', y', w']^T ≈ H * [x, y, 1]^T (dst ≈ H * src) +/// ``` +/// +/// At least **4** matching point pairs are required. For noisy or cluttered +/// data use `HomographyMethod::Ransac` together with an appropriate +/// `ransac_reproj_threshold`. +/// +/// # Arguments +/// +/// * `src_points` – Coordinates in the source (original) image. +/// * `dst_points` – Coordinates of the matching points in the target image. +/// * `method` – Computation method (see [`HomographyMethod`]). +/// * `ransac_reproj_threshold` – Maximum allowed reprojection error (in pixels) +/// to classify a correspondence as an inlier. Only used for RANSAC / RHO. +/// * `mask` – When `Some`, the vector is resized to `src_points.len()` and +/// filled with `1` (inlier) or `0` (outlier) after estimation. +/// +/// # Errors +/// +/// Returns [`PureCvError::InvalidInput`] when fewer than 4 point pairs are +/// given or RANSAC cannot find enough inliers. +/// +/// # Divergences from OpenCV +/// +/// | OpenCV | purecv | +/// |--------|--------| +/// | Accepts `Mat` (float/double, 2-channel or Nx2) | Accepts `&[Point2f]` | +/// | Returns a `Mat` | Returns `Matrix` | +/// | Jacobian output via `mask` Mat | `mask: Option<&mut Vec>` | +pub fn find_homography( + src_points: &[Point2f], + dst_points: &[Point2f], + method: HomographyMethod, + ransac_reproj_threshold: f64, + mask: Option<&mut Vec>, +) -> Result> { + let n = src_points.len(); + if n < 4 || dst_points.len() < n { + return Err(PureCvError::InvalidInput( + "find_homography requires at least 4 matching point pairs".to_string(), + )); + } + if src_points.len() != dst_points.len() { + return Err(PureCvError::InvalidInput( + "src_points and dst_points must have the same length".to_string(), + )); + } + + match method { + HomographyMethod::None => { + let h = dlt_homography(src_points, dst_points)?; + if let Some(m) = mask { + m.clear(); + m.resize(n, 1); + } + Ok(h) + } + HomographyMethod::Ransac | HomographyMethod::Rho | HomographyMethod::LMedS => { + ransac_homography(src_points, dst_points, ransac_reproj_threshold, mask) + } + } +} + +// --------------------------------------------------------------------------- +// DLT (all points, with isotropic normalization) +// --------------------------------------------------------------------------- + +/// Estimate a homography from `n ≥ 4` point pairs using the normalized DLT. +fn dlt_homography(src: &[Point2f], dst: &[Point2f]) -> Result> { + let n = src.len(); + + // --- Isotropic normalization --- + let (src_xy, t1) = normalize_points(src); + let (dst_xy, t2) = normalize_points(dst); + + // --- Build 2n × 9 system matrix A --- + // For each pair (x, y) → (x', y'): + // row 2i: [-x, -y, -1, 0, 0, 0, x'x, x'y, x'] + // row 2i+1: [ 0, 0, 0, -x, -y, -1, y'x, y'y, y'] + let mut a = vec![0.0f64; 2 * n * 9]; + for i in 0..n { + let x = src_xy[2 * i]; + let y = src_xy[2 * i + 1]; + let xp = dst_xy[2 * i]; + let yp = dst_xy[2 * i + 1]; + + let base = 2 * i * 9; + a[base] = -x; + a[base + 1] = -y; + a[base + 2] = -1.0; + // a[base + 3..5] already zero + a[base + 6] = xp * x; + a[base + 7] = xp * y; + a[base + 8] = xp; + + let base = (2 * i + 1) * 9; + // a[base..base+3] already zero + a[base + 3] = -x; + a[base + 4] = -y; + a[base + 5] = -1.0; + a[base + 6] = yp * x; + a[base + 7] = yp * y; + a[base + 8] = yp; + } + + // --- Null-space → vectorized H in normalized coordinates --- + let h_vec = null_space_vector(&a, 2 * n, 9); + let h_norm: [f64; 9] = h_vec + .try_into() + .map_err(|_| PureCvError::InternalError("null_space_vector length mismatch".into()))?; + + // --- Denormalize: H = T2^{-1} * H_norm * T1 --- + let t2_inv = mat3_inv(&t2) + .ok_or_else(|| PureCvError::InternalError("Normalization matrix T2 is singular".into()))?; + let tmp = mat3_mul(&t2_inv, &h_norm); + let h_final = mat3_mul(&tmp, &t1); + + // Scale so that H[2][2] = 1 when it is non-negligible. + let scale = h_final[8]; + let h_data: Vec = if scale.abs() > 1e-12 { + h_final.iter().map(|&v| v / scale).collect() + } else { + h_final.to_vec() + }; + + Ok(Matrix::from_vec(3, 3, 1, h_data)) +} + +// --------------------------------------------------------------------------- +// RANSAC +// --------------------------------------------------------------------------- + +fn ransac_homography( + src: &[Point2f], + dst: &[Point2f], + threshold: f64, + mask: Option<&mut Vec>, +) -> Result> { + let n = src.len(); + const MAX_ITERS: usize = 2000; + const MIN_INLIERS: usize = 4; + + let mut rng = Lcg::new(0x9e37_79b9_7f4a_7c15); + let mut best_count = 0usize; + let mut best_inliers = vec![0u8; n]; + let mut best_h: Option<[f64; 9]> = None; + + for _ in 0..MAX_ITERS { + let idx = sample_no_replace(&mut rng, n, 4); + let s: Vec = idx.iter().map(|&i| src[i]).collect(); + let d: Vec = idx.iter().map(|&i| dst[i]).collect(); + + let h = match dlt_homography(&s, &d) { + Ok(h) => h, + Err(_) => continue, + }; + + let h9: [f64; 9] = h + .data + .try_into() + .map_err(|_| PureCvError::InternalError("unexpected data length".into()))?; + let (count, inliers) = count_inliers(src, dst, &h9, threshold); + + if count > best_count { + best_count = count; + best_inliers = inliers; + best_h = Some(h9); + } + + if best_count >= n * 9 / 10 { + break; + } + } + + if best_count < MIN_INLIERS || best_h.is_none() { + return Err(PureCvError::InvalidInput( + "RANSAC: not enough inliers found".to_string(), + )); + } + + // Refit from all inliers. + let in_src: Vec = src + .iter() + .zip(best_inliers.iter()) + .filter_map(|(&p, &m)| if m == 1 { Some(p) } else { None }) + .collect(); + let in_dst: Vec = dst + .iter() + .zip(best_inliers.iter()) + .filter_map(|(&p, &m)| if m == 1 { Some(p) } else { None }) + .collect(); + + let refined = dlt_homography(&in_src, &in_dst)?; + + if let Some(m) = mask { + let h9: [f64; 9] = refined + .data + .clone() + .try_into() + .map_err(|_| PureCvError::InternalError("unexpected data length".into()))?; + let (_, final_inliers) = count_inliers(src, dst, &h9, threshold); + *m = final_inliers; + } + + Ok(refined) +} + +/// Sample `k` distinct indices from `[0, n)` without replacement. +fn sample_no_replace(rng: &mut Lcg, n: usize, k: usize) -> Vec { + let mut used = vec![false; n]; + let mut out = Vec::with_capacity(k); + while out.len() < k { + let i = rng.next_usize(n); + if !used[i] { + used[i] = true; + out.push(i); + } + } + out +} + +/// Count how many point pairs satisfy the reprojection threshold under `h`. +fn count_inliers( + src: &[Point2f], + dst: &[Point2f], + h: &[f64; 9], + threshold: f64, +) -> (usize, Vec) { + let n = src.len(); + let mut inliers = vec![0u8; n]; + let mut count = 0usize; + let thr2 = threshold * threshold; + + for i in 0..n { + let x = src[i].x as f64; + let y = src[i].y as f64; + let denom = h[6] * x + h[7] * y + h[8]; + if denom.abs() < 1e-12 { + continue; + } + let px = (h[0] * x + h[1] * y + h[2]) / denom; + let py = (h[3] * x + h[4] * y + h[5]) / denom; + let ex = px - dst[i].x as f64; + let ey = py - dst[i].y as f64; + if ex * ex + ey * ey <= thr2 { + inliers[i] = 1; + count += 1; + } + } + (count, inliers) +} + +// --------------------------------------------------------------------------- +// Point normalization +// --------------------------------------------------------------------------- + +/// Isotropic normalization: translate centroid to origin; scale so the mean +/// distance from the origin equals √2. +/// +/// Returns `(flat_xy, T)` where `flat_xy` has shape `[x0, y0, x1, y1, …]` +/// and `T` is the 3×3 row-major normalization matrix such that the +/// normalized point `p̃ = T * [x, y, 1]^T`. +fn normalize_points(pts: &[Point2f]) -> (Vec, [f64; 9]) { + let n = pts.len() as f64; + let cx: f64 = pts.iter().map(|p| p.x as f64).sum::() / n; + let cy: f64 = pts.iter().map(|p| p.y as f64).sum::() / n; + + let mean_dist: f64 = pts + .iter() + .map(|p| { + let dx = p.x as f64 - cx; + let dy = p.y as f64 - cy; + (dx * dx + dy * dy).sqrt() + }) + .sum::() + / n; + + let s = if mean_dist < 1e-12 { + 1.0 + } else { + std::f64::consts::SQRT_2 / mean_dist + }; + + let mut out = vec![0.0f64; pts.len() * 2]; + for (i, p) in pts.iter().enumerate() { + out[2 * i] = s * (p.x as f64 - cx); + out[2 * i + 1] = s * (p.y as f64 - cy); + } + + // T = [ s, 0, -s*cx ] + // [ 0, s, -s*cy ] + // [ 0, 0, 1 ] + let t = [s, 0.0, -s * cx, 0.0, s, -s * cy, 0.0, 0.0, 1.0]; + (out, t) +} diff --git a/src/calib3d/linalg.rs b/src/calib3d/linalg.rs new file mode 100644 index 0000000..f738e43 --- /dev/null +++ b/src/calib3d/linalg.rs @@ -0,0 +1,335 @@ +/* + * linalg.rs + * purecv + * + * This file is part of purecv - WebARKit. + * + * purecv is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * purecv is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with purecv. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2026 WebARKit. + * + * Author(s): Walter Perdan @kalwalt https://github.com/kalwalt + * + */ + +//! Internal linear algebra helpers for the `calib3d` module. +//! +//! All functions operate on flat, row-major `f64` slices/arrays. + +// --------------------------------------------------------------------------- +// Matrix utilities +// --------------------------------------------------------------------------- + +/// Compute A^T * A for a (`rows` × `cols`) matrix `a`, returning a symmetric +/// (`cols` × `cols`) matrix stored row-major. +pub(super) fn mat_ata(a: &[f64], rows: usize, cols: usize) -> Vec { + let mut ata = vec![0.0f64; cols * cols]; + for i in 0..cols { + for j in i..cols { + let mut s = 0.0; + for k in 0..rows { + s += a[k * cols + i] * a[k * cols + j]; + } + ata[i * cols + j] = s; + ata[j * cols + i] = s; + } + } + ata +} + +/// Multiply 3×3 matrices `a` and `b` (row-major), returning their product. +pub(super) fn mat3_mul(a: &[f64; 9], b: &[f64; 9]) -> [f64; 9] { + let mut c = [0.0f64; 9]; + for i in 0..3 { + for j in 0..3 { + let mut s = 0.0; + for k in 0..3 { + s += a[i * 3 + k] * b[k * 3 + j]; + } + c[i * 3 + j] = s; + } + } + c +} + +/// Determinant of a 3×3 row-major matrix. +pub(super) fn mat3_det(m: &[f64; 9]) -> f64 { + m[0] * (m[4] * m[8] - m[5] * m[7]) - m[1] * (m[3] * m[8] - m[5] * m[6]) + + m[2] * (m[3] * m[7] - m[4] * m[6]) +} + +/// Inverse of a 3×3 row-major matrix. Returns `None` if the matrix is singular. +pub(super) fn mat3_inv(m: &[f64; 9]) -> Option<[f64; 9]> { + let det = mat3_det(m); + if det.abs() < 1e-12 { + return None; + } + let d = 1.0 / det; + Some([ + d * (m[4] * m[8] - m[5] * m[7]), + d * (m[2] * m[7] - m[1] * m[8]), + d * (m[1] * m[5] - m[2] * m[4]), + d * (m[5] * m[6] - m[3] * m[8]), + d * (m[0] * m[8] - m[2] * m[6]), + d * (m[2] * m[3] - m[0] * m[5]), + d * (m[3] * m[7] - m[4] * m[6]), + d * (m[1] * m[6] - m[0] * m[7]), + d * (m[0] * m[4] - m[1] * m[3]), + ]) +} + +// --------------------------------------------------------------------------- +// Jacobi symmetric eigenvalue decomposition +// --------------------------------------------------------------------------- + +/// Jacobi eigenvalue decomposition for a real symmetric `n × n` matrix. +/// +/// On entry `a` is the symmetric matrix (row-major flat). On return: +/// - The diagonal of `a` holds the eigenvalues. +/// - `v` (initialized to identity before the call) accumulates the +/// orthogonal matrix whose *columns* are the corresponding eigenvectors. +/// +/// The algorithm performs cyclic sweeps (all off-diagonal (p,q) pairs) +/// until convergence or `MAX_SWEEPS` is reached. +pub(super) fn jacobi_eigen(a: &mut [f64], n: usize, v: &mut [f64]) { + // Initialize v as the identity matrix. + for i in 0..n { + for j in 0..n { + v[i * n + j] = if i == j { 1.0 } else { 0.0 }; + } + } + + const MAX_SWEEPS: usize = 100; + for _ in 0..MAX_SWEEPS { + // Convergence check: total off-diagonal norm. + let mut off = 0.0f64; + for i in 0..n { + for j in (i + 1)..n { + off += a[i * n + j].abs(); + } + } + if off < 1e-15 * (n as f64) { + break; + } + + // One sweep over all (p, q) pairs with p < q. + for p in 0..n { + for q in (p + 1)..n { + let apq = a[p * n + q]; + if apq.abs() < 1e-15 { + continue; + } + let app = a[p * n + p]; + let aqq = a[q * n + q]; + + // Compute Jacobi rotation parameters (Numerical Recipes convention). + let theta = 0.5 * (aqq - app) / apq; + let t = if theta >= 0.0 { + 1.0 / (theta + (1.0 + theta * theta).sqrt()) + } else { + 1.0 / (theta - (1.0 + theta * theta).sqrt()) + }; + let c = 1.0 / (1.0 + t * t).sqrt(); + let s = t * c; + + // Update diagonal elements. + a[p * n + p] = app - t * apq; + a[q * n + q] = aqq + t * apq; + a[p * n + q] = 0.0; + a[q * n + p] = 0.0; + + // Update off-diagonal elements for rows/columns r ≠ p, q. + for r in 0..n { + if r == p || r == q { + continue; + } + let arp = a[r * n + p]; + let arq = a[r * n + q]; + let new_arp = c * arp - s * arq; + let new_arq = s * arp + c * arq; + a[r * n + p] = new_arp; + a[p * n + r] = new_arp; + a[r * n + q] = new_arq; + a[q * n + r] = new_arq; + } + + // Accumulate the rotation into v (eigenvectors as columns). + for r in 0..n { + let vrp = v[r * n + p]; + let vrq = v[r * n + q]; + v[r * n + p] = c * vrp - s * vrq; + v[r * n + q] = s * vrp + c * vrq; + } + } + } + } +} + +// --------------------------------------------------------------------------- +// Null-space vector +// --------------------------------------------------------------------------- + +/// Find the right singular vector of `a` (`rows` × `cols`) corresponding to +/// the *smallest* singular value, i.e. the eigenvector of `A^T A` with the +/// smallest eigenvalue. +pub(super) fn null_space_vector(a: &[f64], rows: usize, cols: usize) -> Vec { + let mut ata = mat_ata(a, rows, cols); + let mut v = vec![0.0f64; cols * cols]; + jacobi_eigen(&mut ata, cols, &mut v); + + // Diagonal of ata now holds eigenvalues; locate the minimum. + let min_idx = (0..cols) + .min_by(|&i, &j| { + ata[i * cols + i] + .partial_cmp(&ata[j * cols + j]) + .unwrap_or(std::cmp::Ordering::Equal) + }) + .unwrap_or(cols - 1); + + // Return column `min_idx` of `v` (the corresponding eigenvector). + (0..cols).map(|i| v[i * cols + min_idx]).collect() +} + +// --------------------------------------------------------------------------- +// 3×3 SVD and nearest rotation +// --------------------------------------------------------------------------- + +/// Singular value decomposition of a 3×3 matrix: `a = U * diag(sigma) * Vt`. +/// +/// Returns `(U, sigma, Vt)` with singular values sorted in *descending* order. +pub(super) fn svd_3x3(a: &[f64; 9]) -> ([f64; 9], [f64; 3], [f64; 9]) { + // Compute A^T A (symmetric 3×3). + let mut ata = [0.0f64; 9]; + for i in 0..3 { + for j in 0..3 { + let mut s = 0.0; + for k in 0..3 { + s += a[k * 3 + i] * a[k * 3 + j]; + } + ata[i * 3 + j] = s; + } + } + + // Eigendecomposition of ATA to get right singular vectors V. + let mut v = [0.0f64; 9]; + jacobi_eigen(&mut ata, 3, &mut v); + + // Sort by descending eigenvalue. + let mut idx = [0usize, 1, 2]; + idx.sort_by(|&i, &j| { + ata[j * 3 + j] + .partial_cmp(&ata[i * 3 + i]) + .unwrap_or(std::cmp::Ordering::Equal) + }); + + // Reorder V columns (right singular vectors). + let mut v_sorted = [0.0f64; 9]; + for (new_col, &old_col) in idx.iter().enumerate() { + for row in 0..3 { + v_sorted[row * 3 + new_col] = v[row * 3 + old_col]; + } + } + + // Singular values = sqrt(eigenvalues), clamped to zero. + let sigma = [ + ata[idx[0] * 3 + idx[0]].max(0.0).sqrt(), + ata[idx[1] * 3 + idx[1]].max(0.0).sqrt(), + ata[idx[2] * 3 + idx[2]].max(0.0).sqrt(), + ]; + + // Compute U = A * V_sorted * diag(1/sigma_i). + let mut u = [0.0f64; 9]; + for col in 0..3 { + let sv = sigma[col]; + if sv > 1e-12 { + for row in 0..3 { + let sum: f64 = (0..3).map(|k| a[row * 3 + k] * v_sorted[k * 3 + col]).sum(); + u[row * 3 + col] = sum / sv; + } + } else if col == 2 && sigma[0] > 1e-12 && sigma[1] > 1e-12 { + // Complete U to a proper orthonormal basis via cross product of columns 0 and 1. + // Column 0: u[0], u[3], u[6]; column 1: u[1], u[4], u[7]; result → column 2. + u[2] = u[3] * u[7] - u[6] * u[4]; + u[5] = u[6] * u[1] - u[0] * u[7]; + u[8] = u[0] * u[4] - u[3] * u[1]; + } + } + + // Vt = V_sorted^T. + let mut vt = [0.0f64; 9]; + for i in 0..3 { + for j in 0..3 { + vt[i * 3 + j] = v_sorted[j * 3 + i]; + } + } + + (u, sigma, vt) +} + +/// Project the 3×3 matrix `m` onto the nearest rotation matrix (det = +1) +/// using SVD: `R = U * diag(1, 1, det(U * Vt)) * Vt`. +pub(super) fn nearest_rotation(m: &[f64; 9]) -> [f64; 9] { + let (u, _sigma, vt) = svd_3x3(m); + let det_u = mat3_det(&u); + let det_vt = mat3_det(&vt); + let sign = (det_u * det_vt).signum(); + + // R = U * diag(1, 1, sign) * Vt. + let mut r = [0.0f64; 9]; + for i in 0..3 { + for j in 0..3 { + let s: f64 = (0..2).map(|k| u[i * 3 + k] * vt[k * 3 + j]).sum::() + + sign * u[i * 3 + 2] * vt[2 * 3 + j]; + r[i * 3 + j] = s; + } + } + r +} + +// --------------------------------------------------------------------------- +// Simple PRNG for RANSAC sampling +// --------------------------------------------------------------------------- + +/// Minimal LCG pseudo-random number generator for RANSAC sampling. +pub(super) struct Lcg { + state: u64, +} + +impl Lcg { + /// Create a new generator from `seed` (odd values improve quality). + pub(super) fn new(seed: u64) -> Self { + Self { state: seed | 1 } + } + + /// Return a pseudo-random `usize` in `[0, n)`. + pub(super) fn next_usize(&mut self, n: usize) -> usize { + // Knuth's multiplicative LCG constants. + self.state = self + .state + .wrapping_mul(6_364_136_223_846_793_005) + .wrapping_add(1_442_695_040_888_963_407); + ((self.state >> 33) as usize) % n + } +} diff --git a/src/calib3d/pose.rs b/src/calib3d/pose.rs new file mode 100644 index 0000000..a355b49 --- /dev/null +++ b/src/calib3d/pose.rs @@ -0,0 +1,752 @@ +/* + * pose.rs + * purecv + * + * This file is part of purecv - WebARKit. + * + * purecv is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * purecv is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with purecv. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2026 WebARKit. + * + * Author(s): Walter Perdan @kalwalt https://github.com/kalwalt + * + */ + +//! Camera pose estimation — `solve_pnp` and `solve_pnp_ransac`. +//! +//! Given *N* 3-D object points and their 2-D image projections, and the +//! camera intrinsic matrix, these functions estimate the object pose +//! (rotation and translation vectors) in the camera coordinate system. + +use crate::core::error::{PureCvError, Result}; +use crate::core::types::{Point2f, Point3f}; +use crate::core::Matrix; + +use super::geometry::rvec_to_rmat; +use super::linalg::{mat3_inv, mat3_mul, nearest_rotation, null_space_vector, Lcg}; + +// --------------------------------------------------------------------------- +// Public enumerations +// --------------------------------------------------------------------------- + +/// Algorithm flag for [`solve_pnp`] / [`solve_pnp_ransac`]. +/// +/// Mirrors `cv::SolvePnPMethod`. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(i32)] +pub enum SolvePnPMethod { + /// Iterative method based on DLT initialisation followed by + /// Gauss-Newton reprojection-error minimisation. + Iterative = 0, + /// P3P (Gao et al. 2003). Requires exactly 4 point pairs. + P3P = 2, + /// Efficient PnP (Lepetit et al. 2009). + EPnP = 1, + /// AP3P (Ke & Roumeliotis 2017). + AP3P = 5, + /// SQPnP (Terzakis & Lourakis 2020). + SqPnP = 6, +} + +// --------------------------------------------------------------------------- +// Public API — solve_pnp +// --------------------------------------------------------------------------- + +/// Finds the object pose from 3-D / 2-D point correspondences. +/// +/// Solves the PnP problem: given `N ≥ 4` 3-D *object points* and their 2-D +/// *image projections*, together with the camera *intrinsic matrix*, computes +/// the rotation vector `rvec` and translation vector `tvec` that describe +/// the object's pose in the camera coordinate system. +/// +/// Follows the OpenCV `cv::solvePnP` convention: +/// ```text +/// image_point = K · [R | t] · object_point +/// ``` +/// +/// # Arguments +/// +/// * `object_points` – World-space 3-D points (at least 4). +/// * `image_points` – Corresponding 2-D image points. +/// * `camera_matrix` – 3×3 intrinsic matrix `K = [[fx,0,cx],[0,fy,cy],[0,0,1]]`. +/// * `dist_coeffs` – Distortion coefficients `[k1,k2,p1,p2[,k3…]]` or `None`. +/// * `rvec` – Output rotation vector (3×1 `f64`). +/// * `tvec` – Output translation vector (3×1 `f64`). +/// * `use_extrinsic_guess` – When `true`, the contents of `rvec`/`tvec` are +/// used as an initial estimate (only effective for `Iterative`). +/// * `flags` – Algorithm selection (see [`SolvePnPMethod`]). +/// +/// # Returns +/// +/// `true` on success. +/// +/// # Errors +/// +/// Returns [`PureCvError::InvalidInput`] when fewer than 4 correspondences are +/// given or the camera matrix is singular. +/// +/// # Divergences from OpenCV +/// +/// | OpenCV | purecv | +/// |--------|--------| +/// | Accepts `Mat` (many layouts) | Accepts `&[Point3f]` / `&[Point2f]` | +/// | Supports distortion undistortion | Only pin-hole (no distortion correction) | +/// | Returns `bool` | Returns `Result` | +#[allow(clippy::too_many_arguments)] +pub fn solve_pnp( + object_points: &[Point3f], + image_points: &[Point2f], + camera_matrix: &Matrix, + dist_coeffs: Option<&[f64]>, + rvec: &mut Matrix, + tvec: &mut Matrix, + use_extrinsic_guess: bool, + _flags: SolvePnPMethod, +) -> Result { + let n = object_points.len(); + if n < 4 || image_points.len() < n { + return Err(PureCvError::InvalidInput( + "solve_pnp requires at least 4 point correspondences".to_string(), + )); + } + if object_points.len() != image_points.len() { + return Err(PureCvError::InvalidInput( + "object_points and image_points must have the same length".to_string(), + )); + } + if camera_matrix.rows != 3 || camera_matrix.cols != 3 || camera_matrix.channels != 1 { + return Err(PureCvError::InvalidInput( + "camera_matrix must be a 3×3 single-channel matrix".to_string(), + )); + } + + let _ = dist_coeffs; // Distortion correction not yet implemented. + + // Extract K entries. + let k = extract_k(camera_matrix)?; + + // Undistort image points (only pin-hole in this release). + let norm_pts = undistort_points(image_points, &k); + + // DLT initial estimate. + let (r_init, t_init) = dlt_pnp(&norm_pts, object_points)?; + + // Gauss-Newton refinement. + let (r_ref, t_ref) = if use_extrinsic_guess + && rvec.rows == 3 + && rvec.cols == 1 + && tvec.rows == 3 + && tvec.cols == 1 + { + let rv = [rvec.data[0], rvec.data[1], rvec.data[2]]; + let r_guess = rvec_to_rmat(rv[0], rv[1], rv[2]); + let t_guess = [tvec.data[0], tvec.data[1], tvec.data[2]]; + gauss_newton_refine(&norm_pts, object_points, &r_guess, &t_guess) + } else { + gauss_newton_refine(&norm_pts, object_points, &r_init, &t_init) + }; + + // Convert rotation matrix → rotation vector. + write_output(r_ref, t_ref, rvec, tvec); + Ok(true) +} + +// --------------------------------------------------------------------------- +// Public API — solve_pnp_ransac +// --------------------------------------------------------------------------- + +/// Finds the object pose from 3-D / 2-D point correspondences, with RANSAC +/// for robustness against outliers. +/// +/// This is the robust counterpart of [`solve_pnp`], applying RANSAC to +/// identify inliers before running a final least-squares fit. +/// +/// # Arguments +/// +/// * `object_points` – World-space 3-D points. +/// * `image_points` – Corresponding 2-D image points. +/// * `camera_matrix` – 3×3 intrinsic matrix. +/// * `dist_coeffs` – Distortion coefficients or `None`. +/// * `rvec` – Output rotation vector (3×1 `f64`). +/// * `tvec` – Output translation vector (3×1 `f64`). +/// * `use_extrinsic_guess` – Use `rvec`/`tvec` as initial estimate. +/// * `iterations_count` – Number of RANSAC iterations (default: 100). +/// * `reproj_threshold` – Maximum reprojection error for an inlier (pixels). +/// * `confidence` – Desired solution confidence (currently unused; +/// iteration count is fixed). +/// * `inliers` – When `Some`, filled with the indices of inlier +/// correspondences. +/// * `flags` – Algorithm selection (see [`SolvePnPMethod`]). +/// +/// # Returns +/// +/// `true` when a valid pose was found with at least 4 inliers. +/// +/// # Errors +/// +/// Returns [`PureCvError::InvalidInput`] when fewer than 4 correspondences are +/// given or the camera matrix is invalid. +#[allow(clippy::too_many_arguments)] +pub fn solve_pnp_ransac( + object_points: &[Point3f], + image_points: &[Point2f], + camera_matrix: &Matrix, + dist_coeffs: Option<&[f64]>, + rvec: &mut Matrix, + tvec: &mut Matrix, + _use_extrinsic_guess: bool, + iterations_count: i32, + reproj_threshold: f32, + confidence: f64, + inliers: Option<&mut Vec>, + _flags: SolvePnPMethod, +) -> Result { + let n = object_points.len(); + if n < 4 || image_points.len() < n { + return Err(PureCvError::InvalidInput( + "solve_pnp_ransac requires at least 4 point correspondences".to_string(), + )); + } + if object_points.len() != image_points.len() { + return Err(PureCvError::InvalidInput( + "object_points and image_points must have the same length".to_string(), + )); + } + + let _ = confidence; // Adaptive iteration count not yet implemented. + let _ = dist_coeffs; // Distortion correction not yet implemented. + + let max_iters = iterations_count.max(1) as usize; + let thr = reproj_threshold as f64; + let thr2 = thr * thr; + + let k = extract_k(camera_matrix)?; + let norm_pts = undistort_points(image_points, &k); + + let mut rng = Lcg::new(0x1234_5678_9abc_def0); + let mut best_count = 0usize; + let mut best_inlier_mask = vec![false; n]; + let mut best_r = [1.0f64, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]; + let mut best_t = [0.0f64; 3]; + let mut found = false; + + for _ in 0..max_iters { + let idx = sample_no_replace(&mut rng, n, 4); + let s_obj: Vec = idx.iter().map(|&i| object_points[i]).collect(); + let s_img: Vec = idx.iter().map(|&i| norm_pts[i]).collect(); + + let (r, t) = match dlt_pnp(&s_img, &s_obj) { + Ok(rt) => rt, + Err(_) => continue, + }; + let (r, t) = gauss_newton_refine(&s_img, &s_obj, &r, &t); + + // Count inliers using reprojection error in *normalized* coordinates. + let (count, mask) = count_pnp_inliers(&norm_pts, object_points, &r, &t, thr2); + + if count > best_count { + best_count = count; + best_inlier_mask = mask; + best_r = r; + best_t = t; + found = true; + } + + if best_count >= n * 9 / 10 { + break; + } + } + + if !found || best_count < 4 { + return Ok(false); + } + + // Refit from all inliers. + let in_obj: Vec = object_points + .iter() + .zip(best_inlier_mask.iter()) + .filter_map(|(&p, &ok)| if ok { Some(p) } else { None }) + .collect(); + let in_img: Vec = norm_pts + .iter() + .zip(best_inlier_mask.iter()) + .filter_map(|(&p, &ok)| if ok { Some(p) } else { None }) + .collect(); + + let (r_final, t_final) = match dlt_pnp(&in_img, &in_obj) { + Ok(rt) => gauss_newton_refine(&in_img, &in_obj, &rt.0, &rt.1), + Err(_) => (best_r, best_t), + }; + + write_output(r_final, t_final, rvec, tvec); + + if let Some(out_inliers) = inliers { + out_inliers.clear(); + for (i, &ok) in best_inlier_mask.iter().enumerate() { + if ok { + out_inliers.push(i as i32); + } + } + } + + Ok(true) +} + +// --------------------------------------------------------------------------- +// DLT initialisation +// --------------------------------------------------------------------------- + +/// DLT solution for the PnP problem using normalised image coordinates. +/// +/// For each correspondence `(X, Y, Z) → (u, v)` in normalised coordinates +/// (after K^{-1} multiplication), the linear constraints are: +/// ```text +/// [ X Y Z 1 0 0 0 0 -u*X -u*Y -u*Z -u ] * p = 0 +/// [ 0 0 0 0 X Y Z 1 -v*X -v*Y -v*Z -v ] * p = 0 +/// ``` +/// The 12-vector `p` vectorises the 3×4 projection matrix `[R|t]`. +fn dlt_pnp(norm_pts: &[Point2f], obj_pts: &[Point3f]) -> Result<([f64; 9], [f64; 3])> { + let n = norm_pts.len(); + // Build 2n × 12 matrix A. + let mut a = vec![0.0f64; 2 * n * 12]; + for i in 0..n { + let xx = obj_pts[i].x as f64; + let yy = obj_pts[i].y as f64; + let zz = obj_pts[i].z as f64; + let u = norm_pts[i].x as f64; + let v = norm_pts[i].y as f64; + + let base = 2 * i * 12; + a[base] = xx; + a[base + 1] = yy; + a[base + 2] = zz; + a[base + 3] = 1.0; + // a[base+4..7] = 0 + a[base + 8] = -u * xx; + a[base + 9] = -u * yy; + a[base + 10] = -u * zz; + a[base + 11] = -u; + + let base = (2 * i + 1) * 12; + // a[base..3] = 0 + a[base + 4] = xx; + a[base + 5] = yy; + a[base + 6] = zz; + a[base + 7] = 1.0; + a[base + 8] = -v * xx; + a[base + 9] = -v * yy; + a[base + 10] = -v * zz; + a[base + 11] = -v; + } + + let p = null_space_vector(&a, 2 * n, 12); + + // Reshape to 3×4 projection matrix. + let scale = (p[0] * p[0] + p[1] * p[1] + p[2] * p[2]).sqrt(); + if scale < 1e-12 { + return Err(PureCvError::InternalError( + "DLT: degenerate null-space".into(), + )); + } + + // Extract rotation (rows 0..2 of P) and translation (column 3). + let r_raw: [f64; 9] = [ + p[0] / scale, + p[1] / scale, + p[2] / scale, + p[4] / scale, + p[5] / scale, + p[6] / scale, + p[8] / scale, + p[9] / scale, + p[10] / scale, + ]; + let t_raw = [p[3] / scale, p[7] / scale, p[11] / scale]; + + // Project onto nearest rotation matrix. + let r = nearest_rotation(&r_raw); + + // Ensure points are in front of the camera (positive z in camera frame). + let sign = if reproject_sign(&r, &t_raw, obj_pts) >= 0 { + 1.0 + } else { + -1.0 + }; + + let r = if sign < 0.0 { + let mut rn = r; + rn.iter_mut().for_each(|v| *v *= -1.0); + nearest_rotation(&rn) + } else { + r + }; + let t = [sign * t_raw[0], sign * t_raw[1], sign * t_raw[2]]; + + Ok((r, t)) +} + +/// Count how many points project in front of the camera to resolve sign ambiguity. +fn reproject_sign(r: &[f64; 9], t: &[f64; 3], pts: &[Point3f]) -> i32 { + let mut pos = 0i32; + for p in pts { + let z = r[6] * p.x as f64 + r[7] * p.y as f64 + r[8] * p.z as f64 + t[2]; + if z > 0.0 { + pos += 1; + } else { + pos -= 1; + } + } + pos +} + +// --------------------------------------------------------------------------- +// Gauss-Newton refinement +// --------------------------------------------------------------------------- + +/// Refine pose `(R, t)` by minimising the sum of squared reprojection errors +/// in normalised image coordinates using the Gauss-Newton method. +fn gauss_newton_refine( + norm_pts: &[Point2f], + obj_pts: &[Point3f], + r_init: &[f64; 9], + t_init: &[f64; 3], +) -> ([f64; 9], [f64; 3]) { + // We parameterise the rotation as a Rodriguez vector. + let mut rv = rmat_to_rvec_approx(r_init); + let mut t = *t_init; + + const MAX_ITER: usize = 20; + const EPS: f64 = 1e-8; + + for _ in 0..MAX_ITER { + let r = rvec_to_rmat(rv[0], rv[1], rv[2]); + let (jtj, jtb, err2) = build_jtj(&r, &rv, &t, norm_pts, obj_pts); + + if err2 < EPS * EPS { + break; + } + + // Solve the 6×6 normal equations ∂(J^T J) δ = J^T b. + let delta = solve_6x6(&jtj, &jtb); + if delta.iter().map(|v| v * v).sum::().sqrt() < EPS { + break; + } + + rv[0] += delta[0]; + rv[1] += delta[1]; + rv[2] += delta[2]; + t[0] += delta[3]; + t[1] += delta[4]; + t[2] += delta[5]; + } + + let r_final = rvec_to_rmat(rv[0], rv[1], rv[2]); + (r_final, t) +} + +/// Build J^T J and J^T r for the reprojection-error Gauss-Newton system +/// (6-parameter: 3 Rodrigues + 3 translation). +fn build_jtj( + r: &[f64; 9], + rv: &[f64; 3], + t: &[f64; 3], + norm_pts: &[Point2f], + obj_pts: &[Point3f], +) -> ([f64; 36], [f64; 6], f64) { + let mut jtj = [0.0f64; 36]; + let mut jtb = [0.0f64; 6]; + let mut total_err2 = 0.0f64; + let theta = (rv[0] * rv[0] + rv[1] * rv[1] + rv[2] * rv[2]).sqrt(); + + for i in 0..norm_pts.len() { + let xx = obj_pts[i].x as f64; + let yy = obj_pts[i].y as f64; + let zz = obj_pts[i].z as f64; + + // Camera-space point. + let cx = r[0] * xx + r[1] * yy + r[2] * zz + t[0]; + let cy = r[3] * xx + r[4] * yy + r[5] * zz + t[1]; + let cz = r[6] * xx + r[7] * yy + r[8] * zz + t[2]; + + if cz.abs() < 1e-12 { + continue; + } + + let inv_cz = 1.0 / cz; + let proj_u = cx * inv_cz; + let proj_v = cy * inv_cz; + + let eu = proj_u - norm_pts[i].x as f64; + let ev = proj_v - norm_pts[i].y as f64; + total_err2 += eu * eu + ev * ev; + + // Jacobian of (proj_u, proj_v) w.r.t. (rx, ry, rz, tx, ty, tz). + // d(proj) / d(camera_point): + let dpdu_dcx = inv_cz; + let dpdu_dcz = -cx * inv_cz * inv_cz; + let dpdv_dcy = inv_cz; + let dpdv_dcz = -cy * inv_cz * inv_cz; + + // d(camera_point) / d(rvec) via Rodrigues derivative (approximate). + // We use a finite-difference approximation for the rotation Jacobian. + let eps = if theta > 1e-4 { theta * 1e-5 } else { 1e-6 }; + let mut j = [0.0f64; 12]; // 2 residuals × 6 params + + for k in 0..3 { + let mut rv_p = *rv; + rv_p[k] += eps; + let r_p = rvec_to_rmat(rv_p[0], rv_p[1], rv_p[2]); + let cx_p = r_p[0] * xx + r_p[1] * yy + r_p[2] * zz + t[0]; + let cy_p = r_p[3] * xx + r_p[4] * yy + r_p[5] * zz + t[1]; + let cz_p = r_p[6] * xx + r_p[7] * yy + r_p[8] * zz + t[2]; + let inv_czp = if cz_p.abs() > 1e-12 { 1.0 / cz_p } else { 0.0 }; + j[k] = (cx_p * inv_czp - proj_u) / eps; + j[6 + k] = (cy_p * inv_czp - proj_v) / eps; + } + + // Translation Jacobian (exact). + j[3] = dpdu_dcx; + j[4] = 0.0; + j[5] = dpdu_dcz; + j[9] = 0.0; + j[10] = dpdv_dcy; + j[11] = dpdv_dcz; + + // Accumulate J^T J and J^T r. + for a in 0..6 { + jtb[a] += j[a] * eu + j[6 + a] * ev; + for b in a..6 { + let v = j[a] * j[b] + j[6 + a] * j[6 + b]; + jtj[a * 6 + b] += v; + if b != a { + jtj[b * 6 + a] += v; + } + } + } + } + + (jtj, jtb, total_err2) +} + +/// Solve 6×6 symmetric positive-(semi-)definite system via Cholesky decomposition. +fn solve_6x6(a: &[f64; 36], b: &[f64; 6]) -> [f64; 6] { + // Simple Gaussian elimination with partial pivoting. + const N: usize = 6; + let mut m = [0.0f64; N * N]; + let mut rhs = [0.0f64; N]; + for i in 0..N { + for j in 0..N { + m[i * N + j] = a[i * N + j]; + } + rhs[i] = -b[i]; // We want to minimise: ½‖Jδ − r‖², so δ = −(J^TJ)^{-1} J^Tr. + } + + let mut perm: [usize; N] = [0, 1, 2, 3, 4, 5]; + for col in 0..N { + // Find pivot. + let mut max_val = m[col * N + col].abs(); + let mut max_row = col; + for row in (col + 1)..N { + let v = m[row * N + col].abs(); + if v > max_val { + max_val = v; + max_row = row; + } + } + if max_val < 1e-14 { + continue; // Degenerate; skip. + } + if max_row != col { + for j in 0..N { + m.swap(col * N + j, max_row * N + j); + } + rhs.swap(col, max_row); + perm.swap(col, max_row); + } + let pivot = m[col * N + col]; + for row in (col + 1)..N { + let factor = m[row * N + col] / pivot; + for j in col..N { + let v = m[col * N + j]; + m[row * N + j] -= factor * v; + } + rhs[row] -= factor * rhs[col]; + } + } + + // Back substitution. + let mut x = [0.0f64; N]; + for i in (0..N).rev() { + let mut s = rhs[i]; + for j in (i + 1)..N { + s -= m[i * N + j] * x[j]; + } + x[i] = if m[i * N + i].abs() > 1e-14 { + s / m[i * N + i] + } else { + 0.0 + }; + } + x +} + +// --------------------------------------------------------------------------- +// RANSAC helpers +// --------------------------------------------------------------------------- + +fn count_pnp_inliers( + norm_pts: &[Point2f], + obj_pts: &[Point3f], + r: &[f64; 9], + t: &[f64; 3], + thr2: f64, +) -> (usize, Vec) { + let n = norm_pts.len(); + let mut mask = vec![false; n]; + let mut count = 0usize; + + for i in 0..n { + let xx = obj_pts[i].x as f64; + let yy = obj_pts[i].y as f64; + let zz = obj_pts[i].z as f64; + + let cx = r[0] * xx + r[1] * yy + r[2] * zz + t[0]; + let cy = r[3] * xx + r[4] * yy + r[5] * zz + t[1]; + let cz = r[6] * xx + r[7] * yy + r[8] * zz + t[2]; + + if cz.abs() < 1e-12 { + continue; + } + let inv_cz = 1.0 / cz; + let eu = cx * inv_cz - norm_pts[i].x as f64; + let ev = cy * inv_cz - norm_pts[i].y as f64; + + if eu * eu + ev * ev <= thr2 { + mask[i] = true; + count += 1; + } + } + (count, mask) +} + +fn sample_no_replace(rng: &mut Lcg, n: usize, k: usize) -> Vec { + let mut used = vec![false; n]; + let mut out = Vec::with_capacity(k); + while out.len() < k { + let i = rng.next_usize(n); + if !used[i] { + used[i] = true; + out.push(i); + } + } + out +} + +// --------------------------------------------------------------------------- +// Misc helpers +// --------------------------------------------------------------------------- + +/// Extract the 3×3 camera matrix as a flat array and its inverse. +fn extract_k(camera_matrix: &Matrix) -> Result<[f64; 9]> { + if camera_matrix.data.len() != 9 { + return Err(PureCvError::InvalidInput( + "camera_matrix must contain exactly 9 elements".into(), + )); + } + let k: [f64; 9] = camera_matrix + .data + .as_slice() + .try_into() + .map_err(|_| PureCvError::InternalError("camera_matrix layout error".into()))?; + Ok(k) +} + +/// Apply K^{-1} to image points to obtain normalised camera coordinates. +fn undistort_points(image_points: &[Point2f], k: &[f64; 9]) -> Vec { + let k_inv = mat3_inv(k); + image_points + .iter() + .map(|p| { + if let Some(ki) = k_inv { + let x = p.x as f64; + let y = p.y as f64; + let w = ki[6] * x + ki[7] * y + ki[8]; + let xn = if w.abs() > 1e-12 { + (ki[0] * x + ki[1] * y + ki[2]) / w + } else { + 0.0 + }; + let yn = if w.abs() > 1e-12 { + (ki[3] * x + ki[4] * y + ki[5]) / w + } else { + 0.0 + }; + Point2f { + x: xn as f32, + y: yn as f32, + } + } else { + *p + } + }) + .collect() +} + +/// Approximate rotation-matrix → Rodrigues vector (fast, for GN initialisation). +fn rmat_to_rvec_approx(r: &[f64; 9]) -> [f64; 3] { + // Use the same formula as geometry::rmat_to_rvec. + let trace_val = ((r[0] + r[4] + r[8] - 1.0) * 0.5).clamp(-1.0, 1.0); + let theta = trace_val.acos(); + if theta.abs() < 1e-10 { + return [0.0, 0.0, 0.0]; + } + let factor = theta / (2.0 * theta.sin()); + [ + factor * (r[7] - r[5]), + factor * (r[2] - r[6]), + factor * (r[3] - r[1]), + ] +} + +/// Write rotation + translation to output matrices. +fn write_output(r: [f64; 9], t: [f64; 3], rvec: &mut Matrix, tvec: &mut Matrix) { + // Rotation matrix → rotation vector. + let rv = rmat_to_rvec_approx(&r); + rvec.rows = 3; + rvec.cols = 1; + rvec.channels = 1; + rvec.data = rv.to_vec(); + + tvec.rows = 3; + tvec.cols = 1; + tvec.channels = 1; + tvec.data = t.to_vec(); +} + +/// Matrix-multiply two 3×3 matrices; exposed for tests. +#[allow(dead_code)] +pub(super) fn mat3_mul_pub(a: &[f64; 9], b: &[f64; 9]) -> [f64; 9] { + mat3_mul(a, b) +} diff --git a/src/calib3d/tests.rs b/src/calib3d/tests.rs index 2e42e91..b14dcfd 100644 --- a/src/calib3d/tests.rs +++ b/src/calib3d/tests.rs @@ -36,9 +36,376 @@ #[cfg(test)] mod calib3d_tests { + use crate::calib3d::{find_homography, rodrigues, solve_pnp, solve_pnp_ransac}; + use crate::calib3d::{HomographyMethod, SolvePnPMethod}; + use crate::core::types::{Point2f, Point3f}; + use crate::core::Matrix; + + // ----------------------------------------------------------------------- + // Helpers + // ----------------------------------------------------------------------- + + fn approx_eq(a: f64, b: f64, tol: f64) -> bool { + (a - b).abs() <= tol + } + + fn mat_approx_eq(a: &Matrix, b: &Matrix, tol: f64) -> bool { + a.rows == b.rows + && a.cols == b.cols + && a.data + .iter() + .zip(b.data.iter()) + .all(|(&x, &y)| approx_eq(x, y, tol)) + } + + // ----------------------------------------------------------------------- + // linalg internals + // ----------------------------------------------------------------------- + + #[test] + fn test_jacobi_eigen_2x2() { + use crate::calib3d::linalg::jacobi_eigen; + // A = [[2, 1], [1, 2]] eigenvalues: 3, 1 + let mut a = [2.0f64, 1.0, 1.0, 2.0]; + let mut v = [0.0f64; 4]; + jacobi_eigen(&mut a, 2, &mut v); + // Eigenvalues should be 1 and 3 (order may vary). + let eigs = [a[0], a[3]]; + assert!(eigs.contains(&3.0) || eigs.iter().any(|&e| approx_eq(e, 3.0, 1e-10))); + assert!(eigs.iter().any(|&e| approx_eq(e, 1.0, 1e-10))); + } + + #[test] + fn test_null_space_vector() { + use crate::calib3d::linalg::null_space_vector; + // A = [[1, 0], [0, 0]] — null space is [0, 1]. + let a = [1.0f64, 0.0, 0.0, 0.0]; + let ns = null_space_vector(&a, 2, 2); + assert!(approx_eq(ns[0].abs(), 0.0, 1e-8)); + assert!(approx_eq(ns[1].abs(), 1.0, 1e-8)); + } + + // ----------------------------------------------------------------------- + // Rodrigues + // ----------------------------------------------------------------------- + + #[test] + fn test_rodrigues_identity() { + // Zero rotation vector → identity matrix. + let src = Matrix::from_vec(3, 1, 1, vec![0.0f64, 0.0, 0.0]); + let mut dst = Matrix::::new(1, 1, 1); + rodrigues(&src, &mut dst).unwrap(); + let eye = Matrix::from_vec(3, 3, 1, vec![1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]); + assert!(mat_approx_eq(&dst, &eye, 1e-12)); + } + + #[test] + fn test_rodrigues_90_deg_x_axis() { + // Rotation of π/2 around the x-axis. + use std::f64::consts::FRAC_PI_2; + let src = Matrix::from_vec(3, 1, 1, vec![FRAC_PI_2, 0.0, 0.0]); + let mut dst = Matrix::::new(1, 1, 1); + rodrigues(&src, &mut dst).unwrap(); + assert_eq!(dst.rows, 3); + assert_eq!(dst.cols, 3); + // Expected: [[1,0,0],[0,0,-1],[0,1,0]] + assert!(approx_eq(dst.data[0], 1.0, 1e-10)); + assert!(approx_eq(dst.data[4], 0.0, 1e-10)); + assert!(approx_eq(dst.data[5], -1.0, 1e-10)); + assert!(approx_eq(dst.data[7], 1.0, 1e-10)); + } + + #[test] + fn test_rodrigues_roundtrip() { + // rvec → rmat → rvec should return the original vector. + let rv_orig = vec![0.3f64, -0.5, 0.8]; + let src = Matrix::from_vec(3, 1, 1, rv_orig.clone()); + let mut rmat = Matrix::::new(1, 1, 1); + rodrigues(&src, &mut rmat).unwrap(); + + let mut rv_back = Matrix::::new(1, 1, 1); + rodrigues(&rmat, &mut rv_back).unwrap(); + + for (a, b) in rv_orig.iter().zip(rv_back.data.iter()) { + assert!(approx_eq(*a, *b, 1e-8), "roundtrip mismatch: {a} vs {b}"); + } + } + + #[test] + fn test_rodrigues_rmat_to_rvec_output_shape() { + let rmat = Matrix::from_vec(3, 3, 1, vec![1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]); + let mut rv = Matrix::::new(1, 1, 1); + rodrigues(&rmat, &mut rv).unwrap(); + assert_eq!(rv.rows, 3); + assert_eq!(rv.cols, 1); + } + + #[test] + fn test_rodrigues_invalid_shape() { + let bad = Matrix::from_vec(2, 2, 1, vec![1.0f64, 0.0, 0.0, 1.0]); + let mut dst = Matrix::::new(1, 1, 1); + assert!(rodrigues(&bad, &mut dst).is_err()); + } + + // ----------------------------------------------------------------------- + // find_homography — minimal point set (exact) + // ----------------------------------------------------------------------- + + /// Build a set of point correspondences that obey a known homography H. + fn make_homography_pts(h: &[f64; 9]) -> (Vec, Vec) { + let src: Vec = vec![ + Point2f { x: 0.0, y: 0.0 }, + Point2f { x: 1.0, y: 0.0 }, + Point2f { x: 1.0, y: 1.0 }, + Point2f { x: 0.0, y: 1.0 }, + Point2f { x: 0.5, y: 0.5 }, + ]; + let dst: Vec = src + .iter() + .map(|p| { + let x = p.x as f64; + let y = p.y as f64; + let w = h[6] * x + h[7] * y + h[8]; + Point2f { + x: ((h[0] * x + h[1] * y + h[2]) / w) as f32, + y: ((h[3] * x + h[4] * y + h[5]) / w) as f32, + } + }) + .collect(); + (src, dst) + } + + #[test] + fn test_find_homography_identity() { + // H = identity → dst == src. + let h_known = [1.0f64, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]; + let (src, dst) = make_homography_pts(&h_known); + let h = find_homography(&src, &dst, HomographyMethod::None, 3.0, None).unwrap(); + // Should be close to identity (up to scale). + for (computed, &expected) in h.data.iter().zip(h_known.iter()) { + assert!( + approx_eq(*computed, expected, 1e-6), + "{computed} vs {expected}" + ); + } + } + + #[test] + fn test_find_homography_pure_translation() { + // H = [[1,0,10],[0,1,20],[0,0,1]] + let h_known = [1.0f64, 0.0, 10.0, 0.0, 1.0, 20.0, 0.0, 0.0, 1.0]; + let (src, dst) = make_homography_pts(&h_known); + let h = find_homography(&src, &dst, HomographyMethod::None, 3.0, None).unwrap(); + for (computed, &expected) in h.data.iter().zip(h_known.iter()) { + assert!( + approx_eq(*computed, expected, 1e-4), + "{computed} vs {expected}" + ); + } + } + + #[test] + fn test_find_homography_general() { + // A general projective homography. + let h_known = [1.2f64, 0.1, 30.0, -0.05, 0.9, 10.0, 0.001, 0.0005, 1.0]; + let (src, dst) = make_homography_pts(&h_known); + let h = find_homography(&src, &dst, HomographyMethod::None, 3.0, None).unwrap(); + for (computed, &expected) in h.data.iter().zip(h_known.iter()) { + assert!( + approx_eq(*computed, expected, 1e-4), + "{computed} vs {expected}" + ); + } + } + + #[test] + fn test_find_homography_too_few_points() { + let src = vec![Point2f { x: 0.0, y: 0.0 }, Point2f { x: 1.0, y: 0.0 }]; + let dst = src.clone(); + assert!(find_homography(&src, &dst, HomographyMethod::None, 3.0, None).is_err()); + } + + #[test] + fn test_find_homography_ransac_no_outliers() { + // With no outliers, RANSAC should produce the same result as plain DLT. + let h_known = [1.0f64, 0.0, 5.0, 0.0, 1.0, 5.0, 0.0, 0.0, 1.0]; + let (src, dst) = make_homography_pts(&h_known); + let mut mask = Vec::new(); + let h = + find_homography(&src, &dst, HomographyMethod::Ransac, 2.0, Some(&mut mask)).unwrap(); + assert_eq!(mask.len(), src.len()); + // All should be inliers. + assert!(mask.iter().all(|&m| m == 1)); + for (computed, &expected) in h.data.iter().zip(h_known.iter()) { + assert!( + approx_eq(*computed, expected, 1e-3), + "{computed} vs {expected}" + ); + } + } + + // ----------------------------------------------------------------------- + // solve_pnp + // ----------------------------------------------------------------------- + + /// Build a synthetic PnP problem with a known pose. + fn make_pnp_data(rvec: [f64; 3], tvec: [f64; 3], k: &[f64; 9]) -> (Vec, Vec) { + use crate::calib3d::geometry::rvec_to_rmat; + let r = rvec_to_rmat(rvec[0], rvec[1], rvec[2]); + let obj: Vec = vec![ + Point3f { + x: 0.0, + y: 0.0, + z: 0.0, + }, + Point3f { + x: 1.0, + y: 0.0, + z: 0.0, + }, + Point3f { + x: 1.0, + y: 1.0, + z: 0.0, + }, + Point3f { + x: 0.0, + y: 1.0, + z: 0.0, + }, + Point3f { + x: 0.5, + y: 0.5, + z: 0.5, + }, + Point3f { + x: -0.5, + y: 0.3, + z: 0.2, + }, + ]; + let img: Vec = obj + .iter() + .map(|p| { + let cx = r[0] * p.x as f64 + r[1] * p.y as f64 + r[2] * p.z as f64 + tvec[0]; + let cy = r[3] * p.x as f64 + r[4] * p.y as f64 + r[5] * p.z as f64 + tvec[1]; + let cz = r[6] * p.x as f64 + r[7] * p.y as f64 + r[8] * p.z as f64 + tvec[2]; + let xn = cx / cz; + let yn = cy / cz; + let u = k[0] * xn + k[2]; + let v = k[4] * yn + k[5]; + Point2f { + x: u as f32, + y: v as f32, + } + }) + .collect(); + (obj, img) + } + + fn make_camera_matrix() -> Matrix { + Matrix::from_vec( + 3, + 3, + 1, + vec![800.0, 0.0, 320.0, 0.0, 800.0, 240.0, 0.0, 0.0, 1.0], + ) + } + + #[test] + fn test_solve_pnp_small_rotation() { + let k = [800.0f64, 0.0, 320.0, 0.0, 800.0, 240.0, 0.0, 0.0, 1.0]; + let true_rv = [0.1f64, 0.05, 0.02]; + let true_tv = [0.0f64, 0.0, 5.0]; + let cam = make_camera_matrix(); + let (obj, img) = make_pnp_data(true_rv, true_tv, &k); + + let mut rvec = Matrix::::new(1, 1, 1); + let mut tvec = Matrix::::new(1, 1, 1); + let ok = solve_pnp( + &obj, + &img, + &cam, + None, + &mut rvec, + &mut tvec, + false, + SolvePnPMethod::Iterative, + ) + .unwrap(); + assert!(ok); + + // Translation z should be close to 5. + assert!( + approx_eq(tvec.data[2], true_tv[2], 0.5), + "tz={} expected ~5", + tvec.data[2] + ); + } + + #[test] + fn test_solve_pnp_too_few_points() { + let cam = make_camera_matrix(); + let obj = vec![ + Point3f { + x: 0.0, + y: 0.0, + z: 0.0 + }; + 3 + ]; + let img = vec![Point2f { x: 0.0, y: 0.0 }; 3]; + let mut rv = Matrix::::new(1, 1, 1); + let mut tv = Matrix::::new(1, 1, 1); + assert!(solve_pnp( + &obj, + &img, + &cam, + None, + &mut rv, + &mut tv, + false, + SolvePnPMethod::Iterative + ) + .is_err()); + } + + // ----------------------------------------------------------------------- + // solve_pnp_ransac + // ----------------------------------------------------------------------- + #[test] - fn test_calib3d_module_scaffold_present() { - // Placeholder test to keep module test structure in place. - assert!(true); + fn test_solve_pnp_ransac_clean_data() { + let k = [800.0f64, 0.0, 320.0, 0.0, 800.0, 240.0, 0.0, 0.0, 1.0]; + let true_rv = [0.0f64, 0.0, 0.1]; + let true_tv = [0.0f64, 0.0, 6.0]; + let cam = make_camera_matrix(); + let (obj, img) = make_pnp_data(true_rv, true_tv, &k); + + let mut rvec = Matrix::::new(1, 1, 1); + let mut tvec = Matrix::::new(1, 1, 1); + let mut inliers = Vec::new(); + let ok = solve_pnp_ransac( + &obj, + &img, + &cam, + None, + &mut rvec, + &mut tvec, + false, + 100, + 2.0, + 0.99, + Some(&mut inliers), + SolvePnPMethod::Iterative, + ) + .unwrap(); + assert!(ok); + assert!(!inliers.is_empty()); + assert!( + approx_eq(tvec.data[2], true_tv[2], 1.0), + "tz={} expected ~6", + tvec.data[2] + ); } } diff --git a/src/lib.rs b/src/lib.rs index d5d24cb..f496e86 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -44,10 +44,13 @@ pub mod video; /// Prelude to easily import common structures pub mod prelude { + pub use crate::calib3d::{ + find_homography, rodrigues, solve_pnp, solve_pnp_ransac, HomographyMethod, SolvePnPMethod, + }; pub use crate::core::types::{ - BorderTypes, Point2f, Point2i, Rect2f, Rect2i, Scalar, Size2f, Size2i, TermCriteria, - TermType, Vec2b, Vec2d, Vec2f, Vec2i, Vec2s, Vec3b, Vec3d, Vec3f, Vec3i, Vec3s, Vec4b, - Vec4d, Vec4f, Vec4i, Vec4s, Vec6d, Vec6f, VecN, + BorderTypes, Point2f, Point2i, Point3f, Rect2f, Rect2i, Scalar, Size2f, Size2i, + TermCriteria, TermType, Vec2b, Vec2d, Vec2f, Vec2i, Vec2s, Vec3b, Vec3d, Vec3f, Vec3i, + Vec3s, Vec4b, Vec4d, Vec4f, Vec4i, Vec4s, Vec6d, Vec6f, VecN, }; pub use crate::core::Matrix; pub use crate::imgproc::derivatives::{laplacian, scharr, sobel}; From ed28467c904db3b25304959922b394594127def7 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 7 May 2026 20:06:50 +0000 Subject: [PATCH 05/11] fix(calib3d): address code review: improve error messages and doc comments Agent-Logs-Url: https://github.com/webarkit/purecv/sessions/5fc1b86e-5d0e-46ea-a26c-d3af094c49f6 Co-authored-by: kalwalt <1275858+kalwalt@users.noreply.github.com> --- src/calib3d/geometry.rs | 5 +++-- src/calib3d/homography.rs | 17 +++++++++++------ src/calib3d/linalg.rs | 5 ++++- src/calib3d/pose.rs | 22 ++++++++++++++-------- 4 files changed, 32 insertions(+), 17 deletions(-) diff --git a/src/calib3d/geometry.rs b/src/calib3d/geometry.rs index adca937..41a8828 100644 --- a/src/calib3d/geometry.rs +++ b/src/calib3d/geometry.rs @@ -116,9 +116,10 @@ pub fn rodrigues(src: &Matrix, dst: &mut Matrix) -> Result<()> { /// /// Uses the Rodrigues formula: /// ```text -/// R = cos(θ)·I + (1 − cos(θ))·k·kᵀ + sin(θ)·[k]× +/// R = cos(θ)·I + (1 − cos(θ))·k·kᵀ + sin(θ)·K_× /// ``` -/// where `θ = ‖r‖` and `k = r / θ`. +/// where `θ = ‖r‖`, `k = r / θ`, and `K_×` is the skew-symmetric +/// (cross-product) matrix of `k`. pub(super) fn rvec_to_rmat(rx: f64, ry: f64, rz: f64) -> [f64; 9] { let theta = (rx * rx + ry * ry + rz * rz).sqrt(); diff --git a/src/calib3d/homography.rs b/src/calib3d/homography.rs index 6a037e2..3cc2aad 100644 --- a/src/calib3d/homography.rs +++ b/src/calib3d/homography.rs @@ -117,9 +117,11 @@ pub fn find_homography( )); } if src_points.len() != dst_points.len() { - return Err(PureCvError::InvalidInput( - "src_points and dst_points must have the same length".to_string(), - )); + return Err(PureCvError::InvalidInput(format!( + "src_points ({}) and dst_points ({}) must have the same length", + src_points.len(), + dst_points.len() + ))); } match method { @@ -181,9 +183,12 @@ fn dlt_homography(src: &[Point2f], dst: &[Point2f]) -> Result> { // --- Null-space → vectorized H in normalized coordinates --- let h_vec = null_space_vector(&a, 2 * n, 9); - let h_norm: [f64; 9] = h_vec - .try_into() - .map_err(|_| PureCvError::InternalError("null_space_vector length mismatch".into()))?; + let h_norm: [f64; 9] = h_vec.try_into().map_err(|v: Vec| { + PureCvError::InternalError(format!( + "null_space_vector: expected 9 elements, got {}", + v.len() + )) + })?; // --- Denormalize: H = T2^{-1} * H_norm * T1 --- let t2_inv = mat3_inv(&t2) diff --git a/src/calib3d/linalg.rs b/src/calib3d/linalg.rs index f738e43..9c9f78e 100644 --- a/src/calib3d/linalg.rs +++ b/src/calib3d/linalg.rs @@ -318,7 +318,10 @@ pub(super) struct Lcg { } impl Lcg { - /// Create a new generator from `seed` (odd values improve quality). + /// Create a new generator from `seed`. + /// + /// The LSB of `seed` is forced to 1 to guarantee the state is odd, + /// which improves the statistical quality of the LCG output. pub(super) fn new(seed: u64) -> Self { Self { state: seed | 1 } } diff --git a/src/calib3d/pose.rs b/src/calib3d/pose.rs index a355b49..4cd1337 100644 --- a/src/calib3d/pose.rs +++ b/src/calib3d/pose.rs @@ -132,9 +132,11 @@ pub fn solve_pnp( )); } if object_points.len() != image_points.len() { - return Err(PureCvError::InvalidInput( - "object_points and image_points must have the same length".to_string(), - )); + return Err(PureCvError::InvalidInput(format!( + "object_points ({}) and image_points ({}) must have the same length", + object_points.len(), + image_points.len() + ))); } if camera_matrix.rows != 3 || camera_matrix.cols != 3 || camera_matrix.channels != 1 { return Err(PureCvError::InvalidInput( @@ -230,9 +232,11 @@ pub fn solve_pnp_ransac( )); } if object_points.len() != image_points.len() { - return Err(PureCvError::InvalidInput( - "object_points and image_points must have the same length".to_string(), - )); + return Err(PureCvError::InvalidInput(format!( + "object_points ({}) and image_points ({}) must have the same length", + object_points.len(), + image_points.len() + ))); } let _ = confidence; // Adaptive iteration count not yet implemented. @@ -425,7 +429,7 @@ fn reproject_sign(r: &[f64; 9], t: &[f64; 3], pts: &[Point3f]) -> i32 { // Gauss-Newton refinement // --------------------------------------------------------------------------- -/// Refine pose `(R, t)` by minimising the sum of squared reprojection errors +/// Refine pose `(R, t)` by minimizing the sum of squared reprojection errors /// in normalised image coordinates using the Gauss-Newton method. fn gauss_newton_refine( norm_pts: &[Point2f], @@ -714,7 +718,9 @@ fn undistort_points(image_points: &[Point2f], k: &[f64; 9]) -> Vec { .collect() } -/// Approximate rotation-matrix → Rodrigues vector (fast, for GN initialisation). +/// Convert rotation matrix → Rodrigues vector for use as a Gauss-Newton +/// initialiser. Returns the same result as `geometry::rmat_to_rvec`, but +/// inlined here to avoid a cross-module call in the hot refinement loop. fn rmat_to_rvec_approx(r: &[f64; 9]) -> [f64; 3] { // Use the same formula as geometry::rmat_to_rvec. let trace_val = ((r[0] + r[4] + r[8] - 1.0) * 0.5).clamp(-1.0, 1.0); From 7063c52b60226d6013e24d5bacea92e44399641c Mon Sep 17 00:00:00 2001 From: Walter Perdan Date: Fri, 8 May 2026 13:16:56 +0200 Subject: [PATCH 06/11] feat(calib3d): add pose estimation example using solve_pnp and rodrigues (#51) --- examples/pose_estimation.rs | 183 ++++++++++++++++++++++++++++++++++++ 1 file changed, 183 insertions(+) create mode 100644 examples/pose_estimation.rs diff --git a/examples/pose_estimation.rs b/examples/pose_estimation.rs new file mode 100644 index 0000000..e3fadd6 --- /dev/null +++ b/examples/pose_estimation.rs @@ -0,0 +1,183 @@ +/* + * pose_estimation.rs + * purecv + * + * This file is part of purecv - WebARKit. + * + * purecv is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * purecv is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with purecv. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2026 WebARKit. + * + * Author(s): Walter Perdan @kalwalt https://github.com/kalwalt + * + */ + +//! Example demonstrating camera pose estimation using `solve_pnp` and `rodrigues`. + +use purecv::calib3d::geometry::rodrigues; +use purecv::calib3d::pose::{solve_pnp, SolvePnPMethod}; +use purecv::core::{ + types::{Point2f, Point3f}, + Matrix, +}; + +fn main() -> Result<(), Box> { + println!("--- PureCV Pose Estimation Example ---\n"); + + // 1. Data Setup + // Define a 10x10 planar marker centered at the origin + let object_points = [ + Point3f { + x: -5.0, + y: -5.0, + z: 0.0, + }, + Point3f { + x: 5.0, + y: -5.0, + z: 0.0, + }, + Point3f { + x: 5.0, + y: 5.0, + z: 0.0, + }, + Point3f { + x: -5.0, + y: 5.0, + z: 0.0, + }, + ]; + + // Simulate where these corners might project in the image + // (Simulating a slight angle) + let image_points = [ + Point2f { x: 260.0, y: 180.0 }, + Point2f { x: 380.0, y: 185.0 }, + Point2f { x: 390.0, y: 300.0 }, + Point2f { x: 250.0, y: 290.0 }, + ]; + + // Define Camera Matrix (640x480 resolution, fx=fy=500, cx=320, cy=240) + let camera_matrix = Matrix::::from_vec( + 3, + 3, + 1, + vec![500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0], + ); + + // Initialize rvec and tvec with a slightly off guess + let mut rvec = Matrix::::from_vec( + 3, + 1, + 1, + vec![0.1, -0.1, 0.0], // guess for rotation + ); + let mut tvec = Matrix::::from_vec( + 3, + 1, + 1, + vec![0.0, 0.0, 20.0], // guess for translation (z=20) + ); + + println!("[1] Initial Extrinsic Guess:"); + println!("rvec guess: {:?}", rvec.data); + println!("tvec guess: {:?}", tvec.data); + + // 2. Solve PnP + println!("\n[2] Solving PnP (Iterative Method)..."); + let success = solve_pnp( + &object_points, + &image_points, + &camera_matrix, + None, // No distortion coeffs + &mut rvec, + &mut tvec, + true, // use_extrinsic_guess + SolvePnPMethod::Iterative, + )?; + + if success { + println!("Success! Refined pose:"); + println!( + "rvec: [{:.4}, {:.4}, {:.4}]", + rvec.data[0], rvec.data[1], rvec.data[2] + ); + println!( + "tvec: [{:.4}, {:.4}, {:.4}]", + tvec.data[0], tvec.data[1], tvec.data[2] + ); + } else { + println!("Failed to solve PnP."); + return Ok(()); + } + + // 3. Rodrigues Conversion + println!("\n[3] Converting rvec to 3x3 Rotation Matrix..."); + let mut rmat = Matrix::::new(3, 3, 1); + rodrigues(&rvec, &mut rmat)?; + + println!("Rotation Matrix (R):"); + println!( + "[{:>7.4}, {:>7.4}, {:>7.4}]", + rmat.data[0], rmat.data[1], rmat.data[2] + ); + println!( + "[{:>7.4}, {:>7.4}, {:>7.4}]", + rmat.data[3], rmat.data[4], rmat.data[5] + ); + println!( + "[{:>7.4}, {:>7.4}, {:>7.4}]", + rmat.data[6], rmat.data[7], rmat.data[8] + ); + + // 4. World Position Calculation + // C = -R^T * t + println!("\n[4] Calculating Camera Position in World Space (C = -R^T * t)..."); + + // R^T elements (Transpose of R) + let rt00 = rmat.data[0]; + let rt01 = rmat.data[3]; + let rt02 = rmat.data[6]; + let rt10 = rmat.data[1]; + let rt11 = rmat.data[4]; + let rt12 = rmat.data[7]; + let rt20 = rmat.data[2]; + let rt21 = rmat.data[5]; + let rt22 = rmat.data[8]; + + let tx = tvec.data[0]; + let ty = tvec.data[1]; + let tz = tvec.data[2]; + + let cx = -(rt00 * tx + rt01 * ty + rt02 * tz); + let cy = -(rt10 * tx + rt11 * ty + rt12 * tz); + let cz = -(rt20 * tx + rt21 * ty + rt22 * tz); + + println!("Camera World Position (X, Y, Z):"); + println!("({:.4}, {:.4}, {:.4})", cx, cy, cz); + + Ok(()) +} From c777fc34d1483b2fb9426e6f7ef567fdd5898745 Mon Sep 17 00:00:00 2001 From: Walter Perdan Date: Fri, 8 May 2026 18:12:12 +0200 Subject: [PATCH 07/11] docs: add calib3d features and pose estimation example to README (#52) --- README.md | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 7bb2682..b915619 100644 --- a/README.md +++ b/README.md @@ -59,6 +59,12 @@ Unlike existing wrappers, **PureCV** is a native rewrite. It aims to provide: ### `purecv-video` - **Optical Flow:** Pyramidal Lucas-Kanade optical flow implementation with `calc_optical_flow_pyr_lk` and `build_optical_flow_pyramid`. Includes robust window-based tracking, sub-pixel accuracy, spatial gradient optimization, and iterative refinement. +### `purecv-calib3d` +- **Pose Estimation:** Camera pose estimation using `solve_pnp` (Iterative) and `solve_pnp_ransac`. +- **Homography:** Direct Linear Transformation (DLT) and RANSAC-based `find_homography` for robust planar perspective mapping. +- **Geometry:** `rodrigues` for converting between rotation vectors and 3x3 rotation matrices. +- **Linear Algebra Utilities:** Jacobi SVD, null-space solver, 3x3 matrix helpers, and an LCG RNG. + ## 🚀 Getting Started ### Installation @@ -209,16 +215,20 @@ cargo run --example pyramids # Hough Transform (Lines and Circles detection) cargo run --example hough_transform + +# Pose Estimation (solve_pnp and rodrigues) +cargo run --example pose_estimation ``` ## 🧪 Testing & Benchmarking ### Running Tests -PureCV uses a comprehensive suite of unit tests to ensure correctness and parity with OpenCV. The test suite currently includes **255 unit tests** covering: +PureCV uses a comprehensive suite of unit tests to ensure correctness and parity with OpenCV. The test suite currently includes **259 unit tests** covering: - **Core module:** Matrix factories, scalar arithmetic variants, bitwise scalar ops, min/max, comparison ops (`compare`, `in_range`), reduction (`reduce`, `count_non_zero`), polar/cartesian conversions, linear algebra (`determinant`, `invert`, `solve`), channel ops (`extract_channel`, `insert_channel`), `DynamicMatrix`, transforms, sorting, clustering, and RNG. - **Imgproc module:** Filters, derivatives, edge detection, color conversions (including gray-to-RGB/BGR/RGBA/BGRA), thresholding, morphology (`erode`, `dilate`), pyramids (`pyr_down`, `pyr_up`), and kernel helpers (`get_gaussian_kernel`, `get_sobel_kernels`). - **Video module:** Tracking and optical flow capabilities including `calc_optical_flow_pyr_lk` and `build_optical_flow_pyramid` implementations. +- **Calib3d module:** SVD, homography estimation, pose estimation (`solve_pnp`), and `rodrigues`. ```bash # Run all tests From 806ef68b3fdae6d4fa7bfc0a803b57c356837447 Mon Sep 17 00:00:00 2001 From: Walter Perdan Date: Fri, 8 May 2026 18:57:55 +0200 Subject: [PATCH 08/11] docs: update examples section to list all available examples --- README.md | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index b915619..98cfb67 100644 --- a/README.md +++ b/README.md @@ -195,16 +195,22 @@ Explore the capabilities of PureCV by running the provided examples: # Basic matrix arithmetic cargo run --example arithmetic +# Vector types and multi-channel operations +cargo run --example vecn_ops + # Structural operations (flip, rotate, split/merge) cargo run --example structural_ops # Color conversion (RGB to Grayscale) cargo run --example color_conversion -# Thresholding — all 5 types (BINARY, BINARY_INV, TRUNC, TOZERO, TOZERO_INV) +# Lookup Table (LUT) transformations +cargo run --example lut_example + +# Thresholding — all 5 types cargo run --example threshold -# Image filters (blur, gaussian, canny, sobel, …) — requires examples/data/butterfly.jpg +# Image filters (blur, gaussian, canny, sobel, …) cargo run --example filters # Morphological operations (erode, dilate, morph_op) @@ -216,6 +222,16 @@ cargo run --example pyramids # Hough Transform (Lines and Circles detection) cargo run --example hough_transform +# Corner Detection (Harris, Shi-Tomasi, Sub-pixel refinement) +cargo run --example corner_detection + +# Discrete Fourier Transform (DFT) +cargo run --example dft_example + +# Optical Flow (Pyramidal Lucas-Kanade) +cargo run --example optical_flow +cargo run --example optical_flow_video + # Pose Estimation (solve_pnp and rodrigues) cargo run --example pose_estimation ``` From 1af03d8821b33b6ba9dc55584cbc8e9c6df1490d Mon Sep 17 00:00:00 2001 From: Walter Perdan Date: Fri, 8 May 2026 19:02:10 +0200 Subject: [PATCH 09/11] fix(calib3d): address PR review comments for pose and homography - Add robust error handling and strict flag checks in PnP - Enforce 6-point minimum bounds for DLT PnP - Normalize reprojection thresholds by focal lengths - Improve singular matrix handling in undistort_points - Add tests covering SVD, homography, and PnP functionality Closes #51 --- examples/pose_estimation.rs | 34 +++++------ src/calib3d/homography.rs | 10 ++-- src/calib3d/pose.rs | 114 ++++++++++++++++++++++-------------- src/calib3d/tests.rs | 4 +- 4 files changed, 93 insertions(+), 69 deletions(-) diff --git a/examples/pose_estimation.rs b/examples/pose_estimation.rs index e3fadd6..f0eefaf 100644 --- a/examples/pose_estimation.rs +++ b/examples/pose_estimation.rs @@ -69,6 +69,16 @@ fn main() -> Result<(), Box> { y: 5.0, z: 0.0, }, + Point3f { + x: 0.0, + y: 0.0, + z: 0.0, + }, + Point3f { + x: 0.0, + y: 5.0, + z: 0.0, + }, ]; // Simulate where these corners might project in the image @@ -78,6 +88,8 @@ fn main() -> Result<(), Box> { Point2f { x: 380.0, y: 185.0 }, Point2f { x: 390.0, y: 300.0 }, Point2f { x: 250.0, y: 290.0 }, + Point2f { x: 320.0, y: 240.0 }, + Point2f { x: 320.0, y: 295.0 }, ]; // Define Camera Matrix (640x480 resolution, fx=fy=500, cx=320, cy=240) @@ -88,23 +100,9 @@ fn main() -> Result<(), Box> { vec![500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0], ); - // Initialize rvec and tvec with a slightly off guess - let mut rvec = Matrix::::from_vec( - 3, - 1, - 1, - vec![0.1, -0.1, 0.0], // guess for rotation - ); - let mut tvec = Matrix::::from_vec( - 3, - 1, - 1, - vec![0.0, 0.0, 20.0], // guess for translation (z=20) - ); - - println!("[1] Initial Extrinsic Guess:"); - println!("rvec guess: {:?}", rvec.data); - println!("tvec guess: {:?}", tvec.data); + // Initialize rvec and tvec + let mut rvec = Matrix::::new(3, 1, 1); + let mut tvec = Matrix::::new(3, 1, 1); // 2. Solve PnP println!("\n[2] Solving PnP (Iterative Method)..."); @@ -115,7 +113,7 @@ fn main() -> Result<(), Box> { None, // No distortion coeffs &mut rvec, &mut tvec, - true, // use_extrinsic_guess + false, // use_extrinsic_guess SolvePnPMethod::Iterative, )?; diff --git a/src/calib3d/homography.rs b/src/calib3d/homography.rs index 3cc2aad..203f89e 100644 --- a/src/calib3d/homography.rs +++ b/src/calib3d/homography.rs @@ -111,11 +111,6 @@ pub fn find_homography( mask: Option<&mut Vec>, ) -> Result> { let n = src_points.len(); - if n < 4 || dst_points.len() < n { - return Err(PureCvError::InvalidInput( - "find_homography requires at least 4 matching point pairs".to_string(), - )); - } if src_points.len() != dst_points.len() { return Err(PureCvError::InvalidInput(format!( "src_points ({}) and dst_points ({}) must have the same length", @@ -123,6 +118,11 @@ pub fn find_homography( dst_points.len() ))); } + if n < 4 { + return Err(PureCvError::InvalidInput( + "find_homography requires at least 4 matching point pairs".to_string(), + )); + } match method { HomographyMethod::None => { diff --git a/src/calib3d/pose.rs b/src/calib3d/pose.rs index 4cd1337..c3e854a 100644 --- a/src/calib3d/pose.rs +++ b/src/calib3d/pose.rs @@ -123,14 +123,9 @@ pub fn solve_pnp( rvec: &mut Matrix, tvec: &mut Matrix, use_extrinsic_guess: bool, - _flags: SolvePnPMethod, + flags: SolvePnPMethod, ) -> Result { let n = object_points.len(); - if n < 4 || image_points.len() < n { - return Err(PureCvError::InvalidInput( - "solve_pnp requires at least 4 point correspondences".to_string(), - )); - } if object_points.len() != image_points.len() { return Err(PureCvError::InvalidInput(format!( "object_points ({}) and image_points ({}) must have the same length", @@ -138,19 +133,34 @@ pub fn solve_pnp( image_points.len() ))); } + if n < 6 { + return Err(PureCvError::InvalidInput( + "solve_pnp requires at least 6 point correspondences for DLT".to_string(), + )); + } + if flags != SolvePnPMethod::Iterative { + return Err(PureCvError::NotImplemented( + "Only Iterative method is supported".to_string(), + )); + } if camera_matrix.rows != 3 || camera_matrix.cols != 3 || camera_matrix.channels != 1 { return Err(PureCvError::InvalidInput( - "camera_matrix must be a 3×3 single-channel matrix".to_string(), + "camera_matrix must be a 3x3 single-channel matrix".to_string(), )); } let _ = dist_coeffs; // Distortion correction not yet implemented. + if use_extrinsic_guess { + return Err(PureCvError::NotImplemented( + "use_extrinsic_guess is not supported yet".to_string(), + )); + } // Extract K entries. let k = extract_k(camera_matrix)?; // Undistort image points (only pin-hole in this release). - let norm_pts = undistort_points(image_points, &k); + let norm_pts = undistort_points(image_points, &k)?; // DLT initial estimate. let (r_init, t_init) = dlt_pnp(&norm_pts, object_points)?; @@ -218,19 +228,14 @@ pub fn solve_pnp_ransac( dist_coeffs: Option<&[f64]>, rvec: &mut Matrix, tvec: &mut Matrix, - _use_extrinsic_guess: bool, + use_extrinsic_guess: bool, iterations_count: i32, reproj_threshold: f32, confidence: f64, inliers: Option<&mut Vec>, - _flags: SolvePnPMethod, + flags: SolvePnPMethod, ) -> Result { let n = object_points.len(); - if n < 4 || image_points.len() < n { - return Err(PureCvError::InvalidInput( - "solve_pnp_ransac requires at least 4 point correspondences".to_string(), - )); - } if object_points.len() != image_points.len() { return Err(PureCvError::InvalidInput(format!( "object_points ({}) and image_points ({}) must have the same length", @@ -238,17 +243,40 @@ pub fn solve_pnp_ransac( image_points.len() ))); } + if n < 6 { + return Err(PureCvError::InvalidInput( + "solve_pnp_ransac requires at least 6 point correspondences".to_string(), + )); + } + if flags != SolvePnPMethod::Iterative { + return Err(PureCvError::NotImplemented( + "Only Iterative method is supported".to_string(), + )); + } + if use_extrinsic_guess { + return Err(PureCvError::NotImplemented( + "use_extrinsic_guess is not supported in ransac yet".to_string(), + )); + } let _ = confidence; // Adaptive iteration count not yet implemented. let _ = dist_coeffs; // Distortion correction not yet implemented. + let k = extract_k(camera_matrix)?; + let norm_pts = undistort_points(image_points, &k)?; + let max_iters = iterations_count.max(1) as usize; - let thr = reproj_threshold as f64; + let fx = k[0]; + let fy = k[4]; + let f_avg = (fx + fy) / 2.0; + if f_avg.abs() < 1e-12 { + return Err(PureCvError::InvalidInput( + "Invalid focal length in camera_matrix".to_string(), + )); + } + let thr = reproj_threshold as f64 / f_avg; let thr2 = thr * thr; - let k = extract_k(camera_matrix)?; - let norm_pts = undistort_points(image_points, &k); - let mut rng = Lcg::new(0x1234_5678_9abc_def0); let mut best_count = 0usize; let mut best_inlier_mask = vec![false; n]; @@ -257,7 +285,7 @@ pub fn solve_pnp_ransac( let mut found = false; for _ in 0..max_iters { - let idx = sample_no_replace(&mut rng, n, 4); + let idx = sample_no_replace(&mut rng, n, 6); let s_obj: Vec = idx.iter().map(|&i| object_points[i]).collect(); let s_img: Vec = idx.iter().map(|&i| norm_pts[i]).collect(); @@ -283,7 +311,7 @@ pub fn solve_pnp_ransac( } } - if !found || best_count < 4 { + if !found || best_count < 6 { return Ok(false); } @@ -688,34 +716,32 @@ fn extract_k(camera_matrix: &Matrix) -> Result<[f64; 9]> { } /// Apply K^{-1} to image points to obtain normalised camera coordinates. -fn undistort_points(image_points: &[Point2f], k: &[f64; 9]) -> Vec { - let k_inv = mat3_inv(k); - image_points +fn undistort_points(image_points: &[Point2f], k: &[f64; 9]) -> Result> { + let ki = mat3_inv(k) + .ok_or_else(|| PureCvError::InvalidInput("camera_matrix is singular".to_string()))?; + + Ok(image_points .iter() .map(|p| { - if let Some(ki) = k_inv { - let x = p.x as f64; - let y = p.y as f64; - let w = ki[6] * x + ki[7] * y + ki[8]; - let xn = if w.abs() > 1e-12 { - (ki[0] * x + ki[1] * y + ki[2]) / w - } else { - 0.0 - }; - let yn = if w.abs() > 1e-12 { - (ki[3] * x + ki[4] * y + ki[5]) / w - } else { - 0.0 - }; - Point2f { - x: xn as f32, - y: yn as f32, - } + let x = p.x as f64; + let y = p.y as f64; + let w = ki[6] * x + ki[7] * y + ki[8]; + let xn = if w.abs() > 1e-12 { + (ki[0] * x + ki[1] * y + ki[2]) / w + } else { + 0.0 + }; + let yn = if w.abs() > 1e-12 { + (ki[3] * x + ki[4] * y + ki[5]) / w } else { - *p + 0.0 + }; + Point2f { + x: xn as f32, + y: yn as f32, } }) - .collect() + .collect()) } /// Convert rotation matrix → Rodrigues vector for use as a Gauss-Newton diff --git a/src/calib3d/tests.rs b/src/calib3d/tests.rs index b14dcfd..ab9770b 100644 --- a/src/calib3d/tests.rs +++ b/src/calib3d/tests.rs @@ -352,9 +352,9 @@ mod calib3d_tests { y: 0.0, z: 0.0 }; - 3 + 5 ]; - let img = vec![Point2f { x: 0.0, y: 0.0 }; 3]; + let img = vec![Point2f { x: 0.0, y: 0.0 }; 5]; let mut rv = Matrix::::new(1, 1, 1); let mut tv = Matrix::::new(1, 1, 1); assert!(solve_pnp( From 686c0982e7bdd0fc0f41db1f924fac41b58d80a4 Mon Sep 17 00:00:00 2001 From: Walter Perdan Date: Sat, 9 May 2026 10:38:42 +0200 Subject: [PATCH 10/11] feat(wasm): finalize calib3d bindings and tests (closes #53) - Fix SolvePnPMethod enum mapping for WASM bindings - Provide type-safe access via DynamicMatrix enhancements - Enable and update Node.js-based WASM integration tests - Suppress too_many_arguments clippy warnings on exported WASM functions - Update crates/wasm/README.md with new calib3d coverage --- crates/wasm/README.md | 6 +- crates/wasm/src/lib.rs | 249 +++++++++++++++++++++++++++++++++++++-- crates/wasm/tests/web.rs | 122 +++++++++++++++++++ src/core/dynamic.rs | 7 ++ 4 files changed, 375 insertions(+), 9 deletions(-) create mode 100644 crates/wasm/tests/web.rs diff --git a/crates/wasm/README.md b/crates/wasm/README.md index 56705b1..498a938 100644 --- a/crates/wasm/README.md +++ b/crates/wasm/README.md @@ -53,9 +53,11 @@ Because WebAssembly runs linearly in memory and holds pointers to Rust `Vec` obj ## API coverage -Right now we have covered a large majority of operations for `core` and `imgproc`: +Right now we have covered a large majority of operations for `core` and `imgproc`, and have started on `calib3d` and `video`: - **Core**: Arithmetic (`add`, `subtract`, `multiply`, `absdiff` etc.), Structural (`hconcat`, `vconcat`, `flip`), Geometry, constants etc. -- **ImgProc**: Filters (`blur`, `gaussian_blur`, `bilateral_filter`), Thresholding (`threshold`), Coloring (`cvt_color`), Edge Derivatives (`canny`, `sobel`, `laplacian`), Morphology (`erode`, `dilate`, `morphology_ex`, `get_structuring_element`), Pyramids (`pyr_down`, `pyr_up`, `build_pyramid`). +- **ImgProc**: Filters (`blur`, `gaussian_blur`, `bilateral_filter`), Thresholding (`threshold`), Coloring (`cvt_color`), Edge Derivatives (`canny`, `sobel`, `laplacian`), Morphology (`erode`, `dilate`, `morphology_ex`, `get_structuring_element`), Pyramids (`pyr_down`, `pyr_up`, `build_pyramid`), Feature Detection (`good_features_to_track`, `corner_sub_pix`). +- **Video**: Optical Flow (`calc_optical_flow_pyr_lk`). +- **Calib3d**: Pose Estimation (`solve_pnp`, `solve_pnp_ransac`), Homography (`find_homography`), and geometry (`rodrigues`). Note: To interface between JavaScript Typed Arrays and `purecv-wasm`, please use the available getter functions (`.data()`) which directly retrieve a Float32Array or Uint8Array view into WASM memory. diff --git a/crates/wasm/src/lib.rs b/crates/wasm/src/lib.rs index c3c47ae..0d19c66 100644 --- a/crates/wasm/src/lib.rs +++ b/crates/wasm/src/lib.rs @@ -124,7 +124,28 @@ fn require_u8<'a>(mat: &'a Mat, op_name: &str) -> Result<&'a Matrix, JsError }) } -#[wasm_bindgen(js_name = "Mat")] +/// Internal helper: require f64 depth, return `&Matrix` or a JsError. +fn require_f64<'a>(mat: &'a Mat, op_name: &str) -> Result<&'a Matrix, JsError> { + mat.inner.as_matrix_f64().ok_or_else(|| { + JsError::new(&format!( + "{op_name} requires f64 depth, but Mat has depth '{}'", + mat.inner.depth_name() + )) + }) +} + +/// Internal helper: require mutable f64 depth, return `&mut Matrix` or a JsError. +fn require_f64_mut<'a>(mat: &'a mut Mat, op_name: &str) -> Result<&'a mut Matrix, JsError> { + let depth_name = mat.inner.depth_name().to_string(); + mat.inner.as_matrix_f64_mut().ok_or_else(move || { + JsError::new(&format!( + "{op_name} requires f64 depth, but Mat has depth '{}'", + depth_name + )) + }) +} + +#[wasm_bindgen] impl Mat { // -- Constructors ------------------------------------------------------- @@ -571,7 +592,7 @@ pub struct Scalar { pub v3: f64, } -#[wasm_bindgen(js_name = "Scalar")] +#[wasm_bindgen] impl Scalar { /// Creates a Scalar from four channel values. #[wasm_bindgen(constructor)] @@ -620,7 +641,7 @@ pub struct Vec2 { pub v1: f64, } -#[wasm_bindgen(js_name = "Vec2")] +#[wasm_bindgen] impl Vec2 { #[wasm_bindgen(constructor)] pub fn new(v0: f64, v1: f64) -> Vec2 { @@ -635,7 +656,7 @@ pub struct Vec3 { pub v2: f64, } -#[wasm_bindgen(js_name = "Vec3")] +#[wasm_bindgen] impl Vec3 { #[wasm_bindgen(constructor)] pub fn new(v0: f64, v1: f64, v2: f64) -> Vec3 { @@ -651,7 +672,7 @@ pub struct Vec4 { pub v3: f64, } -#[wasm_bindgen(js_name = "Vec4")] +#[wasm_bindgen] impl Vec4 { #[wasm_bindgen(constructor)] pub fn new(v0: f64, v1: f64, v2: f64, v3: f64) -> Vec4 { @@ -881,7 +902,7 @@ fn color_code_from_i32(code: i32) -> Result { pub fn convert_color(src: &Mat, code: i32) -> Result { let m = require_u8(src, "cvtColor")?; let cc = color_code_from_i32(code)?; - let result = cvt_color(m, cc).map_err(|e| JsError::new(e))?; + let result = cvt_color(m, cc).map_err(JsError::new)?; Ok(Mat { inner: DynamicMatrix { data: DynamicData::U8(result), @@ -1681,6 +1702,7 @@ pub fn wasm_good_features_to_track( /// * `max_count` – Maximum number of iterations. /// * `epsilon` – Convergence threshold. #[wasm_bindgen(js_name = "cornerSubPix")] +#[allow(clippy::too_many_arguments)] pub fn wasm_corner_sub_pix( src: &Mat, corners: &[f32], @@ -1694,7 +1716,7 @@ pub fn wasm_corner_sub_pix( use purecv::core::types::{Point2f, Size2i, TermCriteria, TermType}; let m = require_f32(src, "cornerSubPix")?; - if corners.len() % 2 != 0 { + if !corners.len().is_multiple_of(2) { return Err(JsError::new( "corners array must have even length (x, y pairs)", )); @@ -1979,3 +2001,216 @@ pub fn optflow_use_initial_flow() -> i32 { pub fn optflow_lk_get_min_eigenvals() -> i32 { optical_flow::OPTFLOW_LK_GET_MIN_EIGENVALS } + +// --------------------------------------------------------------------------- +// Calib3d (Pose Estimation & Homography) +// --------------------------------------------------------------------------- + +use purecv::calib3d::geometry::rodrigues; +use purecv::calib3d::homography::{find_homography, HomographyMethod}; +use purecv::calib3d::pose::{solve_pnp, solve_pnp_ransac, SolvePnPMethod}; + +#[wasm_bindgen] +pub struct Point2fVector { + pub(crate) inner: Vec, +} + +#[wasm_bindgen] +impl Point2fVector { + #[wasm_bindgen(constructor)] + pub fn new() -> Self { + Self { inner: Vec::new() } + } + + pub fn push(&mut self, x: f32, y: f32) { + self.inner.push(purecv::core::types::Point2f::new(x, y)); + } + + pub fn size(&self) -> usize { + self.inner.len() + } + + pub fn clear(&mut self) { + self.inner.clear(); + } +} + +impl Default for Point2fVector { + fn default() -> Self { + Self::new() + } +} + +#[wasm_bindgen] +pub struct Point3fVector { + pub(crate) inner: Vec, +} + +#[wasm_bindgen] +impl Point3fVector { + #[wasm_bindgen(constructor)] + pub fn new() -> Self { + Self { inner: Vec::new() } + } + + pub fn push(&mut self, x: f32, y: f32, z: f32) { + self.inner.push(purecv::core::types::Point3f::new(x, y, z)); + } + + pub fn size(&self) -> usize { + self.inner.len() + } + + pub fn clear(&mut self) { + self.inner.clear(); + } +} + +impl Default for Point3fVector { + fn default() -> Self { + Self::new() + } +} + +fn map_solve_pnp_method(val: i32) -> SolvePnPMethod { + match val { + 1 => SolvePnPMethod::EPnP, + 2 => SolvePnPMethod::P3P, + 5 => SolvePnPMethod::AP3P, + 6 => SolvePnPMethod::SqPnP, + _ => SolvePnPMethod::Iterative, + } +} + +fn map_homography_method(val: i32) -> HomographyMethod { + match val { + 4 => HomographyMethod::LMedS, + 8 => HomographyMethod::Ransac, + 16 => HomographyMethod::Rho, + _ => HomographyMethod::None, + } +} + +#[wasm_bindgen(js_name = "solvePnP")] +#[allow(clippy::too_many_arguments)] +pub fn solve_pnp_wasm( + object_points: &Point3fVector, + image_points: &Point2fVector, + camera_matrix: &Mat, + dist_coeffs: Option>, + rvec: &mut Mat, + tvec: &mut Mat, + use_extrinsic_guess: bool, + flags: i32, +) -> Result { + let cam_mat = require_f64(camera_matrix, "solvePnP")?; + let r_mat = require_f64_mut(rvec, "solvePnP (rvec)")?; + let t_mat = require_f64_mut(tvec, "solvePnP (tvec)")?; + + let dist_ref = dist_coeffs.as_deref(); + + solve_pnp( + &object_points.inner, + &image_points.inner, + cam_mat, + dist_ref, + r_mat, + t_mat, + use_extrinsic_guess, + map_solve_pnp_method(flags), + ) + .map_err(|e| JsError::new(&format!("{e}"))) +} + +#[wasm_bindgen(js_name = "solvePnPRansac")] +#[allow(clippy::too_many_arguments)] +pub fn solve_pnp_ransac_wasm( + object_points: &Point3fVector, + image_points: &Point2fVector, + camera_matrix: &Mat, + dist_coeffs: Option>, + rvec: &mut Mat, + tvec: &mut Mat, + use_extrinsic_guess: bool, + iterations_count: i32, + reproj_threshold: f32, + confidence: f64, + flags: i32, +) -> Result { + let cam_mat = require_f64(camera_matrix, "solvePnPRansac")?; + let r_mat = require_f64_mut(rvec, "solvePnPRansac (rvec)")?; + let t_mat = require_f64_mut(tvec, "solvePnPRansac (tvec)")?; + + let dist_ref = dist_coeffs.as_deref(); + let mut inliers_vec = Vec::new(); + + let success = solve_pnp_ransac( + &object_points.inner, + &image_points.inner, + cam_mat, + dist_ref, + r_mat, + t_mat, + use_extrinsic_guess, + iterations_count, + reproj_threshold, + confidence, + Some(&mut inliers_vec), + map_solve_pnp_method(flags), + ) + .map_err(|e| JsError::new(&format!("{e}")))?; + + let obj = js_sys::Object::new(); + js_sys::Reflect::set( + &obj, + &JsValue::from_str("success"), + &JsValue::from_bool(success), + ) + .map_err(|_| JsError::new("Failed to set success"))?; + + let js_inliers = js_sys::Int32Array::from(inliers_vec.as_slice()); + js_sys::Reflect::set(&obj, &JsValue::from_str("inliers"), &js_inliers) + .map_err(|_| JsError::new("Failed to set inliers"))?; + + Ok(obj.into()) +} + +#[wasm_bindgen(js_name = "findHomography")] +pub fn find_homography_wasm( + src_points: &Point2fVector, + dst_points: &Point2fVector, + method: i32, + ransac_reproj_threshold: f64, +) -> Result { + let mut mask_vec = Vec::new(); + let h_mat = find_homography( + &src_points.inner, + &dst_points.inner, + map_homography_method(method), + ransac_reproj_threshold, + Some(&mut mask_vec), + ) + .map_err(|e| JsError::new(&format!("{e}")))?; + + let obj = js_sys::Object::new(); + let js_h = Mat { + inner: purecv::core::dynamic::DynamicMatrix { + data: purecv::core::dynamic::DynamicData::F64(h_mat), + }, + }; + js_sys::Reflect::set(&obj, &JsValue::from_str("homography"), &JsValue::from(js_h)) + .map_err(|_| JsError::new("Failed to set homography"))?; + + let js_mask = js_sys::Uint8Array::from(mask_vec.as_slice()); + js_sys::Reflect::set(&obj, &JsValue::from_str("mask"), &js_mask) + .map_err(|_| JsError::new("Failed to set mask"))?; + + Ok(obj.into()) +} + +#[wasm_bindgen(js_name = "rodrigues")] +pub fn rodrigues_wasm(src: &Mat, dst: &mut Mat) -> Result<(), JsError> { + let src_mat = require_f64(src, "rodrigues (src)")?; + let dst_mat = require_f64_mut(dst, "rodrigues (dst)")?; + rodrigues(src_mat, dst_mat).map_err(|e| JsError::new(&format!("{e}"))) +} diff --git a/crates/wasm/tests/web.rs b/crates/wasm/tests/web.rs new file mode 100644 index 0000000..0c47cd8 --- /dev/null +++ b/crates/wasm/tests/web.rs @@ -0,0 +1,122 @@ +#![cfg(target_arch = "wasm32")] + +use purecv_wasm::{ + find_homography_wasm, rodrigues_wasm, solve_pnp_wasm, Mat, Point2fVector, Point3fVector, +}; +use wasm_bindgen_test::*; + +#[wasm_bindgen_test] +fn test_rodrigues() { + let src_data = vec![0.0, 0.0, 0.0]; + let mut src = Mat::from_f64_data(3, 1, 1, &src_data).unwrap(); + + let dst_data = vec![0.0; 9]; + let mut dst = Mat::from_f64_data(3, 3, 1, &dst_data).unwrap(); + + rodrigues_wasm(&src, &mut dst).unwrap(); + + let dst_array = dst.data_f64().unwrap(); + assert_eq!(dst_array[0], 1.0); + assert_eq!(dst_array[4], 1.0); + assert_eq!(dst_array[8], 1.0); +} + +fn rvec_to_rmat(rx: f64, ry: f64, rz: f64) -> [f64; 9] { + let theta = (rx * rx + ry * ry + rz * rz).sqrt(); + if theta < 1e-8 { + return [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]; + } + let c = theta.cos(); + let s = theta.sin(); + let c1 = 1.0 - c; + let itheta = 1.0 / theta; + let x = rx * itheta; + let y = ry * itheta; + let z = rz * itheta; + [ + c + x * x * c1, + x * y * c1 - z * s, + x * z * c1 + y * s, + y * x * c1 + z * s, + c + y * y * c1, + y * z * c1 - x * s, + z * x * c1 - y * s, + z * y * c1 + x * s, + c + z * z * c1, + ] +} + +#[wasm_bindgen_test] +fn test_solve_pnp() { + let rvec_true = [0.1, 0.05, 0.02]; + let tvec_true = [0.0, 0.0, 5.0]; + let k = [800.0, 0.0, 320.0, 0.0, 800.0, 240.0, 0.0, 0.0, 1.0]; + let r = rvec_to_rmat(rvec_true[0], rvec_true[1], rvec_true[2]); + + let raw_obj = [ + [0.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [1.0, 1.0, 0.0], + [0.0, 1.0, 0.0], + [0.5, 0.5, 0.5], + [-0.5, 0.3, 0.2], + ]; + + let mut obj_points = Point3fVector::new(); + let mut img_points = Point2fVector::new(); + + for p in &raw_obj { + obj_points.push(p[0] as f32, p[1] as f32, p[2] as f32); + let cx = r[0] * p[0] + r[1] * p[1] + r[2] * p[2] + tvec_true[0]; + let cy = r[3] * p[0] + r[4] * p[1] + r[5] * p[2] + tvec_true[1]; + let cz = r[6] * p[0] + r[7] * p[1] + r[8] * p[2] + tvec_true[2]; + let u = k[0] * (cx / cz) + k[2]; + let v = k[4] * (cy / cz) + k[5]; + img_points.push(u as f32, v as f32); + } + + let camera_matrix = Mat::from_f64_data(3, 3, 1, &k).unwrap(); + + let mut rvec = Mat::from_f64_data(3, 1, 1, &vec![0.0; 3]).unwrap(); + let mut tvec = Mat::from_f64_data(3, 1, 1, &vec![0.0; 3]).unwrap(); + + solve_pnp_wasm( + &obj_points, + &img_points, + &camera_matrix, + None, + &mut rvec, + &mut tvec, + false, + 0, // Iterative + ) + .expect("solve_pnp_wasm failed"); + + let tvec_data = tvec.data_f64().unwrap(); + assert!((tvec_data[2] - 5.0).abs() < 1e-2); +} + +#[wasm_bindgen_test] +fn test_find_homography() { + let mut src_points = Point2fVector::new(); + src_points.push(0.0, 0.0); + src_points.push(1.0, 0.0); + src_points.push(0.0, 1.0); + src_points.push(1.0, 1.0); + + let mut dst_points = Point2fVector::new(); + dst_points.push(10.0, 10.0); + dst_points.push(20.0, 10.0); + dst_points.push(10.0, 20.0); + dst_points.push(20.0, 20.0); + + let mut h = Mat::from_f64_data(3, 3, 1, &vec![0.0; 9]).unwrap(); + + find_homography_wasm( + &src_points, + &dst_points, + 8, // Ransac + 3.0, + ) + .unwrap(); +} diff --git a/src/core/dynamic.rs b/src/core/dynamic.rs index 8817f62..a3824e1 100644 --- a/src/core/dynamic.rs +++ b/src/core/dynamic.rs @@ -409,6 +409,13 @@ impl DynamicMatrix { } } + pub fn as_matrix_f64_mut(&mut self) -> Option<&mut Matrix> { + match &mut self.data { + DynamicData::F64(m) => Some(m), + _ => None, + } + } + // -- Read a single element as f64 (for JS interop) ----------------------- /// Returns the element at `(row, col, channel)` cast to `f64`. From 90ce2a2282a8f7793fd804d46175c2343b1605a4 Mon Sep 17 00:00:00 2001 From: Walter Perdan Date: Sat, 9 May 2026 15:04:15 +0200 Subject: [PATCH 11/11] feat(wasm): add interactive pose estimation demo and update documentation (#53) --- crates/wasm/pkg/README.md | 6 +- crates/wasm/www/example_pose.html | 269 +++++++++++++++++++++++ crates/wasm/www/example_pose.js | 351 ++++++++++++++++++++++++++++++ crates/wasm/www/index.html | 5 + 4 files changed, 629 insertions(+), 2 deletions(-) create mode 100644 crates/wasm/www/example_pose.html create mode 100644 crates/wasm/www/example_pose.js diff --git a/crates/wasm/pkg/README.md b/crates/wasm/pkg/README.md index 56705b1..498a938 100644 --- a/crates/wasm/pkg/README.md +++ b/crates/wasm/pkg/README.md @@ -53,9 +53,11 @@ Because WebAssembly runs linearly in memory and holds pointers to Rust `Vec` obj ## API coverage -Right now we have covered a large majority of operations for `core` and `imgproc`: +Right now we have covered a large majority of operations for `core` and `imgproc`, and have started on `calib3d` and `video`: - **Core**: Arithmetic (`add`, `subtract`, `multiply`, `absdiff` etc.), Structural (`hconcat`, `vconcat`, `flip`), Geometry, constants etc. -- **ImgProc**: Filters (`blur`, `gaussian_blur`, `bilateral_filter`), Thresholding (`threshold`), Coloring (`cvt_color`), Edge Derivatives (`canny`, `sobel`, `laplacian`), Morphology (`erode`, `dilate`, `morphology_ex`, `get_structuring_element`), Pyramids (`pyr_down`, `pyr_up`, `build_pyramid`). +- **ImgProc**: Filters (`blur`, `gaussian_blur`, `bilateral_filter`), Thresholding (`threshold`), Coloring (`cvt_color`), Edge Derivatives (`canny`, `sobel`, `laplacian`), Morphology (`erode`, `dilate`, `morphology_ex`, `get_structuring_element`), Pyramids (`pyr_down`, `pyr_up`, `build_pyramid`), Feature Detection (`good_features_to_track`, `corner_sub_pix`). +- **Video**: Optical Flow (`calc_optical_flow_pyr_lk`). +- **Calib3d**: Pose Estimation (`solve_pnp`, `solve_pnp_ransac`), Homography (`find_homography`), and geometry (`rodrigues`). Note: To interface between JavaScript Typed Arrays and `purecv-wasm`, please use the available getter functions (`.data()`) which directly retrieve a Float32Array or Uint8Array view into WASM memory. diff --git a/crates/wasm/www/example_pose.html b/crates/wasm/www/example_pose.html new file mode 100644 index 0000000..d0f03c2 --- /dev/null +++ b/crates/wasm/www/example_pose.html @@ -0,0 +1,269 @@ + + + + + + + PureCV - Pose Estimation + + + +
+

Initializing WASM...

+
+ +
+
+

Pose Estimation

+

Drag the four marker corners to simulate a perspective view. PureCV computes the 3D camera pose in real-time.

+
+ +
+ +
+

Interactive Marker

+
+ +
+
+ +
+
+ + +
+

Computed Pose OK

+ +
+

Rotation Vector (rvec)

+
—
+
+ +
+

Translation Vector (tvec)

+
—
+
+ +
+

Rotation Matrix (rodrigues)

+
+ — + — + — + — + — + — + — + — + — +
+
+ +
+

Homography (3×3)

+
+ — + — + — + — + — + — + — + — + — +
+
+
+
+
+ + + + diff --git a/crates/wasm/www/example_pose.js b/crates/wasm/www/example_pose.js new file mode 100644 index 0000000..2f876a1 --- /dev/null +++ b/crates/wasm/www/example_pose.js @@ -0,0 +1,351 @@ +/* + * example_pose.js + * purecv + * + * This file is part of purecv - WebARKit. + * + * purecv is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * purecv is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with purecv. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2026 WebARKit. + * + * Author(s): Walter Perdan @kalwalt https://github.com/kalwalt + * + */ + +import { initWasm } from './cv_demo_utils.js'; + +// --------------------------------------------------------------------------- +// Constants +// --------------------------------------------------------------------------- + +const CANVAS_SIZE = 500; +const MARKER_HALF = 5; // half-size of the 3D marker (units) — 10×10 marker +const GRAB_RADIUS = 14; // hit-test radius for corner dragging (px) + +// Simulated camera intrinsics for a 500×500 "image" +const FX = 500, FY = 500, CX = 250, CY = 250; + +// 3D object points: a planar marker lying on Z=0 +// Two extra points are added to help the solver. +const OBJECT_POINTS_3D = [ + { x: -MARKER_HALF, y: -MARKER_HALF, z: 0 }, // top-left + { x: MARKER_HALF, y: -MARKER_HALF, z: 0 }, // top-right + { x: MARKER_HALF, y: MARKER_HALF, z: 0 }, // bottom-right + { x: -MARKER_HALF, y: MARKER_HALF, z: 0 }, // bottom-left + { x: 0, y: 0, z: 0 }, // centre + { x: 0, y: MARKER_HALF, z: 0 }, // bottom-centre +]; + +// Default image-point positions — computed from a slightly tilted frontal +// view (camera at z≈25) so solvePnP produces a clean initial pose. +function defaultCorners() { + return [ + { x: 155, y: 155 }, // TL + { x: 345, y: 150 }, // TR + { x: 350, y: 350 }, // BR + { x: 150, y: 345 }, // BL + ]; +} + +// Derive the 2 auxiliary image points from the 4 corners (centre + bottom-centre) +function auxPointsFromCorners(c) { + return [ + { + x: (c[0].x + c[1].x + c[2].x + c[3].x) / 4, + y: (c[0].y + c[1].y + c[2].y + c[3].y) / 4 + }, + { + x: (c[2].x + c[3].x) / 2, + y: (c[2].y + c[3].y) / 2 + }, + ]; +} + +// --------------------------------------------------------------------------- +// State +// --------------------------------------------------------------------------- + +let cv = null; +let corners = defaultCorners(); +let dragging = -1; // index of corner being dragged, or -1 + +const canvas = document.getElementById('canvas'); +const ctx = canvas.getContext('2d'); + +// --------------------------------------------------------------------------- +// Drawing helpers +// --------------------------------------------------------------------------- + +function drawScene() { + ctx.clearRect(0, 0, CANVAS_SIZE, CANVAS_SIZE); + + // Grid background + ctx.strokeStyle = 'rgba(255,255,255,0.04)'; + ctx.lineWidth = 1; + for (let i = 0; i <= CANVAS_SIZE; i += 25) { + ctx.beginPath(); ctx.moveTo(i, 0); ctx.lineTo(i, CANVAS_SIZE); ctx.stroke(); + ctx.beginPath(); ctx.moveTo(0, i); ctx.lineTo(CANVAS_SIZE, i); ctx.stroke(); + } + + // Semi-transparent filled quad + ctx.fillStyle = 'rgba(79, 172, 254, 0.12)'; + ctx.beginPath(); + ctx.moveTo(corners[0].x, corners[0].y); + for (let i = 1; i < 4; i++) ctx.lineTo(corners[i].x, corners[i].y); + ctx.closePath(); + ctx.fill(); + + // Quad outline + ctx.strokeStyle = '#4facfe'; + ctx.lineWidth = 2; + ctx.beginPath(); + ctx.moveTo(corners[0].x, corners[0].y); + for (let i = 1; i < 4; i++) ctx.lineTo(corners[i].x, corners[i].y); + ctx.closePath(); + ctx.stroke(); + + // Corner dots with labels + const labels = ['TL', 'TR', 'BR', 'BL']; + const colors = ['#22c55e', '#f59e0b', '#ef4444', '#a855f7']; + corners.forEach((pt, i) => { + ctx.fillStyle = colors[i]; + ctx.beginPath(); + ctx.arc(pt.x, pt.y, 8, 0, 2 * Math.PI); + ctx.fill(); + + ctx.fillStyle = '#fff'; + ctx.font = 'bold 11px Inter, system-ui, sans-serif'; + ctx.textAlign = 'center'; + ctx.fillText(labels[i], pt.x, pt.y - 14); + }); + + // Crosshair at image centre + ctx.strokeStyle = 'rgba(255,255,255,0.15)'; + ctx.lineWidth = 1; + ctx.setLineDash([4, 4]); + ctx.beginPath(); ctx.moveTo(CX, 0); ctx.lineTo(CX, CANVAS_SIZE); ctx.stroke(); + ctx.beginPath(); ctx.moveTo(0, CY); ctx.lineTo(CANVAS_SIZE, CY); ctx.stroke(); + ctx.setLineDash([]); +} + +// --------------------------------------------------------------------------- +// Pose computation +// --------------------------------------------------------------------------- + +function computePose() { + if (!cv) return; + + const aux = auxPointsFromCorners(corners); + const allImagePts = [...corners, ...aux]; // 6 points + + // Build Point3fVector + const objPts = new cv.Point3fVector(); + OBJECT_POINTS_3D.forEach(p => objPts.push(p.x, p.y, p.z)); + + // Build Point2fVector + const imgPts = new cv.Point2fVector(); + allImagePts.forEach(p => imgPts.push(p.x, p.y)); + + // Camera matrix (f64, 3×3) + const camMat = cv.Mat.fromF64Data(3, 3, 1, new Float64Array([ + FX, 0, CX, + 0, FY, CY, + 0, 0, 1, + ])); + + // Output rvec / tvec (3×1 f64) + const rvec = cv.Mat.fromF64Data(3, 1, 1, new Float64Array([0, 0, 0])); + const tvec = cv.Mat.fromF64Data(3, 1, 1, new Float64Array([0, 0, 0])); + + let pnpOk = false; + try { + // solvePnP(objPts, imgPts, camMat, distCoeffs, rvec, tvec, useGuess, flags) + // flags=0 → Iterative + pnpOk = cv.solvePnP(objPts, imgPts, camMat, undefined, rvec, tvec, false, 0); + } catch (e) { + console.warn('solvePnP failed:', e); + } + + let rmatData = null; + if (pnpOk) { + // rodrigues: rvec → rotation matrix + const rmat = cv.Mat.fromF64Data(3, 3, 1, new Float64Array(9)); + try { + cv.rodrigues(rvec, rmat); + rmatData = rmat.dataF64(); + } catch (e) { + console.warn('rodrigues failed:', e); + } + rmat.free(); + } + + // findHomography (using the 4 corner correspondences only) + const srcH = new cv.Point2fVector(); + const dstH = new cv.Point2fVector(); + // Source = default (un-dragged) marker corners + const canon = [ + { x: 155, y: 155 }, { x: 345, y: 150 }, + { x: 350, y: 350 }, { x: 150, y: 345 }, + ]; + canon.forEach(p => srcH.push(p.x, p.y)); + corners.forEach(p => dstH.push(p.x, p.y)); + + let homoData = null; + try { + // method=0 (None/DLT), threshold=3.0 + const result = cv.findHomography(srcH, dstH, 0, 3.0); + if (result && result.homography) { + homoData = result.homography.dataF64(); + result.homography.free(); + } + } catch (e) { + console.warn('findHomography failed:', e); + } + + // Update the UI + updateResultsUI(pnpOk, rvec, tvec, rmatData, homoData); + + // Cleanup WASM objects + objPts.free(); + imgPts.free(); + camMat.free(); + rvec.free(); + tvec.free(); + srcH.free(); + dstH.free(); +} + +// --------------------------------------------------------------------------- +// UI updates +// --------------------------------------------------------------------------- + +function fmt(v) { return v.toFixed(4); } + +function updateResultsUI(pnpOk, rvec, tvec, rmatData, homoData) { + const badge = document.getElementById('status-badge'); + if (pnpOk) { + badge.textContent = 'OK'; + badge.className = 'badge badge-ok'; + } else { + badge.textContent = 'FAIL'; + badge.className = 'badge badge-fail'; + } + + if (pnpOk) { + const rv = rvec.dataF64(); + const tv = tvec.dataF64(); + document.getElementById('rvec-display').textContent = + `[${fmt(rv[0])}, ${fmt(rv[1])}, ${fmt(rv[2])}]`; + document.getElementById('tvec-display').textContent = + `[${fmt(tv[0])}, ${fmt(tv[1])}, ${fmt(tv[2])}]`; + } else { + document.getElementById('rvec-display').textContent = '— solve failed —'; + document.getElementById('tvec-display').textContent = '— solve failed —'; + } + + // Rotation matrix + const rmatEl = document.getElementById('rmat-display'); + if (rmatData) { + rmatEl.innerHTML = Array.from(rmatData).slice(0, 9).map(v => + `${fmt(v)}` + ).join(''); + } else { + rmatEl.innerHTML = Array(9).fill('—').join(''); + } + + // Homography + const homoEl = document.getElementById('homo-display'); + if (homoData) { + homoEl.innerHTML = Array.from(homoData).slice(0, 9).map(v => + `${fmt(v)}` + ).join(''); + } else { + homoEl.innerHTML = Array(9).fill('—').join(''); + } +} + +// --------------------------------------------------------------------------- +// Interaction: drag corners +// --------------------------------------------------------------------------- + +function getMousePos(e) { + const rect = canvas.getBoundingClientRect(); + const scaleX = CANVAS_SIZE / rect.width; + const scaleY = CANVAS_SIZE / rect.height; + return { + x: (e.clientX - rect.left) * scaleX, + y: (e.clientY - rect.top) * scaleY, + }; +} + +canvas.addEventListener('pointerdown', (e) => { + const pos = getMousePos(e); + for (let i = 0; i < corners.length; i++) { + const dx = pos.x - corners[i].x; + const dy = pos.y - corners[i].y; + if (Math.sqrt(dx * dx + dy * dy) < GRAB_RADIUS) { + dragging = i; + canvas.setPointerCapture(e.pointerId); + break; + } + } +}); + +canvas.addEventListener('pointermove', (e) => { + if (dragging < 0) return; + const pos = getMousePos(e); + corners[dragging].x = Math.max(10, Math.min(CANVAS_SIZE - 10, pos.x)); + corners[dragging].y = Math.max(10, Math.min(CANVAS_SIZE - 10, pos.y)); + drawScene(); + computePose(); +}); + +canvas.addEventListener('pointerup', () => { dragging = -1; }); +canvas.addEventListener('pointercancel', () => { dragging = -1; }); + +document.getElementById('reset-btn').addEventListener('click', () => { + corners = defaultCorners(); + drawScene(); + computePose(); +}); + +// --------------------------------------------------------------------------- +// Bootstrap +// --------------------------------------------------------------------------- + +async function start() { + try { + cv = await initWasm(); + document.getElementById('loader').classList.add('hidden'); + drawScene(); + computePose(); + } catch (err) { + console.error("WASM initialisation failed:", err); + document.getElementById('loader').innerHTML = + `

Error loading WASM: ${err.message}

`; + } +} + +start(); diff --git a/crates/wasm/www/index.html b/crates/wasm/www/index.html index 7e71862..27b9cf4 100644 --- a/crates/wasm/www/index.html +++ b/crates/wasm/www/index.html @@ -134,6 +134,11 @@

Butterfly Demo

Optical Flow

Track features across webcam frames using pyramidal Lucas-Kanade optical flow.

+ + Calib3d +

Pose Estimation

+

Drag marker corners to compute camera pose (solvePnP, rodrigues) and homography in real-time.

+