From c8dff2d0ccb47fca6d361bd4bd2615af93ed7b9a Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Sun, 25 Oct 2020 09:37:12 -0700 Subject: [PATCH 01/26] Support math in docs --- .cargo/config | 3 ++- .cargo/katex-header.html | 43 ++++++++++++++++++++++++++++++++++++++++ cv-pinhole/Cargo.toml | 1 + 3 files changed, 46 insertions(+), 1 deletion(-) create mode 100644 .cargo/katex-header.html diff --git a/.cargo/config b/.cargo/config index d5135e98..b49224a2 100644 --- a/.cargo/config +++ b/.cargo/config @@ -1,2 +1,3 @@ [build] -rustflags = ["-C", "target-cpu=native"] \ No newline at end of file +rustflags = ["-C", "target-cpu=native"] +rustdocflags = ["--html-in-header", "../.cargo/katex-header.html"] diff --git a/.cargo/katex-header.html b/.cargo/katex-header.html new file mode 100644 index 00000000..dec17c04 --- /dev/null +++ b/.cargo/katex-header.html @@ -0,0 +1,43 @@ + + + + \ No newline at end of file diff --git a/cv-pinhole/Cargo.toml b/cv-pinhole/Cargo.toml index c92945a7..3d6df6fb 100644 --- a/cv-pinhole/Cargo.toml +++ b/cv-pinhole/Cargo.toml @@ -29,3 +29,4 @@ cv-geom = { version = "0.7.0", path = "../cv-geom" } [package.metadata.docs.rs] all-features = true +rustdoc-args = ["--html-in-header", ".cargo/katex-header.html"] From 48e4951578795cf903c8fa13a929a865ec5a5269 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Sun, 25 Oct 2020 09:37:42 -0700 Subject: [PATCH 02/26] Add RadialDistortion --- cv-pinhole/src/lib.rs | 2 + cv-pinhole/src/radial.rs | 161 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 163 insertions(+) create mode 100644 cv-pinhole/src/radial.rs diff --git a/cv-pinhole/src/lib.rs b/cv-pinhole/src/lib.rs index 592416b6..43214982 100644 --- a/cv-pinhole/src/lib.rs +++ b/cv-pinhole/src/lib.rs @@ -9,8 +9,10 @@ extern crate alloc; mod essential; +mod radial; pub use essential::*; +pub use radial::RadialDistortion; use cv_core::nalgebra::{Matrix3, Point2, Point3, Vector2, Vector3}; use cv_core::{ diff --git a/cv-pinhole/src/radial.rs b/cv-pinhole/src/radial.rs new file mode 100644 index 00000000..c7501e40 --- /dev/null +++ b/cv-pinhole/src/radial.rs @@ -0,0 +1,161 @@ +//! Radial distortion model + +use num_traits::Float; + +/// Maxmimum number of iterations to use in Newton-Raphson inversion. +const MAX_ITERATIONS: usize = 100; + +/// Convergence treshold for Newton-Raphson inversion. +const EPSILON: f64 = f64::EPSILON; + +/// Radial distortion model +/// +/// The radial distortion is calibrated by scaling with a degree $(6,6)$ [even](https://en.wikipedia.org/wiki/Even_and_odd_functions#Even_functions) [rational](https://en.wikipedia.org/wiki/Rational_function) function: +/// +/// $$ +/// r' = r ⋅ \frac{1 + k_1 ⋅ r^2 + k_2 ⋅ r^4 + k_3 ⋅ r^6}{1 + k_4 ⋅ r^2 + k_5 ⋅ r^4 + k_6 ⋅ r^6} +/// $$ +/// +#[derive(Clone, PartialEq, Debug)] +pub struct RadialDistortion(pub [f64; 6]); + +impl RadialDistortion { + /// Given $r$ compute $r'$. + pub fn calibrate(&self, r: f64) -> f64 { + r * self.scale_r2(r.powi(2)) + } + + /// Given $r^2$ compute the radial scaling factor $\frac {r'}{r}$. + #[rustfmt::skip] + pub fn scale_r2(&self, r2: f64) -> f64 { + let mut p = self.0[2]; + p *= r2; p += self.0[1]; + p *= r2; p += self.0[0]; + p *= r2; p += 1.0; + let mut q = self.0[5]; + q *= r2; q += self.0[4]; + q *= r2; q += self.0[3]; + q *= r2; q += 1.0; + p / q + } + + /// Given $r'$ compute $r$. + /// + /// # Examples + /// + /// ``` + /// use cv_pinhole::RadialDistortion; + /// + /// let distortion = RadialDistortion([0.1, 0.2, 0.3, 0.4, 0.2, 0.1]); + /// let camera_r = 1.3; + /// let corrected_r = distortion.calibrate(camera_r); + /// let reprojected_r = distortion.uncalibrate(corrected_r); + /// assert_eq!(camera_r, reprojected_r); + /// ``` + /// + /// # Method + /// + /// Given $r'$ solve for $r$. Start with the known relation + /// + /// + /// $$ + /// r' = r ⋅ \frac{1 + k_1 ⋅ r^2 + k_2 ⋅ r^4 + k_3 ⋅ r^6} + /// {1 + k_4 ⋅ r^2 + k_5 ⋅ r^4 + k_6 ⋅ r^6} + /// $$ + /// + /// manipulate the rational relation into a polynomial + /// + /// $$ + /// \begin{aligned} + /// r' ⋅ \p{1 + k_4 ⋅ r^2 + k_5 ⋅ r^4 + k_6 ⋅ r^6} + /// &= r ⋅ \p{1 + k_1 ⋅ r^2 + k_2 ⋅ r^4 + k_3 ⋅ r^6} \\\\ + /// r' + r' ⋅ k_4 ⋅ r^2 + r' ⋅ k_5 ⋅ r^4 + r' ⋅ k_6 ⋅ r^6 + /// &= r + k_1 ⋅ r^3 + k_2 ⋅ r^5 + k_3 ⋅ r^7 \\\\ + /// \end{aligned} + /// $$ + /// + /// $$ + /// r' + r' ⋅ k_4 ⋅ r^2 + r' ⋅ k_5 ⋅ r^4 + r' ⋅ k_6 ⋅ r^6 + /// \- r - k_1 ⋅ r^3 - k_2 ⋅ r^5 - k_3 ⋅ r^7 = 0 + /// $$ + /// + /// That is, we need to find the root of the 7th-order polynomial with coefficients: + /// + /// $$ + /// P(r) = + /// \begin{bmatrix} + /// r' & -1 & r' ⋅ k_4 & -k_1 & r' ⋅ k_5 & -k_2 & r' ⋅ k_6 & - k_3 + /// \end{bmatrix}^T ⋅ V(r) + /// $$ + /// + /// where $V(r) = \begin{bmatrix} 1 & r & r^2 & ᠁ \end{bmatrix}$ is the [Vandermonde matrix](https://en.wikipedia.org/wiki/Vandermonde_matrix). The coefficients of the 6ht-order derivative polynomials are: + /// + /// $$ + /// P'(r) = + /// \begin{bmatrix} + /// -1 & 2 ⋅ r' ⋅ k_4 & -3 ⋅ k_1 & 4 ⋅ r' ⋅ k_5 & -5 ⋅ k_2 & 6 ⋅ r' ⋅ k_6 & - 7 ⋅ k_3 + /// \end{bmatrix}^T ⋅ V(r) + /// $$ + /// + /// Using $r'$ as the starting value we can approximate $r'$ using [Newton's method](https://en.wikipedia.org/wiki/Newton%27s_method): + /// + /// $$ + /// \begin{aligned} + /// r_0 &= r' & r_{i+1} = r_i - \frac{P(r_i)}{P'(r_i)} + /// \end{aligned} + /// $$ + /// + /// **Note.** We could also produce higher derivatives and use [Halley's method](https://en.wikipedia.org/wiki/Halley%27s_method) or higher order [Housholder methods](https://en.wikipedia.org/wiki/Householder%27s_method). + /// + /// The inversion is approximated using at most [`MAX_ITERATIONS`] rounds of Newton-Raphson + /// or when the $\abs{r_{i+1} - r_i}$ is less than [`EPSILON`] times $r_{i+1}$. + #[rustfmt::skip] + pub fn uncalibrate(&self, mut r: f64) -> f64 { + let p = [ + r, -1.0, + r * self.0[3], -self.0[0], + r * self.0[4], -self.0[1], + r * self.0[5], -self.0[2], + ]; + // Iterate Newtons method + for _ in 0..MAX_ITERATIONS { + let mut value = p[7]; + value *= r; value += p[6]; + value *= r; value += p[5]; + value *= r; value += p[4]; + value *= r; value += p[3]; + value *= r; value += p[2]; + value *= r; value += p[1]; + value *= r; value += p[0]; + let mut deriv = p[7] * 7.0; + deriv *= r; deriv += p[6] * 6.0; + deriv *= r; deriv += p[5] * 5.0; + deriv *= r; deriv += p[4] * 4.0; + deriv *= r; deriv += p[3] * 3.0; + deriv *= r; deriv += p[2] * 2.0; + deriv *= r; deriv += p[1]; + let delta = value / deriv; + r -= delta; + if delta.abs() <= EPSILON * r { + break; + } + } + r + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_invert() { + let distortion = RadialDistortion([0.1, 0.2, 0.3, 0.4, 0.2, 0.1]); + let test_values = [0.0, 0.5, 1.0, 1.5]; + for &camera_r in &test_values { + let corrected = distortion.calibrate(camera_r); + let reprojected = distortion.uncalibrate(corrected); + assert_eq!(camera_r, reprojected); + } + } +} From b85c78989057c512bca76f910ad6514ca1a8ef8b Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Mon, 2 Nov 2020 11:14:46 -0800 Subject: [PATCH 03/26] Doc tweaks --- .cargo/config | 2 +- .cargo/katex-header.html | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/.cargo/config b/.cargo/config index b49224a2..5608b2b1 100644 --- a/.cargo/config +++ b/.cargo/config @@ -1,3 +1,3 @@ [build] rustflags = ["-C", "target-cpu=native"] -rustdocflags = ["--html-in-header", "../.cargo/katex-header.html"] +rustdocflags = ["--html-in-header", "./.cargo/katex-header.html"] diff --git a/.cargo/katex-header.html b/.cargo/katex-header.html index dec17c04..ca63b907 100644 --- a/.cargo/katex-header.html +++ b/.cargo/katex-header.html @@ -16,6 +16,7 @@ \gdef\C{\mathbb C} \gdef\∪{\bigcup} \gdef\∀{\forall} + \gdef\vec#1{\bm{#1}} \gdef\delim#1#2#3{\mathopen{}\left#1 #2 \right#3\mathclose{}} \gdef\p#1{\delim({#1})} \gdef\abs#1{\delim\lvert{#1}\rvert} From b7ec96463134b89cf34ed07047ccdf37c5731ba7 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Mon, 2 Nov 2020 11:16:11 -0800 Subject: [PATCH 04/26] Generic distortion functions --- cv-pinhole/src/camera.rs | 154 ++++++++++++++++++ .../src/distortion_function/even_poly_2.rs | 150 +++++++++++++++++ .../even_rational_6_6.rs} | 27 ++- .../src/distortion_function/identity.rs | 24 +++ cv-pinhole/src/distortion_function/mod.rs | 26 +++ .../src/distortion_function/polynomial.rs | 50 ++++++ .../src/distortion_function/rational.rs | 38 +++++ cv-pinhole/src/lib.rs | 5 +- 8 files changed, 457 insertions(+), 17 deletions(-) create mode 100644 cv-pinhole/src/camera.rs create mode 100644 cv-pinhole/src/distortion_function/even_poly_2.rs rename cv-pinhole/src/{radial.rs => distortion_function/even_rational_6_6.rs} (91%) create mode 100644 cv-pinhole/src/distortion_function/identity.rs create mode 100644 cv-pinhole/src/distortion_function/mod.rs create mode 100644 cv-pinhole/src/distortion_function/polynomial.rs create mode 100644 cv-pinhole/src/distortion_function/rational.rs diff --git a/cv-pinhole/src/camera.rs b/cv-pinhole/src/camera.rs new file mode 100644 index 00000000..a7c499be --- /dev/null +++ b/cv-pinhole/src/camera.rs @@ -0,0 +1,154 @@ +use crate::CameraIntrinsics; +use crate::DistortionFunction; +use crate::NormalizedKeyPoint; + +use cv_core::nalgebra::{Matrix2, Vector2}; +use cv_core::{CameraModel, ImagePoint, KeyPoint}; + +/// Camera with distortion. +/// +/// The distortion model is compatible with OpenCV. +/// +/// Given image coordinates $(x,y)$ and $r = x^2 + y^2$ the undistorted coordinates $(x', y')$ are: +/// +/// $$ +/// \begin{bmatrix} x' \\\\ y' \end{bmatrix} = +/// \vec f \p{\begin{bmatrix} x \\\\ y \end{bmatrix}} = \begin{bmatrix} +/// x ⋅ f_r(r^2) + \p{2⋅x⋅\p{t_1 ⋅ x + t_2 ⋅ y} + t_1 ⋅ r^2} ⋅ f_t(r^2) + f_{px}(r^2) +/// \\\\[.8em] +/// y ⋅ f_r(r^2) + \p{2⋅y⋅\p{t_1 ⋅ x + t_2 ⋅ y} + t_2 ⋅ r^2} ⋅ f_t(r^2) + f_{py}(r^2) +/// \end{bmatrix} +/// $$ +/// +/// where $f_r$ is the *radial* distortion function, $(t_1, t_2, f_t)$ specify the *tangential* distortion (See [Brown][b66] p. 454) and $(f_{px}, f_{py})$ the *thin prism* distortion. +/// +/// **Note.** The tangential distortion compensates for lenses not being entirely parallel with the +/// image plane. Despite what the name may suggest, its contribution is not orthogonal to the +/// radial distortion. +/// +/// # References +/// +/// * D.C. Brown (1996). Decentering Distortion of Lenses. [online][b66]. +/// +/// [b66]: https://web.archive.org/web/20180312205006/https://www.asprs.org/wp-content/uploads/pers/1966journal/may/1966_may_444-462.pdf +/// +/// +/// https://medium.com/analytics-vidhya/camera-calibration-with-opencv-f324679c6eb7 +/// +/// https://spectrogrism.readthedocs.io/en/latest/distortions.html +/// +/// +#[derive(Clone, PartialEq, PartialOrd, Debug)] +// #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct Camera +where + R: DistortionFunction, + T: DistortionFunction, + P: DistortionFunction, +{ + linear: CameraIntrinsics, + radial_distortion: R, + tangential: [f64; 2], + tangential_distortion: T, + prism_distortion: [P; 2], +} + +impl Camera +where + R: DistortionFunction, + T: DistortionFunction, + P: DistortionFunction, +{ + pub fn new(linear: CameraIntrinsics, radial_distortion: R, tangential_distortion: T) -> Self { + todo!() + // Self { + // linear, + // radial_distortion, + // tangential_distortion, + // } + } + + /// Computes $\vec f(\mathtt{point})$. + pub fn correct(&self, point: Vector2) -> Vector2 { + let r2 = point.norm_squared(); + let f_r = self.radial_distortion.evaluate(r2); + let f_t = self.tangential_distortion.evaluate(r2); + let f_px = self.prism_distortion[0].evaluate(r2) - 1.0; + let f_py = self.prism_distortion[1].evaluate(r2) - 1.0; + let t = self.tangential[0] * point.x + self.tangential[1] * point.y; + let t_x = 2.0 * point.x * t + self.tangential[0] * r2; + let t_y = 2.0 * point.y * t + self.tangential[1] * r2; + Vector2::new( + point.x * f_r + t_x * f_t + f_px, + point.y * f_r + t_y * f_t + f_py, + ) + } + + /// Computes $\mathbf{J}_{\vec f}(\mathtt{point})$. + /// + /// $$ + /// \mathbf{J}_{\vec f} = \begin{bmatrix} + /// \frac{\partial f_x}{\partial x} & \frac{\partial f_x}{\partial y} \\\\[.8em] + /// \frac{\partial f_y}{\partial x} & \frac{\partial f_y}{\partial y} + /// \end{bmatrix} + /// $$ + /// + /// $$ + /// \begin{aligned} + /// \frac{\partial f_x}{\partial x} &= + /// f_r(r^2) + 2 ⋅ x^2 ⋅ f_r'(r^2) \\\\ &\phantom{=} + /// + \p{6⋅t_1 ⋅ x + 2 ⋅ t_2 ⋅ y} ⋅ f_t(r^2) \\\\ &\phantom{=} + /// + \p{2⋅x⋅\p{t_1 ⋅ x + t_2 ⋅ y} + t_1 ⋅ r^2} ⋅ 2 ⋅ x ⋅ f_t'(r^2) \\\\ &\phantom{=} + /// + 2 ⋅ x ⋅ f_{px}'(r^2) + /// \end{aligned} + /// $$ + /// + /// $$ + /// \begin{aligned} + /// \frac{\partial f_x}{\partial y} &= + /// 2 ⋅ x ⋅ y^2 ⋅ f_r'(r^2) \\\\ &\phantom{=} + /// + \p{2⋅t_1 ⋅ y + 2 ⋅ t_2 ⋅ x} ⋅ f_t(r^2) \\\\ &\phantom{=} + /// + \p{2⋅x⋅\p{t_1 ⋅ x + t_2 ⋅ y} + t_1 ⋅ r^2} ⋅ 2 ⋅ y ⋅ f_t'(r^2) \\\\ &\phantom{=} + /// + 2 ⋅ y ⋅ f_{px}'(r^2) + /// \end{aligned} + /// $$ + /// + pub fn jacobian(&self, point: Vector2) -> Matrix2 { + let r2 = point.norm_squared(); + let (f_r, df_r) = self.radial_distortion.with_derivative(r2); + let (f_t, df_t) = self.tangential_distortion.with_derivative(r2); + let df_px = self.prism_distortion[0].derivative(r2); + let df_py = self.prism_distortion[1].derivative(r2); + + todo!() + // Matrix2::new() + } +} + +impl CameraModel for Camera +where + R: DistortionFunction, + T: DistortionFunction, + P: DistortionFunction, +{ + type Projection = NormalizedKeyPoint; + + fn calibrate(&self, point: Point) -> Self::Projection { + let NormalizedKeyPoint(distorted) = self.linear.calibrate(point); + let distorted_r = distorted.coords.norm(); + let corrected_r = self.radial_distortion.evaluate(distorted_r); + let r_factor = corrected_r / distorted_r; + let corrected = (distorted.coords * r_factor).into(); + + NormalizedKeyPoint(corrected) + } + + fn uncalibrate(&self, projection: Self::Projection) -> KeyPoint { + let NormalizedKeyPoint(corrected) = projection; + let corrected_r = corrected.coords.norm(); + let distorted_r = self.radial_distortion.inverse(corrected_r); + let r_factor = distorted_r / corrected_r; + let distorted = NormalizedKeyPoint((corrected.coords * r_factor).into()); + self.linear.uncalibrate(distorted).into() + } +} diff --git a/cv-pinhole/src/distortion_function/even_poly_2.rs b/cv-pinhole/src/distortion_function/even_poly_2.rs new file mode 100644 index 00000000..20609aac --- /dev/null +++ b/cv-pinhole/src/distortion_function/even_poly_2.rs @@ -0,0 +1,150 @@ +use super::DistortionFunction; +use num_traits::Float; + +/// Maxmimum number of iterations to use in Newton-Raphson inversion. +const MAX_ITERATIONS: usize = 100; + +/// Convergence treshold for Newton-Raphson inversion. +const EPSILON: f64 = f64::EPSILON; + +/// Even $(6,6)$-rational distortion function +/// +/// The radial distortion is calibrated by scaling with a degree $(6,6)$ [even](https://en.wikipedia.org/wiki/Even_and_odd_functions#Even_functions) [polynomial](https://en.wikipedia.org/wiki/Polynomial) function: +/// +/// $$ +/// r' = r ⋅ \p{1 + k_1 ⋅ r^2} +/// $$ +/// +#[derive(Clone, PartialEq, Default, Debug)] +pub struct EvenPoly2(f64); + +impl EvenPoly2 {} + +impl DistortionFunction for EvenPoly2 { + /// Given $r$ compute $r'$. + #[rustfmt::skip] + fn evaluate(&self, r: f64) -> f64 { + let r2 = r * r; + r * (1.0 + self.0 * r2) + } + + /// Given $r'$ compute $r$. + /// + /// # Examples + /// + /// ``` + /// use cv_pinhole::RadialDistortion; + /// + /// let distortion = RadialDistortion([0.1, 0.2, 0.3, 0.4, 0.2, 0.1]); + /// let camera_r = 1.3; + /// let corrected_r = distortion.calibrate(camera_r); + /// let reprojected_r = distortion.uncalibrate(corrected_r); + /// assert_eq!(camera_r, reprojected_r); + /// ``` + /// + /// # Method + /// + /// Given $r'$ solve for $r$. Start with the known relation + /// + /// + /// $$ + /// r' = r ⋅ \p{1 + k_1 ⋅ r^2} + /// $$ + /// + /// manipulate the rational relation into a polynomial + /// + /// $$ + /// \begin{aligned} + /// r' ⋅ \p{1 + k_4 ⋅ r^2 + k_5 ⋅ r^4 + k_6 ⋅ r^6} + /// &= r ⋅ \p{1 + k_1 ⋅ r^2 + k_2 ⋅ r^4 + k_3 ⋅ r^6} \\\\ + /// r' + r' ⋅ k_4 ⋅ r^2 + r' ⋅ k_5 ⋅ r^4 + r' ⋅ k_6 ⋅ r^6 + /// &= r + k_1 ⋅ r^3 + k_2 ⋅ r^5 + k_3 ⋅ r^7 \\\\ + /// \end{aligned} + /// $$ + /// + /// $$ + /// r' + r' ⋅ k_4 ⋅ r^2 + r' ⋅ k_5 ⋅ r^4 + r' ⋅ k_6 ⋅ r^6 + /// \- r - k_1 ⋅ r^3 - k_2 ⋅ r^5 - k_3 ⋅ r^7 = 0 + /// $$ + /// + /// That is, we need to find the root of the 7th-order polynomial with coefficients: + /// + /// $$ + /// P(r) = + /// \begin{bmatrix} + /// r' & -1 & r' ⋅ k_4 & -k_1 & r' ⋅ k_5 & -k_2 & r' ⋅ k_6 & - k_3 + /// \end{bmatrix}^T ⋅ V(r) + /// $$ + /// + /// where $V(r) = \begin{bmatrix} 1 & r & r^2 & ᠁ \end{bmatrix}$ is the [Vandermonde matrix](https://en.wikipedia.org/wiki/Vandermonde_matrix). The coefficients of the 6ht-order derivative polynomials are: + /// + /// $$ + /// P'(r) = + /// \begin{bmatrix} + /// -1 & 2 ⋅ r' ⋅ k_4 & -3 ⋅ k_1 & 4 ⋅ r' ⋅ k_5 & -5 ⋅ k_2 & 6 ⋅ r' ⋅ k_6 & - 7 ⋅ k_3 + /// \end{bmatrix}^T ⋅ V(r) + /// $$ + /// + /// Using $r'$ as the starting value we can approximate $r'$ using [Newton's method](https://en.wikipedia.org/wiki/Newton%27s_method): + /// + /// $$ + /// \begin{aligned} + /// r_0 &= r' & r_{i+1} = r_i - \frac{P(r_i)}{P'(r_i)} + /// \end{aligned} + /// $$ + /// + /// **Note.** We could also produce higher derivatives and use [Halley's method](https://en.wikipedia.org/wiki/Halley%27s_method) or higher order [Housholder methods](https://en.wikipedia.org/wiki/Householder%27s_method). + /// + /// The inversion is approximated using at most [`MAX_ITERATIONS`] rounds of Newton-Raphson + /// or when the $\abs{r_{i+1} - r_i}$ is less than [`EPSILON`] times $r_{i+1}$. + #[rustfmt::skip] + fn inverse(&self, mut r: f64) -> f64 { + todo!() + // let p = [ + // r, -1.0, + // r * self.0[3], -self.0[0], + // r * self.0[4], -self.0[1], + // r * self.0[5], -self.0[2], + // ]; + // // Iterate Newtons method + // for _ in 0..MAX_ITERATIONS { + // let mut value = p[7]; + // value *= r; value += p[6]; + // value *= r; value += p[5]; + // value *= r; value += p[4]; + // value *= r; value += p[3]; + // value *= r; value += p[2]; + // value *= r; value += p[1]; + // value *= r; value += p[0]; + // let mut deriv = p[7] * 7.0; + // deriv *= r; deriv += p[6] * 6.0; + // deriv *= r; deriv += p[5] * 5.0; + // deriv *= r; deriv += p[4] * 4.0; + // deriv *= r; deriv += p[3] * 3.0; + // deriv *= r; deriv += p[2] * 2.0; + // deriv *= r; deriv += p[1]; + // let delta = value / deriv; + // r -= delta; + // if Float::abs(delta) <= EPSILON * r { + // break; + // } + // } + // r + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_invert() { + let distortion = RadialDistortion([0.1, 0.2, 0.3, 0.4, 0.2, 0.1]); + let test_values = [0.0, 0.5, 1.0, 1.5]; + for &camera_r in &test_values { + let corrected = distortion.calibrate(camera_r); + let reprojected = distortion.uncalibrate(corrected); + assert_eq!(camera_r, reprojected); + } + } +} diff --git a/cv-pinhole/src/radial.rs b/cv-pinhole/src/distortion_function/even_rational_6_6.rs similarity index 91% rename from cv-pinhole/src/radial.rs rename to cv-pinhole/src/distortion_function/even_rational_6_6.rs index c7501e40..98b08ac2 100644 --- a/cv-pinhole/src/radial.rs +++ b/cv-pinhole/src/distortion_function/even_rational_6_6.rs @@ -1,5 +1,4 @@ -//! Radial distortion model - +use super::DistortionFunction; use num_traits::Float; /// Maxmimum number of iterations to use in Newton-Raphson inversion. @@ -8,7 +7,7 @@ const MAX_ITERATIONS: usize = 100; /// Convergence treshold for Newton-Raphson inversion. const EPSILON: f64 = f64::EPSILON; -/// Radial distortion model +/// Even $(6,6)$-rational distortion function /// /// The radial distortion is calibrated by scaling with a degree $(6,6)$ [even](https://en.wikipedia.org/wiki/Even_and_odd_functions#Even_functions) [rational](https://en.wikipedia.org/wiki/Rational_function) function: /// @@ -16,18 +15,16 @@ const EPSILON: f64 = f64::EPSILON; /// r' = r ⋅ \frac{1 + k_1 ⋅ r^2 + k_2 ⋅ r^4 + k_3 ⋅ r^6}{1 + k_4 ⋅ r^2 + k_5 ⋅ r^4 + k_6 ⋅ r^6} /// $$ /// -#[derive(Clone, PartialEq, Debug)] -pub struct RadialDistortion(pub [f64; 6]); +#[derive(Clone, PartialEq, Default, Debug)] +pub struct EvenRational66(pub [f64; 6]); -impl RadialDistortion { - /// Given $r$ compute $r'$. - pub fn calibrate(&self, r: f64) -> f64 { - r * self.scale_r2(r.powi(2)) - } +impl EvenRational66 {} - /// Given $r^2$ compute the radial scaling factor $\frac {r'}{r}$. +impl DistortionFunction for EvenRational66 { + /// Given $r$ compute $r'$. #[rustfmt::skip] - pub fn scale_r2(&self, r2: f64) -> f64 { + fn evaluate(&self, r: f64) -> f64 { + let r2 = r * r; let mut p = self.0[2]; p *= r2; p += self.0[1]; p *= r2; p += self.0[0]; @@ -36,7 +33,7 @@ impl RadialDistortion { q *= r2; q += self.0[4]; q *= r2; q += self.0[3]; q *= r2; q += 1.0; - p / q + r * p / q } /// Given $r'$ compute $r$. @@ -110,7 +107,7 @@ impl RadialDistortion { /// The inversion is approximated using at most [`MAX_ITERATIONS`] rounds of Newton-Raphson /// or when the $\abs{r_{i+1} - r_i}$ is less than [`EPSILON`] times $r_{i+1}$. #[rustfmt::skip] - pub fn uncalibrate(&self, mut r: f64) -> f64 { + fn inverse(&self, mut r: f64) -> f64 { let p = [ r, -1.0, r * self.0[3], -self.0[0], @@ -136,7 +133,7 @@ impl RadialDistortion { deriv *= r; deriv += p[1]; let delta = value / deriv; r -= delta; - if delta.abs() <= EPSILON * r { + if Float::abs(delta) <= EPSILON * r { break; } } diff --git a/cv-pinhole/src/distortion_function/identity.rs b/cv-pinhole/src/distortion_function/identity.rs new file mode 100644 index 00000000..96610c69 --- /dev/null +++ b/cv-pinhole/src/distortion_function/identity.rs @@ -0,0 +1,24 @@ +use super::DistortionFunction; +use cv_core::nalgebra::{ + allocator::Allocator, zero, ArrayStorage, DefaultAllocator, Dim, DimName, NamedDim, VectorN, U1, +}; + +/// Identity distortion, i.e. no distortion at all. +#[derive(Clone, PartialEq, Default, Debug)] +struct Identity; + +impl DistortionFunction for Identity { + type NumParameters; + + fn evaluate(&self, value: f64) -> f64 { + value + } + + fn derivative(&self, value: f64) -> f64 { + 0.0 + } + + fn inverse(&self, value: f64) -> f64 { + value + } +} diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs new file mode 100644 index 00000000..2aca4595 --- /dev/null +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -0,0 +1,26 @@ +mod polynomial; +mod rational; + +use cv_core::nalgebra::Dim; + +pub trait DistortionFunction { + /// Type level number of parameters + type NumParameters: Dim; + + /// Undo distortion. + fn evaluate(&self, value: f64) -> f64; + + /// Evaluate with derivative + fn derivative(&self, value: f64) -> f64; + + /// Evaluate with derivative + fn with_derivative(&self, value: f64) -> (f64, f64) { + (self.evaluate(value), self.derivative(value)) + } + + /// Apply distortion. + fn inverse(&self, value: f64) -> f64; + + // TODO: Parameters, derivatives, Jacobians, etc for calibration + // fn parmaeters(&self) -> VectorN; +} diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs new file mode 100644 index 00000000..ee24764e --- /dev/null +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -0,0 +1,50 @@ +use super::DistortionFunction; +use cv_core::nalgebra::{ + allocator::Allocator, zero, ArrayStorage, DefaultAllocator, Dim, DimName, NamedDim, VectorN, U1, +}; + +#[derive(Clone, Debug)] +struct Polynomial(VectorN) +where + Degree: Dim, + DefaultAllocator: Allocator; + +impl DistortionFunction for Polynomial +where + Degree: Dim, + DefaultAllocator: Allocator, +{ + type NumParameters = Degree; + + fn evaluate(&self, value: f64) -> f64 { + // Basic horner evaluation. + // Ideally the compiler unrolls the loop and realizes that the first. + // multiplication is redundant. + let mut result = 0.0; + for &coefficient in self.0.iter() { + result *= value; + result += coefficient; + } + result + } + + fn derivative(&self, value: f64) -> f64 { + self.with_derivative(value).1 + } + + fn with_derivative(&self, value: f64) -> (f64, f64) { + let mut result = 0.0; + let mut derivative = 0.0; + for &coefficient in self.0.iter() { + derivative *= value; + derivative += result; + result *= value; + result += coefficient; + } + (result, derivative) + } + + fn inverse(&self, value: f64) -> f64 { + todo!() + } +} diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs new file mode 100644 index 00000000..7ba946ea --- /dev/null +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -0,0 +1,38 @@ +use super::DistortionFunction; +use cv_core::nalgebra::DimAdd; +use cv_core::nalgebra::DimSum; + +#[derive(Clone, Debug, Default)] +struct Rational(P, Q) +where + P: DistortionFunction, + Q: DistortionFunction; + +impl DistortionFunction for Rational +where + P: DistortionFunction, + Q: DistortionFunction, + P::NumParameters: DimAdd, +{ + type NumParameters = DimSum; + + fn evaluate(&self, value: f64) -> f64 { + let p = self.0.evaluate(value); + let q = self.1.evaluate(value); + p / q + } + + fn derivative(&self, value: f64) -> f64 { + self.with_derivative(value).1 + } + + fn with_derivative(&self, value: f64) -> (f64, f64) { + let (p, dp) = self.0.with_derivative(value); + let (q, dq) = self.1.with_derivative(value); + (p / q, (dp * q - p * dq) / (q * q)) + } + + fn inverse(&self, value: f64) -> f64 { + todo!() + } +} diff --git a/cv-pinhole/src/lib.rs b/cv-pinhole/src/lib.rs index 43214982..2f0ec5a0 100644 --- a/cv-pinhole/src/lib.rs +++ b/cv-pinhole/src/lib.rs @@ -8,11 +8,12 @@ #[cfg(feature = "alloc")] extern crate alloc; +mod camera; +mod distortion_function; mod essential; -mod radial; +pub use distortion_function::DistortionFunction; pub use essential::*; -pub use radial::RadialDistortion; use cv_core::nalgebra::{Matrix3, Point2, Point3, Vector2, Vector3}; use cv_core::{ From 6d8d91a0f0a5076927a3666c8192110abfb9d24b Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Mon, 2 Nov 2020 11:35:53 -0800 Subject: [PATCH 05/26] Rational inverse --- .../src/distortion_function/rational.rs | 40 +++++++++++++++++-- 1 file changed, 37 insertions(+), 3 deletions(-) diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index 7ba946ea..9b739045 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -1,6 +1,6 @@ use super::DistortionFunction; -use cv_core::nalgebra::DimAdd; -use cv_core::nalgebra::DimSum; +use cv_core::nalgebra::{DimAdd, DimSum}; +use num_traits::Float; #[derive(Clone, Debug, Default)] struct Rational(P, Q) @@ -32,7 +32,41 @@ where (p / q, (dp * q - p * dq) / (q * q)) } + /// Numerically invert the function. + /// + /// # Method + /// + /// Starting with the function definition: + /// + /// $$ + /// y = \frac{P(x)}{Q(x)} + /// $$ + /// + /// We manipulate this into a root finding problem linear in $P$, $Q$ which + /// gives $y ⋅ Q(x) - P(x) = 0$. To solve this we use the Newton-Raphson + /// method with $x_0 = y$ and + /// + /// $$ + /// x_{i+1} = x_i - \frac{y ⋅ Q(x) - P(x)}{y ⋅ Q'(x) - P'(x)} + /// $$ + /// fn inverse(&self, value: f64) -> f64 { - todo!() + // Maxmimum number of iterations to use in Newton-Raphson inversion. + const MAX_ITERATIONS: usize = 100; + + // Convergence treshold for Newton-Raphson inversion. + const EPSILON: f64 = f64::EPSILON; + + let mut x = value; + for _ in 0..MAX_ITERATIONS { + let (p, dp) = self.0.with_derivative(x); + let (q, dq) = self.1.with_derivative(x); + let delta = (value * q - p) / (value * dq - dp); + x -= delta; + if Float::abs(delta) <= EPSILON * x { + break; + } + } + x } } From e20bfc0cabbe77cf95016efafa8e41b087c86ab6 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Mon, 2 Nov 2020 21:36:52 -0800 Subject: [PATCH 06/26] Parameters --- .../src/distortion_function/identity.rs | 16 ++++- cv-pinhole/src/distortion_function/mod.rs | 16 ++++- .../src/distortion_function/polynomial.rs | 69 +++++++++++++++++-- .../src/distortion_function/rational.rs | 31 ++++++--- 4 files changed, 110 insertions(+), 22 deletions(-) diff --git a/cv-pinhole/src/distortion_function/identity.rs b/cv-pinhole/src/distortion_function/identity.rs index 96610c69..404cbe75 100644 --- a/cv-pinhole/src/distortion_function/identity.rs +++ b/cv-pinhole/src/distortion_function/identity.rs @@ -1,6 +1,7 @@ use super::DistortionFunction; use cv_core::nalgebra::{ - allocator::Allocator, zero, ArrayStorage, DefaultAllocator, Dim, DimName, NamedDim, VectorN, U1, + allocator::Allocator, storage::Storage, zero, ArrayStorage, DefaultAllocator, Dim, DimName, + NamedDim, Vector, VectorN, U0, U1, }; /// Identity distortion, i.e. no distortion at all. @@ -8,7 +9,18 @@ use cv_core::nalgebra::{ struct Identity; impl DistortionFunction for Identity { - type NumParameters; + type NumParameters = U0; + + fn from_parameters(parameters: Vector) -> Self + where + S: Storage, + { + Self + } + + fn parameters(&self) -> VectorN { + VectorN::zeros_generic(U0, U1) + } fn evaluate(&self, value: f64) -> f64 { value diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs index 2aca4595..405d4b39 100644 --- a/cv-pinhole/src/distortion_function/mod.rs +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -1,9 +1,15 @@ +mod identity; mod polynomial; mod rational; -use cv_core::nalgebra::Dim; +use cv_core::nalgebra::{ + allocator::Allocator, storage::Storage, DefaultAllocator, Dim, Vector, VectorN, +}; -pub trait DistortionFunction { +pub trait DistortionFunction: Clone +where + DefaultAllocator: Allocator, +{ /// Type level number of parameters type NumParameters: Dim; @@ -22,5 +28,9 @@ pub trait DistortionFunction { fn inverse(&self, value: f64) -> f64; // TODO: Parameters, derivatives, Jacobians, etc for calibration - // fn parmaeters(&self) -> VectorN; + fn parameters(&self) -> VectorN; + + fn from_parameters(parameters: Vector) -> Self + where + S: Storage; } diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index ee24764e..842caeb1 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -1,21 +1,46 @@ +// TODO: Currently the polynomial is expressed in coefficient form, but for +// numerical evaluation other forms such as Chebyshev or Lagrange may be more +// accurate. + use super::DistortionFunction; use cv_core::nalgebra::{ - allocator::Allocator, zero, ArrayStorage, DefaultAllocator, Dim, DimName, NamedDim, VectorN, U1, + allocator::Allocator, storage::Storage, zero, ArrayStorage, DefaultAllocator, Dim, DimName, + NamedDim, Vector, VectorN, U1, }; +use num_traits::{Float, Zero}; -#[derive(Clone, Debug)] -struct Polynomial(VectorN) +#[derive(Clone, PartialEq, Debug)] +pub struct Polynomial(VectorN) where - Degree: Dim, DefaultAllocator: Allocator; -impl DistortionFunction for Polynomial +impl Default for Polynomial +where + DefaultAllocator: Allocator, + VectorN: Zero, +{ + fn default() -> Self { + Self(VectorN::zero()) + } +} + +impl DistortionFunction for Polynomial where - Degree: Dim, DefaultAllocator: Allocator, { type NumParameters = Degree; + fn from_parameters(parameters: Vector) -> Self + where + S: Storage, + { + Self(parameters.into_owned()) + } + + fn parameters(&self) -> VectorN { + self.0.clone() + } + fn evaluate(&self, value: f64) -> f64 { // Basic horner evaluation. // Ideally the compiler unrolls the loop and realizes that the first. @@ -32,6 +57,10 @@ where self.with_derivative(value).1 } + /// Simultaneously compute value and first derivative. + /// + /// # Method + /// fn with_derivative(&self, value: f64) -> (f64, f64) { let mut result = 0.0; let mut derivative = 0.0; @@ -44,7 +73,33 @@ where (result, derivative) } + /// Numerically invert the function. + /// + /// # Method + /// + /// We solve for a root of $P(x) - y$ using the Newton-Raphson method with + /// $x_0 = y$ and + /// + /// $$ + /// x_{i+1} = x_i - \frac{P(x) - value}{P'(x)} + /// $$ + /// fn inverse(&self, value: f64) -> f64 { - todo!() + // Maxmimum number of iterations to use in Newton-Raphson inversion. + const MAX_ITERATIONS: usize = 100; + + // Convergence treshold for Newton-Raphson inversion. + const EPSILON: f64 = f64::EPSILON; + + let mut x = value; + for _ in 0..MAX_ITERATIONS { + let (p, dp) = self.with_derivative(x); + let delta = (p - value) / dp; + x -= delta; + if Float::abs(delta) <= EPSILON * x { + break; + } + } + x } } diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index 9b739045..60505529 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -1,20 +1,20 @@ +use super::polynomial::Polynomial; use super::DistortionFunction; -use cv_core::nalgebra::{DimAdd, DimSum}; +use cv_core::nalgebra::{allocator::Allocator, DefaultAllocator, Dim, DimAdd, DimName, DimSum}; use num_traits::Float; -#[derive(Clone, Debug, Default)] -struct Rational(P, Q) +#[derive(Clone, PartialEq, Debug)] +struct Rational(Polynomial, Polynomial) where - P: DistortionFunction, - Q: DistortionFunction; + DefaultAllocator: Allocator, + DefaultAllocator: Allocator; -impl DistortionFunction for Rational +impl DistortionFunction for Rational where - P: DistortionFunction, - Q: DistortionFunction, - P::NumParameters: DimAdd, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, { - type NumParameters = DimSum; + type NumParameters = DP; fn evaluate(&self, value: f64) -> f64 { let p = self.0.evaluate(value); @@ -69,4 +69,15 @@ where } x } + + fn parameters(&self) -> nalgebra::VectorN { + todo!() + } + + fn from_parameters(parameters: nalgebra::Vector) -> Self + where + S: nalgebra::storage::Storage, + { + todo!() + } } From 890b5ecfa0db4a3c73be34ff123f95ce03560c7c Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Mon, 2 Nov 2020 22:09:11 -0800 Subject: [PATCH 07/26] Params for rational --- .../src/distortion_function/rational.rs | 38 ++++++++++++++----- 1 file changed, 29 insertions(+), 9 deletions(-) diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index 60505529..b231e295 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -1,20 +1,30 @@ use super::polynomial::Polynomial; use super::DistortionFunction; -use cv_core::nalgebra::{allocator::Allocator, DefaultAllocator, Dim, DimAdd, DimName, DimSum}; -use num_traits::Float; +use cv_core::nalgebra::{ + allocator::Allocator, storage::Storage, DefaultAllocator, Dim, DimAdd, DimName, DimSum, Vector, + VectorN, U1, +}; +use num_traits::{Float, Zero}; #[derive(Clone, PartialEq, Debug)] struct Rational(Polynomial, Polynomial) where + DP: DimAdd, DefaultAllocator: Allocator, - DefaultAllocator: Allocator; + DefaultAllocator: Allocator, + DefaultAllocator: Allocator>; impl DistortionFunction for Rational where + DP: DimName, + DQ: DimName, + DP: DimAdd, + DimSum: DimName, DefaultAllocator: Allocator, DefaultAllocator: Allocator, + DefaultAllocator: Allocator>, { - type NumParameters = DP; + type NumParameters = DimSum; fn evaluate(&self, value: f64) -> f64 { let p = self.0.evaluate(value); @@ -70,14 +80,24 @@ where x } - fn parameters(&self) -> nalgebra::VectorN { - todo!() + fn parameters(&self) -> VectorN { + let mut parameters = VectorN::::zero(); + parameters + .fixed_slice_mut::(0, 0) + .copy_from(&self.0.parameters()); + parameters + .fixed_slice_mut::(DP::dim(), 0) + .copy_from(&self.1.parameters()); + parameters } - fn from_parameters(parameters: nalgebra::Vector) -> Self + fn from_parameters(parameters: Vector) -> Self where - S: nalgebra::storage::Storage, + S: Storage, { - todo!() + Self( + Polynomial::from_parameters(parameters.fixed_slice::(0, 0)), + Polynomial::from_parameters(parameters.fixed_slice::(DP::dim(), 0)), + ) } } From ad20c85a72890de4a654d68ee51364a9e71887c5 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Tue, 3 Nov 2020 13:12:30 -0800 Subject: [PATCH 08/26] Gradient, stack/unstack --- .../src/distortion_function/identity.rs | 4 + cv-pinhole/src/distortion_function/mod.rs | 14 ++-- .../src/distortion_function/polynomial.rs | 4 + .../src/distortion_function/rational.rs | 81 ++++++++++++++++--- 4 files changed, 85 insertions(+), 18 deletions(-) diff --git a/cv-pinhole/src/distortion_function/identity.rs b/cv-pinhole/src/distortion_function/identity.rs index 404cbe75..fe4c80e8 100644 --- a/cv-pinhole/src/distortion_function/identity.rs +++ b/cv-pinhole/src/distortion_function/identity.rs @@ -33,4 +33,8 @@ impl DistortionFunction for Identity { fn inverse(&self, value: f64) -> f64 { value } + + fn gradient(&self, value: f64) -> VectorN { + todo!() + } } diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs index 405d4b39..c7325046 100644 --- a/cv-pinhole/src/distortion_function/mod.rs +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -13,6 +13,12 @@ where /// Type level number of parameters type NumParameters: Dim; + fn from_parameters(parameters: Vector) -> Self + where + S: Storage; + + fn parameters(&self) -> VectorN; + /// Undo distortion. fn evaluate(&self, value: f64) -> f64; @@ -27,10 +33,6 @@ where /// Apply distortion. fn inverse(&self, value: f64) -> f64; - // TODO: Parameters, derivatives, Jacobians, etc for calibration - fn parameters(&self) -> VectorN; - - fn from_parameters(parameters: Vector) -> Self - where - S: Storage; + /// Parameter gradient + fn gradient(&self, value: f64) -> VectorN; } diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index 842caeb1..b72e1b6f 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -102,4 +102,8 @@ where } x } + + fn gradient(&self, value: f64) -> VectorN { + todo!() + } } diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index b231e295..7216b916 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -1,8 +1,8 @@ use super::polynomial::Polynomial; use super::DistortionFunction; use cv_core::nalgebra::{ - allocator::Allocator, storage::Storage, DefaultAllocator, Dim, DimAdd, DimName, DimSum, Vector, - VectorN, U1, + allocator::Allocator, storage::Storage, DefaultAllocator, Dim, DimAdd, DimName, DimSum, + SliceStorage, Vector, VectorN, U1, }; use num_traits::{Float, Zero}; @@ -81,23 +81,80 @@ where } fn parameters(&self) -> VectorN { - let mut parameters = VectorN::::zero(); - parameters - .fixed_slice_mut::(0, 0) - .copy_from(&self.0.parameters()); - parameters - .fixed_slice_mut::(DP::dim(), 0) - .copy_from(&self.1.parameters()); - parameters + stack(self.0.parameters(), self.1.parameters()) } fn from_parameters(parameters: Vector) -> Self where S: Storage, { + let (pp, pq) = unstack(¶meters); Self( - Polynomial::from_parameters(parameters.fixed_slice::(0, 0)), - Polynomial::from_parameters(parameters.fixed_slice::(DP::dim(), 0)), + Polynomial::from_parameters(pp), + Polynomial::from_parameters(pq), ) } + + /// Parameter gradient + /// + /// # Method + /// + /// $$ + /// ∇_{\vec β​} \frac{P(x, \vec β​_p)}{Q(x, \vec β​_q)} + /// $$ + /// + /// $$ + /// \p{∇_{\vec β​} P(x, \vec β​_p)} ⋅ \frac 1{Q(x, \vec β​_q)} - + /// \p{∇_{\vec β​} Q(x, \vec β​_q)} ⋅ \frac{P(x, \vec β​_p)}{Q(x, \vec β​_q)^2} + /// $$ + /// + fn gradient(&self, value: f64) -> VectorN { + let p = self.0.evaluate(value); + let q = self.1.evaluate(value); + let mut dp = self.0.gradient(value); + let mut dq = self.1.gradient(value); + dp *= 1.0 / q; + dq *= -p / (q * q); + stack(dp, dq) + } +} + +fn stack( + vec1: Vector, + vec2: Vector, +) -> VectorN> +where + D1: DimName, + D2: DimName, + S1: Storage, + S2: Storage, + D1: DimAdd, + DimSum: DimName, + DefaultAllocator: Allocator>, +{ + let mut result = VectorN::>::zero(); + result.fixed_slice_mut::(0, 0).copy_from(&vec1); + result + .fixed_slice_mut::(D1::dim(), 0) + .copy_from(&vec2); + result +} + +fn unstack<'a, D1, D2, S>( + vector: &'a Vector, S>, +) -> ( + Vector>, + Vector>, +) +where + D1: DimName, + D2: DimName, + S: Storage>, + D1: DimAdd, + DimSum: DimName, +{ + ( + vector.fixed_slice::(0, 0), + vector.fixed_slice::(D1::dim(), 0), + ) } From 0e1f7652571f71049a729de046cc2a2eebaf9136 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Tue, 3 Nov 2020 23:00:15 -0800 Subject: [PATCH 09/26] Gradients in polynomial --- cv-pinhole/src/distortion_function/polynomial.rs | 7 ++++++- cv-pinhole/src/distortion_function/rational.rs | 13 ++++++++----- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index b72e1b6f..cc3015a2 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -104,6 +104,11 @@ where } fn gradient(&self, value: f64) -> VectorN { - todo!() + let mut factor = 1.0; + VectorN::from_fn_generic(self.0.data.shape().0, U1, move |_, _| { + let coefficient = factor; + factor *= value; + coefficient + }) } } diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index 7216b916..af2669d1 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -111,14 +111,15 @@ where fn gradient(&self, value: f64) -> VectorN { let p = self.0.evaluate(value); let q = self.1.evaluate(value); - let mut dp = self.0.gradient(value); - let mut dq = self.1.gradient(value); - dp *= 1.0 / q; - dq *= -p / (q * q); - stack(dp, dq) + stack( + self.0.gradient(value) * (1.0 / q), + self.1.gradient(value) * (-p / (q * q)), + ) } } +/// Stack two statically sized nalgebra vectors. Allocates a new vector for the +/// result on the stack. fn stack( vec1: Vector, vec2: Vector, @@ -140,6 +141,8 @@ where result } +/// Split a statically sized nalgebra vector in two. Returns vectors that reference +/// slices of the input vector. fn unstack<'a, D1, D2, S>( vector: &'a Vector, S>, ) -> ( From 03a4560dcf96dc19a7a49bac571e0270e6644926 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Wed, 4 Nov 2020 13:10:12 -0800 Subject: [PATCH 10/26] Camera trait bounds --- cv-pinhole/src/camera.rs | 44 ++++++++++++++++++++++++++++------------ 1 file changed, 31 insertions(+), 13 deletions(-) diff --git a/cv-pinhole/src/camera.rs b/cv-pinhole/src/camera.rs index a7c499be..743f8174 100644 --- a/cv-pinhole/src/camera.rs +++ b/cv-pinhole/src/camera.rs @@ -2,7 +2,7 @@ use crate::CameraIntrinsics; use crate::DistortionFunction; use crate::NormalizedKeyPoint; -use cv_core::nalgebra::{Matrix2, Vector2}; +use cv_core::nalgebra::{allocator::Allocator, DefaultAllocator, Dim, Matrix2, Vector2}; use cv_core::{CameraModel, ImagePoint, KeyPoint}; /// Camera with distortion. @@ -40,11 +40,17 @@ use cv_core::{CameraModel, ImagePoint, KeyPoint}; /// #[derive(Clone, PartialEq, PartialOrd, Debug)] // #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct Camera +struct Camera where - R: DistortionFunction, - T: DistortionFunction, - P: DistortionFunction, + R: DistortionFunction, + T: DistortionFunction, + P: DistortionFunction, + D1: Dim, + D2: Dim, + D3: Dim, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, { linear: CameraIntrinsics, radial_distortion: R, @@ -53,11 +59,17 @@ where prism_distortion: [P; 2], } -impl Camera +impl Camera where - R: DistortionFunction, - T: DistortionFunction, - P: DistortionFunction, + R: DistortionFunction, + T: DistortionFunction, + P: DistortionFunction, + D1: Dim, + D2: Dim, + D3: Dim, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, { pub fn new(linear: CameraIntrinsics, radial_distortion: R, tangential_distortion: T) -> Self { todo!() @@ -125,11 +137,17 @@ where } } -impl CameraModel for Camera +impl CameraModel for Camera where - R: DistortionFunction, - T: DistortionFunction, - P: DistortionFunction, + R: DistortionFunction, + T: DistortionFunction, + P: DistortionFunction, + D1: Dim, + D2: Dim, + D3: Dim, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, { type Projection = NormalizedKeyPoint; From 2f0b0adc6a1f92b6ded75b5114f843a4c062b434 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Wed, 4 Nov 2020 13:11:56 -0800 Subject: [PATCH 11/26] Simplify trait constraints --- cv-pinhole/src/camera.rs | 51 +++++++++++++++++----------------------- 1 file changed, 21 insertions(+), 30 deletions(-) diff --git a/cv-pinhole/src/camera.rs b/cv-pinhole/src/camera.rs index 743f8174..e573b1f1 100644 --- a/cv-pinhole/src/camera.rs +++ b/cv-pinhole/src/camera.rs @@ -40,17 +40,14 @@ use cv_core::{CameraModel, ImagePoint, KeyPoint}; /// #[derive(Clone, PartialEq, PartialOrd, Debug)] // #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct Camera +struct Camera where - R: DistortionFunction, - T: DistortionFunction, - P: DistortionFunction, - D1: Dim, - D2: Dim, - D3: Dim, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, + R: DistortionFunction, + T: DistortionFunction, + P: DistortionFunction, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, { linear: CameraIntrinsics, radial_distortion: R, @@ -59,17 +56,14 @@ where prism_distortion: [P; 2], } -impl Camera +impl Camera where - R: DistortionFunction, - T: DistortionFunction, - P: DistortionFunction, - D1: Dim, - D2: Dim, - D3: Dim, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, + R: DistortionFunction, + T: DistortionFunction, + P: DistortionFunction, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, { pub fn new(linear: CameraIntrinsics, radial_distortion: R, tangential_distortion: T) -> Self { todo!() @@ -137,17 +131,14 @@ where } } -impl CameraModel for Camera +impl CameraModel for Camera where - R: DistortionFunction, - T: DistortionFunction, - P: DistortionFunction, - D1: Dim, - D2: Dim, - D3: Dim, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, + R: DistortionFunction, + T: DistortionFunction, + P: DistortionFunction, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, { type Projection = NormalizedKeyPoint; From 324ce9c5a751f53f3269baa953e14ad258fd7381 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Wed, 18 Nov 2020 21:14:51 -0800 Subject: [PATCH 12/26] Camera and tests --- .cargo/katex-header.html | 2 +- cv-pinhole/Cargo.toml | 5 + cv-pinhole/src/camera.rs | 306 +++++++++++++++--- .../src/distortion_function/even_poly_2.rs | 150 --------- .../distortion_function/even_rational_6_6.rs | 158 --------- cv-pinhole/src/distortion_function/fisheye.rs | 111 +++++++ .../src/distortion_function/identity.rs | 8 +- cv-pinhole/src/distortion_function/mod.rs | 52 ++- .../src/distortion_function/polynomial.rs | 73 +++++ .../src/distortion_function/rational.rs | 91 +++++- cv-pinhole/src/lib.rs | 6 +- 11 files changed, 602 insertions(+), 360 deletions(-) delete mode 100644 cv-pinhole/src/distortion_function/even_poly_2.rs delete mode 100644 cv-pinhole/src/distortion_function/even_rational_6_6.rs create mode 100644 cv-pinhole/src/distortion_function/fisheye.rs diff --git a/.cargo/katex-header.html b/.cargo/katex-header.html index ca63b907..8ab6e93d 100644 --- a/.cargo/katex-header.html +++ b/.cargo/katex-header.html @@ -30,7 +30,7 @@ \gdef\intersection{\cup} \gdef\comp#1{\overline{#1}} \gdef\powerset#1{\mathcal{P}\p{#1}} - \gdef\d{\operatorname{d}\\!} + \gdef\d{\operatorname{d}\!} `, { macros }); renderMathInElement(document.body, { delimiters: [ diff --git a/cv-pinhole/Cargo.toml b/cv-pinhole/Cargo.toml index 3d6df6fb..99d07734 100644 --- a/cv-pinhole/Cargo.toml +++ b/cv-pinhole/Cargo.toml @@ -27,6 +27,11 @@ nalgebra = { version = "0.22.0", default-features = false } [dev-dependencies] cv-geom = { version = "0.7.0", path = "../cv-geom" } +criterion = "0.3.3" +proptest = "0.10.1" +static_assertions = "1.1.0" +float_eq = "0.5.0" + [package.metadata.docs.rs] all-features = true rustdoc-args = ["--html-in-header", ".cargo/katex-header.html"] diff --git a/cv-pinhole/src/camera.rs b/cv-pinhole/src/camera.rs index e573b1f1..05d88e1a 100644 --- a/cv-pinhole/src/camera.rs +++ b/cv-pinhole/src/camera.rs @@ -1,86 +1,114 @@ +use crate::distortion_function::DistortionFunction; use crate::CameraIntrinsics; -use crate::DistortionFunction; use crate::NormalizedKeyPoint; use cv_core::nalgebra::{allocator::Allocator, DefaultAllocator, Dim, Matrix2, Vector2}; use cv_core::{CameraModel, ImagePoint, KeyPoint}; -/// Camera with distortion. +/// Realistic camera with distortions. /// -/// The distortion model is compatible with OpenCV. +/// Implements a realistic camera model with [optical distortions](https://en.wikipedia.org/wiki/Distortion_(optics)): /// -/// Given image coordinates $(x,y)$ and $r = x^2 + y^2$ the undistorted coordinates $(x', y')$ are: +/// * *Radial distortion* models imperfect lens shapes. +/// * *Decentering distortion* models lenses being off-centered of the optical axis. +/// * *Thin prism distortion* models lenses not being orthogonal to optical axis. +/// +/// In particular it implements the [Brown-Conrady][b71] distortion model with added thin prism +/// correction as in [Weng][w92], comparable to the model in [OpenCV][o45]. Instead of a fixed +/// number of coefficients, the camera model uses generic [`DistortionFunction`]s, provided as a type +/// parameter. +/// +/// Given image coordinates $(x,y)$ and $r = x^2 + y^2$ the undistorted coordinates $(x', y')$ are +/// computed as: /// /// $$ -/// \begin{bmatrix} x' \\\\ y' \end{bmatrix} = -/// \vec f \p{\begin{bmatrix} x \\\\ y \end{bmatrix}} = \begin{bmatrix} -/// x ⋅ f_r(r^2) + \p{2⋅x⋅\p{t_1 ⋅ x + t_2 ⋅ y} + t_1 ⋅ r^2} ⋅ f_t(r^2) + f_{px}(r^2) -/// \\\\[.8em] -/// y ⋅ f_r(r^2) + \p{2⋅y⋅\p{t_1 ⋅ x + t_2 ⋅ y} + t_2 ⋅ r^2} ⋅ f_t(r^2) + f_{py}(r^2) +/// \begin{bmatrix} x' \\\\ y' \end{bmatrix} = \begin{bmatrix} +/// x ⋅ f_r(r^2, \vec θ_r) + \p{2⋅x⋅\p{t_1 ⋅ x + t_2 ⋅ y} + t_1 ⋅ r^2} ⋅ f_t(r^2, \vec θ_d) + f_{p}(r^2, \vec θ_{x}) +/// \\\\ +/// y ⋅ f_r(r^2, \vec θ_r) + \p{2⋅y⋅\p{t_1 ⋅ x + t_2 ⋅ y} + t_2 ⋅ r^2} ⋅ f_t(r^2, \vec θ_d) + f_{p}(r^2, \vec θ_{y}) /// \end{bmatrix} /// $$ /// -/// where $f_r$ is the *radial* distortion function, $(t_1, t_2, f_t)$ specify the *tangential* distortion (See [Brown][b66] p. 454) and $(f_{px}, f_{py})$ the *thin prism* distortion. +/// where $f_r$ is the radial distortion function of type `R`, $(t_1, t_2, f_t)$ specify the +/// decentering distortion, with $f_t$ of type `D` and $(f_{px}, f_{py})$ specify the thin prism +/// of type `T`. /// -/// **Note.** The tangential distortion compensates for lenses not being entirely parallel with the -/// image plane. Despite what the name may suggest, its contribution is not orthogonal to the -/// radial distortion. +/// The parameter vector of the distortion is +/// +/// $$ +/// \vec θ = \begin{bmatrix} \vec θ_r \\\\ t_1 \\\\ t_2 \\\\ \vec θ_d \\\\ \vec θ_x \\\\ \vec θ_y \end{bmatrix} +/// $$ /// /// # References /// -/// * D.C. Brown (1996). Decentering Distortion of Lenses. [online][b66]. +/// * D.C. Brown (1996). Decentering Distortion of Lenses. [pdf][b66]. +/// * D.C. Brown (1971). Close-Range Camera Calibration. [pdf][b71]. +/// * C. Stachniss (2020). Camera Parameters - Extrinsics and Intrinsics. [lecture video][s20]. +/// * S. Chen, H. Jin, J. Chien, E.Chan, D. Goldman (2010). Adobe Camera Model. [pdf][a10]. +/// * J. Weng (1992). Camera Calibration with Distortion Models and Accuracy Evaluation. [pdf][w92]. /// /// [b66]: https://web.archive.org/web/20180312205006/https://www.asprs.org/wp-content/uploads/pers/1966journal/may/1966_may_444-462.pdf +/// [b71]: https://www.asprs.org/wp-content/uploads/pers/1971journal/aug/1971_aug_855-866.pdf +/// [s20]: https://youtu.be/uHApDqH-8UE?t=2643 +/// [a10]: http://download.macromedia.com/pub/labs/lensprofile_creator/lensprofile_creator_cameramodel.pdf +/// [w92]: https://www.cs.auckland.ac.nz/courses/compsci773s1c/lectures/camera%20distortion.pdf +/// [o45]: https://docs.opencv.org/4.5.0/d9/d0c/group__calib3d.html /// +/// # To do /// -/// https://medium.com/analytics-vidhya/camera-calibration-with-opencv-f324679c6eb7 -/// -/// https://spectrogrism.readthedocs.io/en/latest/distortions.html -/// +/// * Chromatic abberation. +/// * Vigneting correction. +/// * Support [Tilt/Scheimpflug](https://en.wikipedia.org/wiki/Scheimpflug_principle) lenses. See [Louhichi et al.](https://iopscience.iop.org/article/10.1088/0957-0233/18/8/037) for a mathematical model. /// #[derive(Clone, PartialEq, PartialOrd, Debug)] // #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct Camera +pub struct Camera where R: DistortionFunction, + D: DistortionFunction, T: DistortionFunction, - P: DistortionFunction, DefaultAllocator: Allocator, + DefaultAllocator: Allocator, DefaultAllocator: Allocator, - DefaultAllocator: Allocator, { linear: CameraIntrinsics, radial_distortion: R, tangential: [f64; 2], - tangential_distortion: T, - prism_distortion: [P; 2], + tangential_distortion: D, + prism_distortion: [T; 2], } -impl Camera +impl Camera where R: DistortionFunction, + D: DistortionFunction, T: DistortionFunction, - P: DistortionFunction, DefaultAllocator: Allocator, + DefaultAllocator: Allocator, DefaultAllocator: Allocator, - DefaultAllocator: Allocator, { - pub fn new(linear: CameraIntrinsics, radial_distortion: R, tangential_distortion: T) -> Self { - todo!() - // Self { - // linear, - // radial_distortion, - // tangential_distortion, - // } + pub fn new( + linear: CameraIntrinsics, + radial_distortion: R, + tangential_distortion: D, + prism_distortion: [T; 2], + ) -> Self { + Self { + linear, + radial_distortion, + tangential: [0.0, 0.0], + tangential_distortion, + prism_distortion, + } } /// Computes $\vec f(\mathtt{point})$. pub fn correct(&self, point: Vector2) -> Vector2 { let r2 = point.norm_squared(); let f_r = self.radial_distortion.evaluate(r2); - let f_t = self.tangential_distortion.evaluate(r2); - let f_px = self.prism_distortion[0].evaluate(r2) - 1.0; - let f_py = self.prism_distortion[1].evaluate(r2) - 1.0; + let f_t = 1.0; + let f_px = self.prism_distortion[0].evaluate(r2); + let f_py = self.prism_distortion[1].evaluate(r2); let t = self.tangential[0] * point.x + self.tangential[1] * point.y; let t_x = 2.0 * point.x * t + self.tangential[0] * r2; let t_y = 2.0 * point.y * t + self.tangential[1] * r2; @@ -90,7 +118,7 @@ where ) } - /// Computes $\mathbf{J}_{\vec f}(\mathtt{point})$. + /// Computes $∇_{\vec x} \vec f\p{\vec x, \vec θ}$. /// /// $$ /// \mathbf{J}_{\vec f} = \begin{bmatrix} @@ -161,3 +189,207 @@ where self.linear.uncalibrate(distorted).into() } } + +pub mod camera { + use super::Camera; + use crate::distortion_function::{Identity, Polynomial, Rational}; + use cv_core::nalgebra::{U1, U2, U3, U4}; + + /// OpenCV Camera model with 4 parameters + pub type OpenCV4 = Camera, Identity, Identity>; + + pub type OpenCV5 = Camera, Identity, Identity>; + + pub type OpenCV8 = Camera, Identity, Identity>; + + pub type OpenCV12 = Camera, Identity, Polynomial>; +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::distortion_function::{Identity, Polynomial, Rational}; + use cv_core::nalgebra::{Point2, Vector3, VectorN, U1, U2, U3, U4, U8}; + + #[test] + fn camera_1() { + // Calibration parameters for a GoPro Hero 6 using OpenCV + let focal: [f64; 2] = [1.77950719e+0, 1.77867606e+03]; + let center: [f64; 2] = [2.03258212e+03, 1.52051932e+03]; + #[rustfmt::skip] + let distortion: [f64; 12] = [ + 2.56740016e+01, 1.52496764e+01, -5.01712057e-04, + 1.09310463e-03, 6.72953083e-01, 2.59544797e+01, + 2.24213453e+01, 3.04318306e+00, -3.23278793e-03, + 9.53176056e-05, -9.35687185e-05, 2.96341863e-05]; + // The OpenCV + let distorted = [1718.3195, 1858.1052]; + let undistorted = [-0.17996894, 0.19362147]; + + let mut intrinsic = CameraIntrinsics::identity(); + intrinsic.focals = Vector2::new(focal[0], focal[1]); + intrinsic.principal_point = Point2::new(center[0], center[1]); + #[rustfmt::skip] + let radial = Rational::::from_parameters( + VectorN::::from_column_slice_generic(U8, U1, &[ + distortion[4], distortion[1], distortion[0], 1.0, + distortion[7], distortion[6], distortion[5], 1.0, + ]) + ); + let tangential = Identity; + let prism = [ + Polynomial::::from_parameters(Vector3::new(distortion[9], distortion[8], 0.0)), + Polynomial::::from_parameters(Vector3::new(distortion[11], distortion[10], 0.0)), + ]; + + let camera = camera::OpenCV12::new(intrinsic, radial, tangential, prism); + + let distorted = Vector2::new(1718.3195, 1858.1052); + let undistorted = camera.correct(distorted); + let expected = Vector2::new(-0.17996894, 0.19362147); + println!("{:#?}", undistorted); + + let distorted = Vector2::new(579.8596, 2575.0476); + let undistorted = camera.correct(distorted); + let expected = Vector2::new(-1.1464612, 0.8380858); + println!("{:#?}", undistorted); + + let source = [ + [-0.17996894, 0.19362147], + [-0.17968875, 0.24255648], + [-0.17944576, 0.29271868], + [-0.17916603, 0.34418374], + [-0.17893367, 0.39675304], + [-0.17863423, 0.45061454], + [-0.1782976, 0.5059651], + [-0.17797716, 0.56267387], + [-0.17769286, 0.62078154], + [-0.22691241, 0.1944314], + [-0.22716342, 0.24451172], + [-0.22748885, 0.29591268], + [-0.22788039, 0.3486148], + [-0.22820546, 0.4025184], + [-0.22855103, 0.4577957], + [-0.22891289, 0.51458186], + [-0.22923958, 0.572837], + [-0.22955456, 0.6325643], + [-0.2760499, 0.19522506], + [-0.27696946, 0.24657097], + [-0.27798527, 0.29921785], + [-0.27897915, 0.35328886], + [-0.27998206, 0.40862617], + [-0.28099003, 0.46533334], + [-0.28205746, 0.5236744], + [-0.2831492, 0.5835057], + [-0.28426304, 0.6449713], + [-0.3276202, 0.19608246], + [-0.3292424, 0.24866305], + [-0.3309349, 0.3026873], + [-0.33262616, 0.3581513], + [-0.33433127, 0.4149485], + [-0.33617917, 0.4732646], + [-0.33799866, 0.5331862], + [-0.33992943, 0.5947946], + [-0.34184524, 0.65808254], + [-0.38177538, 0.19692989], + [-0.38411814, 0.25095174], + [-0.3866268, 0.30635145], + [-0.38909492, 0.36327213], + [-0.3916721, 0.42171457], + [-0.39424962, 0.48160818], + [-0.39702195, 0.5432604], + [-0.39976382, 0.6066253], + [-0.40267637, 0.671886], + [-0.43867132, 0.19787468], + [-0.44195008, 0.2532662], + [-0.44519594, 0.31016332], + [-0.44858345, 0.3686112], + [-0.45198414, 0.42872244], + [-0.45556828, 0.49036437], + [-0.45910528, 0.55382824], + [-0.46299413, 0.6192026], + [-0.466843, 0.6863503], + [-0.49861795, 0.1987457], + [-0.5026725, 0.2557629], + [-0.5069558, 0.31417185], + [-0.51119965, 0.37434888], + [-0.5157122, 0.43618685], + [-0.5201786, 0.49963355], + [-0.5250113, 0.56500995], + [-0.529766, 0.63237023], + [-0.53485453, 0.70181614], + [-0.56156373, 0.19966456], + [-0.5667678, 0.25814596], + [-0.5719744, 0.31840706], + [-0.57744604, 0.38031867], + [-0.5828779, 0.4440139], + [-0.58866763, 0.50954485], + [-0.59443516, 0.5769114], + [-0.6006511, 0.64649254], + [-0.60675824, 0.71801007], + [-0.628279, 0.20040697], + [-0.6344018, 0.26069504], + [-0.64092815, 0.32267714], + [-0.6474141, 0.38664073], + [-0.6542724, 0.4522714], + [-0.6610637, 0.51989216], + [-0.6683662, 0.5895391], + [-0.6756135, 0.66126966], + [-0.6833462, 0.7354248], + [-0.6985042, 0.20141184], + [-0.7060343, 0.2632588], + [-0.71366554, 0.3273413], + [-0.7216309, 0.39327696], + [-0.7295911, 0.46094468], + [-0.7380902, 0.53080666], + [-0.7465825, 0.6027301], + [-0.7556527, 0.6770891], + [-0.7646206, 0.75370055], + [-0.7728095, 0.20244533], + [-0.78165317, 0.26625928], + [-0.7907952, 0.33220664], + [-0.8001839, 0.40018782], + [-0.8098858, 0.4701321], + [-0.8196281, 0.54215276], + [-0.83001417, 0.6167809], + [-0.8402761, 0.6934916], + [-0.8512289, 0.7733144], + [-0.8518392, 0.20366336], + [-0.862248, 0.26939756], + [-0.8729417, 0.33734974], + [-0.8838656, 0.40754512], + [-0.89505607, 0.4797963], + [-0.90671587, 0.5545365], + [-0.9185119, 0.63145286], + [-0.93089086, 0.71136534], + [-0.9433173, 0.7935529], + [-0.93565977, 0.20484428], + [-0.9476607, 0.27270162], + [-0.9600662, 0.3428219], + [-0.97279584, 0.41510573], + [-0.9859218, 0.49006754], + [-0.9992419, 0.56712496], + [-1.0130187, 0.6472003], + [-1.0271673, 0.72976106], + [-1.0417787, 0.8154801], + [-1.02476, 0.20601285], + [-1.038625, 0.27609447], + [-1.0529088, 0.34840867], + [-1.0676082, 0.42343187], + [-1.0825946, 0.50079226], + [-1.0980939, 0.5808948], + [-1.1138377, 0.6636289], + [-1.1300658, 0.7495205], + [-1.1464612, 0.8380858], + ]; + for i in 0..source.len() { + let [x, y] = source[i]; + let source = Vector2::new(x, y); + let destination = camera.correct(source); + let (x, y) = (destination[0], destination[1]); + let x = x * focal[0] + center[0]; + let y = y * focal[1] + center[1]; + println!("[{}, {}],", x, y); + } + } +} diff --git a/cv-pinhole/src/distortion_function/even_poly_2.rs b/cv-pinhole/src/distortion_function/even_poly_2.rs deleted file mode 100644 index 20609aac..00000000 --- a/cv-pinhole/src/distortion_function/even_poly_2.rs +++ /dev/null @@ -1,150 +0,0 @@ -use super::DistortionFunction; -use num_traits::Float; - -/// Maxmimum number of iterations to use in Newton-Raphson inversion. -const MAX_ITERATIONS: usize = 100; - -/// Convergence treshold for Newton-Raphson inversion. -const EPSILON: f64 = f64::EPSILON; - -/// Even $(6,6)$-rational distortion function -/// -/// The radial distortion is calibrated by scaling with a degree $(6,6)$ [even](https://en.wikipedia.org/wiki/Even_and_odd_functions#Even_functions) [polynomial](https://en.wikipedia.org/wiki/Polynomial) function: -/// -/// $$ -/// r' = r ⋅ \p{1 + k_1 ⋅ r^2} -/// $$ -/// -#[derive(Clone, PartialEq, Default, Debug)] -pub struct EvenPoly2(f64); - -impl EvenPoly2 {} - -impl DistortionFunction for EvenPoly2 { - /// Given $r$ compute $r'$. - #[rustfmt::skip] - fn evaluate(&self, r: f64) -> f64 { - let r2 = r * r; - r * (1.0 + self.0 * r2) - } - - /// Given $r'$ compute $r$. - /// - /// # Examples - /// - /// ``` - /// use cv_pinhole::RadialDistortion; - /// - /// let distortion = RadialDistortion([0.1, 0.2, 0.3, 0.4, 0.2, 0.1]); - /// let camera_r = 1.3; - /// let corrected_r = distortion.calibrate(camera_r); - /// let reprojected_r = distortion.uncalibrate(corrected_r); - /// assert_eq!(camera_r, reprojected_r); - /// ``` - /// - /// # Method - /// - /// Given $r'$ solve for $r$. Start with the known relation - /// - /// - /// $$ - /// r' = r ⋅ \p{1 + k_1 ⋅ r^2} - /// $$ - /// - /// manipulate the rational relation into a polynomial - /// - /// $$ - /// \begin{aligned} - /// r' ⋅ \p{1 + k_4 ⋅ r^2 + k_5 ⋅ r^4 + k_6 ⋅ r^6} - /// &= r ⋅ \p{1 + k_1 ⋅ r^2 + k_2 ⋅ r^4 + k_3 ⋅ r^6} \\\\ - /// r' + r' ⋅ k_4 ⋅ r^2 + r' ⋅ k_5 ⋅ r^4 + r' ⋅ k_6 ⋅ r^6 - /// &= r + k_1 ⋅ r^3 + k_2 ⋅ r^5 + k_3 ⋅ r^7 \\\\ - /// \end{aligned} - /// $$ - /// - /// $$ - /// r' + r' ⋅ k_4 ⋅ r^2 + r' ⋅ k_5 ⋅ r^4 + r' ⋅ k_6 ⋅ r^6 - /// \- r - k_1 ⋅ r^3 - k_2 ⋅ r^5 - k_3 ⋅ r^7 = 0 - /// $$ - /// - /// That is, we need to find the root of the 7th-order polynomial with coefficients: - /// - /// $$ - /// P(r) = - /// \begin{bmatrix} - /// r' & -1 & r' ⋅ k_4 & -k_1 & r' ⋅ k_5 & -k_2 & r' ⋅ k_6 & - k_3 - /// \end{bmatrix}^T ⋅ V(r) - /// $$ - /// - /// where $V(r) = \begin{bmatrix} 1 & r & r^2 & ᠁ \end{bmatrix}$ is the [Vandermonde matrix](https://en.wikipedia.org/wiki/Vandermonde_matrix). The coefficients of the 6ht-order derivative polynomials are: - /// - /// $$ - /// P'(r) = - /// \begin{bmatrix} - /// -1 & 2 ⋅ r' ⋅ k_4 & -3 ⋅ k_1 & 4 ⋅ r' ⋅ k_5 & -5 ⋅ k_2 & 6 ⋅ r' ⋅ k_6 & - 7 ⋅ k_3 - /// \end{bmatrix}^T ⋅ V(r) - /// $$ - /// - /// Using $r'$ as the starting value we can approximate $r'$ using [Newton's method](https://en.wikipedia.org/wiki/Newton%27s_method): - /// - /// $$ - /// \begin{aligned} - /// r_0 &= r' & r_{i+1} = r_i - \frac{P(r_i)}{P'(r_i)} - /// \end{aligned} - /// $$ - /// - /// **Note.** We could also produce higher derivatives and use [Halley's method](https://en.wikipedia.org/wiki/Halley%27s_method) or higher order [Housholder methods](https://en.wikipedia.org/wiki/Householder%27s_method). - /// - /// The inversion is approximated using at most [`MAX_ITERATIONS`] rounds of Newton-Raphson - /// or when the $\abs{r_{i+1} - r_i}$ is less than [`EPSILON`] times $r_{i+1}$. - #[rustfmt::skip] - fn inverse(&self, mut r: f64) -> f64 { - todo!() - // let p = [ - // r, -1.0, - // r * self.0[3], -self.0[0], - // r * self.0[4], -self.0[1], - // r * self.0[5], -self.0[2], - // ]; - // // Iterate Newtons method - // for _ in 0..MAX_ITERATIONS { - // let mut value = p[7]; - // value *= r; value += p[6]; - // value *= r; value += p[5]; - // value *= r; value += p[4]; - // value *= r; value += p[3]; - // value *= r; value += p[2]; - // value *= r; value += p[1]; - // value *= r; value += p[0]; - // let mut deriv = p[7] * 7.0; - // deriv *= r; deriv += p[6] * 6.0; - // deriv *= r; deriv += p[5] * 5.0; - // deriv *= r; deriv += p[4] * 4.0; - // deriv *= r; deriv += p[3] * 3.0; - // deriv *= r; deriv += p[2] * 2.0; - // deriv *= r; deriv += p[1]; - // let delta = value / deriv; - // r -= delta; - // if Float::abs(delta) <= EPSILON * r { - // break; - // } - // } - // r - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_invert() { - let distortion = RadialDistortion([0.1, 0.2, 0.3, 0.4, 0.2, 0.1]); - let test_values = [0.0, 0.5, 1.0, 1.5]; - for &camera_r in &test_values { - let corrected = distortion.calibrate(camera_r); - let reprojected = distortion.uncalibrate(corrected); - assert_eq!(camera_r, reprojected); - } - } -} diff --git a/cv-pinhole/src/distortion_function/even_rational_6_6.rs b/cv-pinhole/src/distortion_function/even_rational_6_6.rs deleted file mode 100644 index 98b08ac2..00000000 --- a/cv-pinhole/src/distortion_function/even_rational_6_6.rs +++ /dev/null @@ -1,158 +0,0 @@ -use super::DistortionFunction; -use num_traits::Float; - -/// Maxmimum number of iterations to use in Newton-Raphson inversion. -const MAX_ITERATIONS: usize = 100; - -/// Convergence treshold for Newton-Raphson inversion. -const EPSILON: f64 = f64::EPSILON; - -/// Even $(6,6)$-rational distortion function -/// -/// The radial distortion is calibrated by scaling with a degree $(6,6)$ [even](https://en.wikipedia.org/wiki/Even_and_odd_functions#Even_functions) [rational](https://en.wikipedia.org/wiki/Rational_function) function: -/// -/// $$ -/// r' = r ⋅ \frac{1 + k_1 ⋅ r^2 + k_2 ⋅ r^4 + k_3 ⋅ r^6}{1 + k_4 ⋅ r^2 + k_5 ⋅ r^4 + k_6 ⋅ r^6} -/// $$ -/// -#[derive(Clone, PartialEq, Default, Debug)] -pub struct EvenRational66(pub [f64; 6]); - -impl EvenRational66 {} - -impl DistortionFunction for EvenRational66 { - /// Given $r$ compute $r'$. - #[rustfmt::skip] - fn evaluate(&self, r: f64) -> f64 { - let r2 = r * r; - let mut p = self.0[2]; - p *= r2; p += self.0[1]; - p *= r2; p += self.0[0]; - p *= r2; p += 1.0; - let mut q = self.0[5]; - q *= r2; q += self.0[4]; - q *= r2; q += self.0[3]; - q *= r2; q += 1.0; - r * p / q - } - - /// Given $r'$ compute $r$. - /// - /// # Examples - /// - /// ``` - /// use cv_pinhole::RadialDistortion; - /// - /// let distortion = RadialDistortion([0.1, 0.2, 0.3, 0.4, 0.2, 0.1]); - /// let camera_r = 1.3; - /// let corrected_r = distortion.calibrate(camera_r); - /// let reprojected_r = distortion.uncalibrate(corrected_r); - /// assert_eq!(camera_r, reprojected_r); - /// ``` - /// - /// # Method - /// - /// Given $r'$ solve for $r$. Start with the known relation - /// - /// - /// $$ - /// r' = r ⋅ \frac{1 + k_1 ⋅ r^2 + k_2 ⋅ r^4 + k_3 ⋅ r^6} - /// {1 + k_4 ⋅ r^2 + k_5 ⋅ r^4 + k_6 ⋅ r^6} - /// $$ - /// - /// manipulate the rational relation into a polynomial - /// - /// $$ - /// \begin{aligned} - /// r' ⋅ \p{1 + k_4 ⋅ r^2 + k_5 ⋅ r^4 + k_6 ⋅ r^6} - /// &= r ⋅ \p{1 + k_1 ⋅ r^2 + k_2 ⋅ r^4 + k_3 ⋅ r^6} \\\\ - /// r' + r' ⋅ k_4 ⋅ r^2 + r' ⋅ k_5 ⋅ r^4 + r' ⋅ k_6 ⋅ r^6 - /// &= r + k_1 ⋅ r^3 + k_2 ⋅ r^5 + k_3 ⋅ r^7 \\\\ - /// \end{aligned} - /// $$ - /// - /// $$ - /// r' + r' ⋅ k_4 ⋅ r^2 + r' ⋅ k_5 ⋅ r^4 + r' ⋅ k_6 ⋅ r^6 - /// \- r - k_1 ⋅ r^3 - k_2 ⋅ r^5 - k_3 ⋅ r^7 = 0 - /// $$ - /// - /// That is, we need to find the root of the 7th-order polynomial with coefficients: - /// - /// $$ - /// P(r) = - /// \begin{bmatrix} - /// r' & -1 & r' ⋅ k_4 & -k_1 & r' ⋅ k_5 & -k_2 & r' ⋅ k_6 & - k_3 - /// \end{bmatrix}^T ⋅ V(r) - /// $$ - /// - /// where $V(r) = \begin{bmatrix} 1 & r & r^2 & ᠁ \end{bmatrix}$ is the [Vandermonde matrix](https://en.wikipedia.org/wiki/Vandermonde_matrix). The coefficients of the 6ht-order derivative polynomials are: - /// - /// $$ - /// P'(r) = - /// \begin{bmatrix} - /// -1 & 2 ⋅ r' ⋅ k_4 & -3 ⋅ k_1 & 4 ⋅ r' ⋅ k_5 & -5 ⋅ k_2 & 6 ⋅ r' ⋅ k_6 & - 7 ⋅ k_3 - /// \end{bmatrix}^T ⋅ V(r) - /// $$ - /// - /// Using $r'$ as the starting value we can approximate $r'$ using [Newton's method](https://en.wikipedia.org/wiki/Newton%27s_method): - /// - /// $$ - /// \begin{aligned} - /// r_0 &= r' & r_{i+1} = r_i - \frac{P(r_i)}{P'(r_i)} - /// \end{aligned} - /// $$ - /// - /// **Note.** We could also produce higher derivatives and use [Halley's method](https://en.wikipedia.org/wiki/Halley%27s_method) or higher order [Housholder methods](https://en.wikipedia.org/wiki/Householder%27s_method). - /// - /// The inversion is approximated using at most [`MAX_ITERATIONS`] rounds of Newton-Raphson - /// or when the $\abs{r_{i+1} - r_i}$ is less than [`EPSILON`] times $r_{i+1}$. - #[rustfmt::skip] - fn inverse(&self, mut r: f64) -> f64 { - let p = [ - r, -1.0, - r * self.0[3], -self.0[0], - r * self.0[4], -self.0[1], - r * self.0[5], -self.0[2], - ]; - // Iterate Newtons method - for _ in 0..MAX_ITERATIONS { - let mut value = p[7]; - value *= r; value += p[6]; - value *= r; value += p[5]; - value *= r; value += p[4]; - value *= r; value += p[3]; - value *= r; value += p[2]; - value *= r; value += p[1]; - value *= r; value += p[0]; - let mut deriv = p[7] * 7.0; - deriv *= r; deriv += p[6] * 6.0; - deriv *= r; deriv += p[5] * 5.0; - deriv *= r; deriv += p[4] * 4.0; - deriv *= r; deriv += p[3] * 3.0; - deriv *= r; deriv += p[2] * 2.0; - deriv *= r; deriv += p[1]; - let delta = value / deriv; - r -= delta; - if Float::abs(delta) <= EPSILON * r { - break; - } - } - r - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_invert() { - let distortion = RadialDistortion([0.1, 0.2, 0.3, 0.4, 0.2, 0.1]); - let test_values = [0.0, 0.5, 1.0, 1.5]; - for &camera_r in &test_values { - let corrected = distortion.calibrate(camera_r); - let reprojected = distortion.uncalibrate(corrected); - assert_eq!(camera_r, reprojected); - } - } -} diff --git a/cv-pinhole/src/distortion_function/fisheye.rs b/cv-pinhole/src/distortion_function/fisheye.rs new file mode 100644 index 00000000..4955decc --- /dev/null +++ b/cv-pinhole/src/distortion_function/fisheye.rs @@ -0,0 +1,111 @@ +use super::DistortionFunction; +use cv_core::nalgebra::{ + allocator::Allocator, storage::Storage, zero, ArrayStorage, DefaultAllocator, Dim, DimName, + NamedDim, Vector, Vector1, VectorN, U1, +}; +use num_traits::{Float, Zero}; + +/// Parametric fisheye radial distortion function. +/// +/// Implement non-rectilinear (i.e. fisheye) projections. Perhaps using the Fisheye Factor from +/// +/// $$ +/// R = \begin{cases} +/// \frac {f}{k} ⋅ \tan \p{k ⋅ θ} & 0 < k ≤ 1 \\\\ +/// f ⋅ θ & k = 0 \\\\ +/// \frac {f}{k} ⋅\sin \p{k ⋅ θ} & -1 ≤ k < 0 \\\\ +/// \end{cases} +/// $$ +/// +/// Varying $k ∈ [-1, 1]$ will gradually transform from rectilinear to orthographic projection. +/// +/// substituting $θ = \arctan r$: +/// +/// $$ +/// \frac R r = \frac f r ⋅ \begin{cases} +/// \frac {1}{k} ⋅ \tan \p{k ⋅ \arctan r} & 0 < k ≤ 1 \\\\ +/// \arctan r & k = 0 \\\\ +/// \frac {1}{k} ⋅\sin \p{k ⋅ \arctan r} & -1 ≤ k < 0 \\\\ +/// \end{cases} +/// $$ +/// +/// $$ +/// \frac R r = \frac f r ⋅ \begin{cases} +/// r & k = 1 \\\\ +/// \frac {1}{k} ⋅ \tan \p{k ⋅ \arctan r} & \frac 12 < k < 1 \\\\ +/// \frac {1}{k} ⋅ \tan \p{k ⋅ \arctan r} & k = \frac 12 \\\\ +/// \frac {1}{k} ⋅ \tan \p{k ⋅ \arctan r} & 0 < k < \frac 12 \\\\ +/// \arctan r & k = 0 \\\\ +/// \frac {1}{k} ⋅\sin \p{k ⋅ \arctan r} & -1 ≤ k < 0 \\\\ +/// \end{cases} +/// $$ +/// +/// We can factor this as $R = f ⋅ f(θ, k)$ where the function $f$ is a distortion function: +/// +/// $$ +/// f(x, \vec β) = \begin{cases} +/// \frac {1}{β_0} ⋅ \tan \p{β_0 ⋅ x} & 0 < β_0 ≤ 1 \\\\ +/// x & β_0 = 0 \\\\ +/// \frac {1}{β_0} ⋅\sin \p{β_0 ⋅ x} & -1 ≤ β_0 < 0 \\\\ +/// \end{cases} +/// $$ +/// +/// +/// # References +/// +/// * Wikipedia Fisheye Lens. [][wiki] +/// * Lensfun documentation. +/// * PTGui v11 Support question 3.28. [link][ptgui]. +/// * Panotools wiki. [link][panotools]. +/// +/// [wiki]: https://en.wikipedia.org/wiki/Fisheye_lens +/// [opencv]: https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html +/// [lensfun]: https://lensfun.github.io/manual/latest/corrections.html +/// [ptgui]: https://www.ptgui.com/support.html#3_28 +/// [panotools]: https://wiki.panotools.org/Fisheye_Projection +/// +/// +#[derive(Clone, PartialEq, Debug)] +pub struct Fisheye(f64); + +impl Default for Fisheye { + fn default() -> Self { + Fisheye(1.0) + } +} + +impl DistortionFunction for Fisheye { + type NumParameters = U1; + + fn from_parameters(parameters: Vector) -> Self + where + S: Storage, + { + Self(parameters[0]) + } + + fn parameters(&self) -> VectorN { + Vector1::new(self.0) + } + + fn evaluate(&self, value: f64) -> f64 { + todo!() + } + + fn derivative(&self, value: f64) -> f64 { + self.with_derivative(value).1 + } + + /// Simultaneously compute value and first derivative. + fn with_derivative(&self, value: f64) -> (f64, f64) { + todo!() + } + + fn inverse(&self, value: f64) -> f64 { + todo!() + } + + fn gradient(&self, value: f64) -> VectorN { + todo!() + } +} diff --git a/cv-pinhole/src/distortion_function/identity.rs b/cv-pinhole/src/distortion_function/identity.rs index fe4c80e8..3a20fb8f 100644 --- a/cv-pinhole/src/distortion_function/identity.rs +++ b/cv-pinhole/src/distortion_function/identity.rs @@ -5,8 +5,14 @@ use cv_core::nalgebra::{ }; /// Identity distortion, i.e. no distortion at all. +/// +/// $$ +/// f(x) = x +/// $$ +/// +/// Refresh, maybe? #[derive(Clone, PartialEq, Default, Debug)] -struct Identity; +pub struct Identity; impl DistortionFunction for Identity { type NumParameters = U0; diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs index c7325046..c7b6667e 100644 --- a/cv-pinhole/src/distortion_function/mod.rs +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -1,3 +1,4 @@ +mod fisheye; mod identity; mod polynomial; mod rational; @@ -6,33 +7,72 @@ use cv_core::nalgebra::{ allocator::Allocator, storage::Storage, DefaultAllocator, Dim, Vector, VectorN, }; +// Re-exports +#[doc(inline)] +pub use identity::Identity; + +#[doc(inline)] +pub use polynomial::Polynomial; + +#[doc(inline)] +pub use rational::Rational; + +#[doc(inline)] +pub use fisheye::Fisheye; + +/// Trait for parameterized functions specifying 1D distortions. +/// +/// $$ +/// y = f(x, \vec β) +/// $$ +/// +/// Provides evaluations, inverse, derivative and derivative with respect to +/// parameters. +/// +/// The function $f$ is assumed to be monotonic. +/// +/// # To do +/// +/// * Generalize to arbitrary input/output dimensions. +/// pub trait DistortionFunction: Clone where DefaultAllocator: Allocator, { - /// Type level number of parameters + /// The number of parameters, $\dim \vec β$ as a nalgebra type level integer. + /// + /// # To do + /// + /// * Make this [`DimName`](cv_core::nalgebra::DimName) or provide a method + /// to retrieve the dynamic value. type NumParameters: Dim; + /// Create a new instance from parameters $\vec β$. fn from_parameters(parameters: Vector) -> Self where S: Storage; + /// Get function parameters $\vec β$. fn parameters(&self) -> VectorN; - /// Undo distortion. + /// Evaluate $f(\mathtt{value}, \vec β)$. fn evaluate(&self, value: f64) -> f64; - /// Evaluate with derivative + /// Evaluate the derivative $f'(\mathtt{value}, \vec β)$ where $f' = \frac{\d}{\d x} f$. fn derivative(&self, value: f64) -> f64; - /// Evaluate with derivative + /// Simultaneously evaluate function and its derivative. + /// + /// The default implementation combines the results from + /// [`Self::evaluate`] and [`Self::derivative`]. When it is more efficient + /// to evaluate them together this function can be implemented. fn with_derivative(&self, value: f64) -> (f64, f64) { (self.evaluate(value), self.derivative(value)) } - /// Apply distortion. + /// Evaluate the inverse $f^{-1}(\mathtt{value}, \vec β)$. fn inverse(&self, value: f64) -> f64; - /// Parameter gradient + /// Parameter gradient $∇_{\vec β​} f(\mathtt{value}, \vec β)$. fn gradient(&self, value: f64) -> VectorN; } diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index cc3015a2..6bb99ee6 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -9,6 +9,16 @@ use cv_core::nalgebra::{ }; use num_traits::{Float, Zero}; +/// Polynomial distortion function +/// +/// $$ +/// f(x, \vec β) = β_0 + β_1 ​⋅ x + β_2 ​⋅ x^2 + ⋯ + β_n ⋅ x^n +/// $$ +/// +/// # To do +/// +/// * (Bug) Parameters are currently in reverse order. +/// #[derive(Clone, PartialEq, Debug)] pub struct Polynomial(VectorN) where @@ -112,3 +122,66 @@ where }) } } + +#[cfg(test)] +mod tests { + use super::*; + use cv_core::nalgebra::{Vector4, U4}; + use float_eq::assert_float_eq; + use proptest::prelude::*; + + #[rustfmt::skip] + fn polynomial_1() -> Polynomial { + Polynomial::from_parameters( + Vector4::new( + 0.6729530830603175, 15.249676433312018, 25.67400161236561, 1.0, + ) + ) + } + + #[test] + fn test_evaluate_1() { + let radial = polynomial_1(); + let result = radial.evaluate(0.06987809296337355); + assert_float_eq!(result, 2.86874326561548, ulps <= 0); + } + + #[test] + fn test_inverse_1() { + let radial = polynomial_1(); + let result = radial.inverse(2.86874326561548); + assert_float_eq!(result, 0.06987809296337355, ulps <= 0); + } + + #[test] + fn test_finite_difference_1() { + let h = f64::EPSILON.powf(1.0 / 3.0); + let radial = polynomial_1(); + proptest!(|(r in 0.0..2.0)| { + let h = f64::max(h * 0.1, h * r); + let deriv = radial.derivative(r); + let approx = (radial.evaluate(r + h) - radial.evaluate(r - h)) / (2.0 * h); + assert_float_eq!(deriv, approx, rmax <= 1e2 * h * h); + }); + } + + #[test] + fn test_roundtrip_forward_1() { + let radial = polynomial_1(); + proptest!(|(r in 0.0..2.0)| { + let eval = radial.evaluate(r); + let inv = radial.inverse(eval); + assert_float_eq!(inv, r, rmax <= 1e2 * f64::EPSILON); + }); + } + + #[test] + fn test_roundtrip_reverse_1() { + let radial = polynomial_1(); + proptest!(|(r in 0.0..2.0)| { + let inv = radial.inverse(r); + let eval = radial.evaluate(inv); + assert_float_eq!(eval, r, rmax <= 1e4 * f64::EPSILON); + }); + } +} diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index af2669d1..e5d50766 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -6,8 +6,20 @@ use cv_core::nalgebra::{ }; use num_traits::{Float, Zero}; +/// Rational distortion function. +/// +/// $$ +/// f(x, \vec β) = \frac{ +/// β_0 + β_1 ​⋅ x + β_2 ​⋅ x^2 + ⋯ + β_n ⋅ x^n}{ +/// β_{n+1} + β_{n+2} ​⋅ x + β_{n+3} ​⋅ x^2 + ⋯ + β_{n+1 +m} ⋅ x^{m} +/// } +/// $$ +/// +/// Where $n = \mathtt{DP} - 1$ and $m = \mathtt{DQ} - 1$. +/// +/// #[derive(Clone, PartialEq, Debug)] -struct Rational(Polynomial, Polynomial) +pub struct Rational(Polynomial, Polynomial) where DP: DimAdd, DefaultAllocator: Allocator, @@ -60,15 +72,20 @@ where /// x_{i+1} = x_i - \frac{y ⋅ Q(x) - P(x)}{y ⋅ Q'(x) - P'(x)} /// $$ /// + /// # To do + /// + /// * Improve numerical precision. Currently the Newton-Raphson can oscillate + /// or diverge. + /// fn inverse(&self, value: f64) -> f64 { // Maxmimum number of iterations to use in Newton-Raphson inversion. - const MAX_ITERATIONS: usize = 100; + const MAX_ITERATIONS: usize = 20; - // Convergence treshold for Newton-Raphson inversion. + // Convergence threshold for Newton-Raphson inversion. const EPSILON: f64 = f64::EPSILON; let mut x = value; - for _ in 0..MAX_ITERATIONS { + for _i in 0..MAX_ITERATIONS { let (p, dp) = self.0.with_derivative(x); let (q, dq) = self.1.with_derivative(x); let delta = (value * q - p) / (value * dq - dp); @@ -161,3 +178,69 @@ where vector.fixed_slice::(D1::dim(), 0), ) } + +#[cfg(test)] +mod tests { + use super::*; + use cv_core::nalgebra::{VectorN, U4, U8}; + use float_eq::assert_float_eq; + use proptest::prelude::*; + + /// OpenCV12 radial distortion for a GoPro Hero 6 + #[rustfmt::skip] + fn rational_1() -> Rational { + Rational::from_parameters( + VectorN::from_column_slice_generic(U8, U1, &[ + 0.6729530830603175, 15.249676433312018, 25.67400161236561, 1.0, + 3.0431830552169914, 22.421345314744390, 25.95447969279203, 1.0, + ]) + ) + } + + #[test] + fn test_evaluate_1() { + let radial = rational_1(); + let result = radial.evaluate(0.06987809296337355); + assert_float_eq!(result, 0.9810452524397972, ulps <= 0); + } + + #[test] + fn test_inverse_1() { + let radial = rational_1(); + let result = radial.inverse(0.9810452524397972); + assert_float_eq!(result, 0.06987809296337355, ulps <= 8); + } + + #[test] + fn test_finite_difference_1() { + let h = f64::EPSILON.powf(1.0 / 3.0); + let radial = rational_1(); + proptest!(|(r in 0.0..2.0)| { + let h = f64::max(h * 0.1, h * r); + let deriv = radial.derivative(r); + let approx = (radial.evaluate(r + h) - radial.evaluate(r - h)) / (2.0 * h); + assert_float_eq!(deriv, approx, rmax <= 1e4 * h * h); + }); + } + + #[test] + fn test_roundtrip_forward_1() { + let radial = rational_1(); + proptest!(|(r in 0.0..2.0)| { + let eval = radial.evaluate(r); + dbg!(r, eval); + let inv = radial.inverse(eval); + assert_float_eq!(inv, r, abs <= 1e8 * f64::EPSILON); + }); + } + + #[test] + fn test_roundtrip_reverse_1() { + let radial = rational_1(); + proptest!(|(r in 0.0..2.0)| { + let inv = radial.inverse(r); + let eval = radial.evaluate(inv); + assert_float_eq!(eval, r, ulps <= 150); + }); + } +} diff --git a/cv-pinhole/src/lib.rs b/cv-pinhole/src/lib.rs index 2f0ec5a0..0cb9577a 100644 --- a/cv-pinhole/src/lib.rs +++ b/cv-pinhole/src/lib.rs @@ -3,16 +3,16 @@ //! the light came from that hit that pixel. It can also be used to convert backwards from the 3d back to the 2d //! using the `uncalibrate` method from the [`cv_core::CameraModel`] trait. -#![no_std] +// #![cfg_attr(not(test), no_std)] #[cfg(feature = "alloc")] extern crate alloc; mod camera; -mod distortion_function; +pub mod distortion_function; mod essential; -pub use distortion_function::DistortionFunction; +pub use camera::Camera; pub use essential::*; use cv_core::nalgebra::{Matrix3, Point2, Point3, Vector2, Vector3}; From 4c28a122ff7a82f9cb0adb4b81a9401a6367631c Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Thu, 19 Nov 2020 17:08:20 -0800 Subject: [PATCH 13/26] Fix polynnomial coefficient order --- .../src/distortion_function/polynomial.rs | 23 +++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index 6bb99ee6..921d4038 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -4,8 +4,7 @@ use super::DistortionFunction; use cv_core::nalgebra::{ - allocator::Allocator, storage::Storage, zero, ArrayStorage, DefaultAllocator, Dim, DimName, - NamedDim, Vector, VectorN, U1, + allocator::Allocator, storage::Storage, DefaultAllocator, Dim, Vector, VectorN, U1, }; use num_traits::{Float, Zero}; @@ -56,9 +55,9 @@ where // Ideally the compiler unrolls the loop and realizes that the first. // multiplication is redundant. let mut result = 0.0; - for &coefficient in self.0.iter() { + for i in (0..self.0.nrows()).rev() { result *= value; - result += coefficient; + result += self.0[i]; } result } @@ -74,11 +73,11 @@ where fn with_derivative(&self, value: f64) -> (f64, f64) { let mut result = 0.0; let mut derivative = 0.0; - for &coefficient in self.0.iter() { + for i in (0..self.0.nrows()).rev() { derivative *= value; derivative += result; result *= value; - result += coefficient; + result += self.0[i]; } (result, derivative) } @@ -130,13 +129,13 @@ mod tests { use float_eq::assert_float_eq; use proptest::prelude::*; - #[rustfmt::skip] fn polynomial_1() -> Polynomial { - Polynomial::from_parameters( - Vector4::new( - 0.6729530830603175, 15.249676433312018, 25.67400161236561, 1.0, - ) - ) + Polynomial::from_parameters(Vector4::new( + 1.0, + 25.67400161236561, + 15.249676433312018, + 0.6729530830603175, + )) } #[test] From 8652352b643db8386c59f20c3470c573578ad079 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Thu, 19 Nov 2020 17:09:09 -0800 Subject: [PATCH 14/26] Fix polynomial test --- cv-pinhole/src/distortion_function/polynomial.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index 921d4038..2ec36ce6 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -170,7 +170,7 @@ mod tests { proptest!(|(r in 0.0..2.0)| { let eval = radial.evaluate(r); let inv = radial.inverse(eval); - assert_float_eq!(inv, r, rmax <= 1e2 * f64::EPSILON); + assert_float_eq!(inv, r, rmax <= 1e4 * f64::EPSILON); }); } From ba88ac7b63ce0db79b4a961e78c97e1c30ffd838 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Sun, 22 Nov 2020 00:12:24 -0800 Subject: [PATCH 15/26] Newton-Bisection --- cv-pinhole/src/distortion_function/mod.rs | 68 ++++++++++++++++++- .../src/distortion_function/polynomial.rs | 34 +--------- .../src/distortion_function/rational.rs | 61 ++++++----------- 3 files changed, 90 insertions(+), 73 deletions(-) diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs index c7b6667e..f3a40c66 100644 --- a/cv-pinhole/src/distortion_function/mod.rs +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -71,7 +71,73 @@ where } /// Evaluate the inverse $f^{-1}(\mathtt{value}, \vec β)$. - fn inverse(&self, value: f64) -> f64; + /// + /// # Method + /// + /// Uses a Newton-Bisection hybrid method based on [1] that is guaranteed to + /// converge to almost machine precision. + /// + /// # Resources + /// + /// Numerical Recipes 2nd edition. p. 365 + /// + /// + /// + /// # Panics + /// + /// Panics when an inverse does not exist in the range $x \in [0, 3]$. + /// + fn inverse(&self, value: f64) -> f64 { + let (mut xl, mut xh) = (0.0, 3.0); + let fl = self.evaluate(xl) - value; + if fl == 0.0 { + return xl; + } + let fh = self.evaluate(xh) - value; + if fh == 0.0 { + return xh; + } + if fl * fh > 0.0 { + panic!("Inverse outside of bracket [0, 3]."); + } + if fl > 0.0 { + std::mem::swap(&mut xl, &mut xh); + } + let mut rts = 0.5 * (xl + xh); + let mut dxold = (xl - xh).abs(); + let mut dx = dxold; + let (mut f, mut df) = self.with_derivative(rts); + loop { + if (((rts - xh) * df - f) * ((rts - xl) * df - f) > 0.0) + || (2.0 * f.abs() > (dxold * df).abs()) + { + // Bisection step + dxold = dx; + dx = 0.5 * (xh - xl); + rts = xl + dx; + if xl == rts || xh == rts { + return rts; + } + } else { + // Newton step + dxold = dx; + dx = f / df; + let tmp = rts; + rts -= dx; + if tmp == rts { + return rts; + } + } + let (nf, ndf) = self.with_derivative(rts); + f = nf - value; + df = ndf; + if f < 0.0 { + xl = rts; + } else { + xh = rts; + } + } + } /// Parameter gradient $∇_{\vec β​} f(\mathtt{value}, \vec β)$. fn gradient(&self, value: f64) -> VectorN; diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index 2ec36ce6..db1a64b3 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -82,36 +82,6 @@ where (result, derivative) } - /// Numerically invert the function. - /// - /// # Method - /// - /// We solve for a root of $P(x) - y$ using the Newton-Raphson method with - /// $x_0 = y$ and - /// - /// $$ - /// x_{i+1} = x_i - \frac{P(x) - value}{P'(x)} - /// $$ - /// - fn inverse(&self, value: f64) -> f64 { - // Maxmimum number of iterations to use in Newton-Raphson inversion. - const MAX_ITERATIONS: usize = 100; - - // Convergence treshold for Newton-Raphson inversion. - const EPSILON: f64 = f64::EPSILON; - - let mut x = value; - for _ in 0..MAX_ITERATIONS { - let (p, dp) = self.with_derivative(x); - let delta = (p - value) / dp; - x -= delta; - if Float::abs(delta) <= EPSILON * x { - break; - } - } - x - } - fn gradient(&self, value: f64) -> VectorN { let mut factor = 1.0; VectorN::from_fn_generic(self.0.data.shape().0, U1, move |_, _| { @@ -177,10 +147,12 @@ mod tests { #[test] fn test_roundtrip_reverse_1() { let radial = polynomial_1(); - proptest!(|(r in 0.0..2.0)| { + proptest!(|(r in 1.0..2.0)| { let inv = radial.inverse(r); let eval = radial.evaluate(inv); assert_float_eq!(eval, r, rmax <= 1e4 * f64::EPSILON); }); } + + // TODO: Test parameter gradient using finite differences. } diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index e5d50766..85c6e90f 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -17,6 +17,8 @@ use num_traits::{Float, Zero}; /// /// Where $n = \mathtt{DP} - 1$ and $m = \mathtt{DQ} - 1$. /// +/// See [`impl DistortionFunction`](#impl-DistortionFunction). +/// /// #[derive(Clone, PartialEq, Debug)] pub struct Rational(Polynomial, Polynomial) @@ -48,12 +50,6 @@ where self.with_derivative(value).1 } - fn with_derivative(&self, value: f64) -> (f64, f64) { - let (p, dp) = self.0.with_derivative(value); - let (q, dq) = self.1.with_derivative(value); - (p / q, (dp * q - p * dq) / (q * q)) - } - /// Numerically invert the function. /// /// # Method @@ -64,37 +60,18 @@ where /// y = \frac{P(x)}{Q(x)} /// $$ /// - /// We manipulate this into a root finding problem linear in $P$, $Q$ which - /// gives $y ⋅ Q(x) - P(x) = 0$. To solve this we use the Newton-Raphson - /// method with $x_0 = y$ and - /// /// $$ - /// x_{i+1} = x_i - \frac{y ⋅ Q(x) - P(x)}{y ⋅ Q'(x) - P'(x)} + /// \frac{P'(x)}{Q(x)} - \frac{P(x)}{Q(x)^2} ⋅ Q'(x) /// $$ /// - /// # To do - /// - /// * Improve numerical precision. Currently the Newton-Raphson can oscillate - /// or diverge. + /// $$ + /// \frac{P'(x) ⋅ Q(x) - P(x) ⋅ Q'(x)}{Q(x)^2} + /// $$ /// - fn inverse(&self, value: f64) -> f64 { - // Maxmimum number of iterations to use in Newton-Raphson inversion. - const MAX_ITERATIONS: usize = 20; - - // Convergence threshold for Newton-Raphson inversion. - const EPSILON: f64 = f64::EPSILON; - - let mut x = value; - for _i in 0..MAX_ITERATIONS { - let (p, dp) = self.0.with_derivative(x); - let (q, dq) = self.1.with_derivative(x); - let delta = (value * q - p) / (value * dq - dp); - x -= delta; - if Float::abs(delta) <= EPSILON * x { - break; - } - } - x + fn with_derivative(&self, value: f64) -> (f64, f64) { + let (p, dp) = self.0.with_derivative(value); + let (q, dq) = self.1.with_derivative(value); + (p / q, (dp * q - p * dq) / (q * q)) } fn parameters(&self) -> VectorN { @@ -191,8 +168,8 @@ mod tests { fn rational_1() -> Rational { Rational::from_parameters( VectorN::from_column_slice_generic(U8, U1, &[ - 0.6729530830603175, 15.249676433312018, 25.67400161236561, 1.0, - 3.0431830552169914, 22.421345314744390, 25.95447969279203, 1.0, + 1.0, 25.67400161236561, 15.249676433312018, 0.6729530830603175, + 1.0, 25.95447969279203, 22.421345314744390, 3.0431830552169914, ]) ) } @@ -200,7 +177,7 @@ mod tests { #[test] fn test_evaluate_1() { let radial = rational_1(); - let result = radial.evaluate(0.06987809296337355); + let result = radial.evaluate(0.06987809296337322); assert_float_eq!(result, 0.9810452524397972, ulps <= 0); } @@ -208,7 +185,7 @@ mod tests { fn test_inverse_1() { let radial = rational_1(); let result = radial.inverse(0.9810452524397972); - assert_float_eq!(result, 0.06987809296337355, ulps <= 8); + assert_float_eq!(result, 0.06987809296337322, ulps <= 8); } #[test] @@ -228,19 +205,21 @@ mod tests { let radial = rational_1(); proptest!(|(r in 0.0..2.0)| { let eval = radial.evaluate(r); - dbg!(r, eval); let inv = radial.inverse(eval); - assert_float_eq!(inv, r, abs <= 1e8 * f64::EPSILON); + let eval2 = radial.evaluate(inv); + assert_float_eq!(eval, eval2, rmax <= 3.0 * f64::EPSILON); }); } #[test] fn test_roundtrip_reverse_1() { let radial = rational_1(); - proptest!(|(r in 0.0..2.0)| { + proptest!(|(r in 0.7..1.0)| { let inv = radial.inverse(r); let eval = radial.evaluate(inv); - assert_float_eq!(eval, r, ulps <= 150); + assert_float_eq!(eval, r, rmax <= 3.0 * f64::EPSILON); }); } + + // TODO: Test parameter gradient using finite differences. } From 526fd6255d97111f853aaa949e62755f979b3519 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Sun, 22 Nov 2020 13:57:53 -0800 Subject: [PATCH 16/26] Implement Fisheye --- cv-pinhole/src/distortion_function/fisheye.rs | 99 ++++++++++++++++--- 1 file changed, 86 insertions(+), 13 deletions(-) diff --git a/cv-pinhole/src/distortion_function/fisheye.rs b/cv-pinhole/src/distortion_function/fisheye.rs index 4955decc..9b7f92ed 100644 --- a/cv-pinhole/src/distortion_function/fisheye.rs +++ b/cv-pinhole/src/distortion_function/fisheye.rs @@ -1,9 +1,5 @@ use super::DistortionFunction; -use cv_core::nalgebra::{ - allocator::Allocator, storage::Storage, zero, ArrayStorage, DefaultAllocator, Dim, DimName, - NamedDim, Vector, Vector1, VectorN, U1, -}; -use num_traits::{Float, Zero}; +use cv_core::nalgebra::{storage::Storage, Vector, Vector1, VectorN, U1}; /// Parametric fisheye radial distortion function. /// @@ -89,23 +85,100 @@ impl DistortionFunction for Fisheye { } fn evaluate(&self, value: f64) -> f64 { - todo!() + match self.0 { + k if k < 0.0 => (k * value.atan()).sin() / k, + k if k == 0.0 => value.atan(), + k if k < 1.0 => (k * value.atan()).tan() / k, + _ => value, + } } fn derivative(&self, value: f64) -> f64 { - self.with_derivative(value).1 + match self.0 { + k if k < 0.0 => (k * value.atan()).cos() / (1.0 + value.powi(2)), + k if k == 0.0 => 1.0 / (1.0 + value.powi(2)), + k if k < 1.0 => (k * value.atan()).cos().powi(-2) / (1.0 + value.powi(2)), + _ => 1.0, + } } - /// Simultaneously compute value and first derivative. - fn with_derivative(&self, value: f64) -> (f64, f64) { - todo!() + fn inverse(&self, value: f64) -> f64 { + match self.0 { + k if k < 0.0 => ((k * value).asin() / k).tan(), + k if k == 0.0 => value.tan(), + k if k < 1.0 => ((k * value).atan() / k).tan(), + _ => value, + } } - fn inverse(&self, value: f64) -> f64 { + fn gradient(&self, _value: f64) -> VectorN { todo!() } +} - fn gradient(&self, value: f64) -> VectorN { - todo!() +#[cfg(test)] +mod tests { + use super::*; + use cv_core::nalgebra::Vector1; + use float_eq::assert_float_eq; + use proptest::prelude::*; + + #[test] + fn test_finite_difference() { + let h = f64::EPSILON.powf(1.0 / 3.0); + let test = |k, r| { + let fisheye = Fisheye::from_parameters(Vector1::new(k)); + let h = f64::max(h * 0.1, h * r); + let deriv = fisheye.derivative(r); + let approx = (fisheye.evaluate(r + h) - fisheye.evaluate(r - h)) / (2.0 * h); + assert_float_eq!(deriv, approx, rmax <= 2e2 * h * h); + }; + proptest!(|(k in -1.0..1.0, r in 0.0..1.0)| { + test(k, r); + }); + proptest!(|(r in 0.0..1.0)| { + test(0.0, r); + }); + proptest!(|(r in 0.0..1.0)| { + test(1.0, r); + }); + } + + #[test] + fn test_roundtrip_forward() { + let test = |k, r| { + let fisheye = Fisheye::from_parameters(Vector1::new(k)); + let eval = fisheye.evaluate(r); + let inv = fisheye.inverse(eval); + assert_float_eq!(inv, r, rmax <= 1e1 * f64::EPSILON); + }; + proptest!(|(k in -1.0..1.0, r in 0.0..2.0)| { + test(k, r); + }); + proptest!(|(r in 0.0..1.0)| { + test(0.0, r); + }); + proptest!(|(r in 0.0..1.0)| { + test(1.0, r); + }); + } + + #[test] + fn test_roundtrip_reverse() { + let test = |k, r| { + let fisheye = Fisheye::from_parameters(Vector1::new(k)); + let inv = fisheye.inverse(r); + let eval = fisheye.evaluate(inv); + assert_float_eq!(eval, r, rmax <= 1e1 * f64::EPSILON); + }; + proptest!(|(k in -1.0..1.0, r in 0.0..0.75)| { + test(k, r); + }); + proptest!(|(k in -1.0..1.0, r in 0.0..0.75)| { + test(0.0, r); + }); + proptest!(|(k in -1.0..1.0, r in 0.0..0.75)| { + test(1.0, r); + }); } } From 79a368249f52ab1658ed540ac43aecbd998de65a Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Sun, 22 Nov 2020 14:53:45 -0800 Subject: [PATCH 17/26] Document Fisheye --- cv-pinhole/src/distortion_function/fisheye.rs | 55 +++++-------------- 1 file changed, 14 insertions(+), 41 deletions(-) diff --git a/cv-pinhole/src/distortion_function/fisheye.rs b/cv-pinhole/src/distortion_function/fisheye.rs index 9b7f92ed..b8612f88 100644 --- a/cv-pinhole/src/distortion_function/fisheye.rs +++ b/cv-pinhole/src/distortion_function/fisheye.rs @@ -3,58 +3,29 @@ use cv_core::nalgebra::{storage::Storage, Vector, Vector1, VectorN, U1}; /// Parametric fisheye radial distortion function. /// -/// Implement non-rectilinear (i.e. fisheye) projections. Perhaps using the Fisheye Factor from +/// Implements various projection functions using the generalized relation with a single +/// parameter $k$. /// /// $$ -/// R = \begin{cases} -/// \frac {f}{k} ⋅ \tan \p{k ⋅ θ} & 0 < k ≤ 1 \\\\ -/// f ⋅ θ & k = 0 \\\\ -/// \frac {f}{k} ⋅\sin \p{k ⋅ θ} & -1 ≤ k < 0 \\\\ -/// \end{cases} -/// $$ -/// -/// Varying $k ∈ [-1, 1]$ will gradually transform from rectilinear to orthographic projection. -/// -/// substituting $θ = \arctan r$: -/// -/// $$ -/// \frac R r = \frac f r ⋅ \begin{cases} -/// \frac {1}{k} ⋅ \tan \p{k ⋅ \arctan r} & 0 < k ≤ 1 \\\\ -/// \arctan r & k = 0 \\\\ -/// \frac {1}{k} ⋅\sin \p{k ⋅ \arctan r} & -1 ≤ k < 0 \\\\ -/// \end{cases} -/// $$ -/// -/// $$ -/// \frac R r = \frac f r ⋅ \begin{cases} -/// r & k = 1 \\\\ -/// \frac {1}{k} ⋅ \tan \p{k ⋅ \arctan r} & \frac 12 < k < 1 \\\\ -/// \frac {1}{k} ⋅ \tan \p{k ⋅ \arctan r} & k = \frac 12 \\\\ -/// \frac {1}{k} ⋅ \tan \p{k ⋅ \arctan r} & 0 < k < \frac 12 \\\\ -/// \arctan r & k = 0 \\\\ -/// \frac {1}{k} ⋅\sin \p{k ⋅ \arctan r} & -1 ≤ k < 0 \\\\ -/// \end{cases} -/// $$ -/// -/// We can factor this as $R = f ⋅ f(θ, k)$ where the function $f$ is a distortion function: -/// -/// $$ -/// f(x, \vec β) = \begin{cases} -/// \frac {1}{β_0} ⋅ \tan \p{β_0 ⋅ x} & 0 < β_0 ≤ 1 \\\\ -/// x & β_0 = 0 \\\\ -/// \frac {1}{β_0} ⋅\sin \p{β_0 ⋅ x} & -1 ≤ β_0 < 0 \\\\ +/// r' = \\begin{cases} +/// \sin \p{k ⋅ \arctan r} ⋅ \frac 1 k & k < 0 \\\\ +/// \arctan r & k = 0 \\\\ +/// \tan \p{k ⋅ \arctan r} ⋅ \frac 1 k & k > 0 \\\\ /// \end{cases} /// $$ /// +/// Varying $k ∈ [-1, 1]$ will gradually transform from orthographic to rectilinear projection. In +/// particular for $k = 1$ the equation simplifies to $r' = r$ representing the non-Fisheye +/// rectilinear projection. /// /// # References /// -/// * Wikipedia Fisheye Lens. [][wiki] -/// * Lensfun documentation. +/// * Wikipedia Fisheye Lens. [link][wiki]. +/// * Lensfun documentation. [link][lensfun]. /// * PTGui v11 Support question 3.28. [link][ptgui]. /// * Panotools wiki. [link][panotools]. /// -/// [wiki]: https://en.wikipedia.org/wiki/Fisheye_lens +/// [wiki]: https://en.wikipedia.org/wiki/Fisheye_lens#Mapping_function /// [opencv]: https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html /// [lensfun]: https://lensfun.github.io/manual/latest/corrections.html /// [ptgui]: https://www.ptgui.com/support.html#3_28 @@ -181,4 +152,6 @@ mod tests { test(1.0, r); }); } + + // TODO: Test parameter gradient using finite differences. } From 8936aa9f5a2f13966133173e21083d01d19fc16d Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Tue, 24 Nov 2020 12:47:44 -0800 Subject: [PATCH 18/26] Test using exact --- .../src/distortion_function/polynomial.rs | 116 +++++++++++++----- 1 file changed, 82 insertions(+), 34 deletions(-) diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index db1a64b3..fcbd72b7 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -16,7 +16,7 @@ use num_traits::{Float, Zero}; /// /// # To do /// -/// * (Bug) Parameters are currently in reverse order. +/// * Make it work with `Degree = Dynamic`. /// #[derive(Clone, PartialEq, Debug)] pub struct Polynomial(VectorN) @@ -98,59 +98,107 @@ mod tests { use cv_core::nalgebra::{Vector4, U4}; use float_eq::assert_float_eq; use proptest::prelude::*; + use rug::Float; - fn polynomial_1() -> Polynomial { - Polynomial::from_parameters(Vector4::new( - 1.0, - 25.67400161236561, - 15.249676433312018, - 0.6729530830603175, - )) + impl Polynomial + where + DefaultAllocator: Allocator, + { + /// Use RUG + MPFR to compute the nearest f64s to the exact values. + pub fn with_derivative_exact(&self, x: f64) -> (f64, f64) { + const PREC: u32 = 1000; // Compute using 1000 bit accuracy + let x = Float::with_val(PREC, x); + let mut value = Float::new(PREC); + let mut derivative = Float::new(PREC); + for i in (0..self.0.nrows()).rev() { + derivative *= &x; + derivative += &value; + value *= &x; + value += self.0[i]; + } + (value.to_f64(), derivative.to_f64()) + } + } + + #[rustfmt::skip] + const POLYS: &[&[f64]] = &[ + &[1.0, 25.67400161236561, 15.249676433312018, 0.6729530830603175], + &[1.0, 25.95447969279203, 22.421345314744390, 3.0431830552169914], + ]; + + fn polynomials(index: usize) -> Polynomial { + Polynomial::from_parameters(Vector4::from_column_slice(POLYS[index])) + } + + fn polynomial() -> impl Strategy> { + (0_usize..POLYS.len()).prop_map(polynomials) } #[test] - fn test_evaluate_1() { - let radial = polynomial_1(); - let result = radial.evaluate(0.06987809296337355); - assert_float_eq!(result, 2.86874326561548, ulps <= 0); + fn test_evaluate_literal() { + let f = polynomials(0); + let x = 0.06987809296337355; + let value = f.evaluate(x); + assert_float_eq!(value, 2.86874326561548, ulps <= 0); } #[test] - fn test_inverse_1() { - let radial = polynomial_1(); - let result = radial.inverse(2.86874326561548); - assert_float_eq!(result, 0.06987809296337355, ulps <= 0); + fn test_evaluate() { + proptest!(|(f in polynomial(), x in 0.0..2.0)| { + let value = f.evaluate(x); + let expected = f.with_derivative_exact(x).0; + assert_float_eq!(value, expected, rmax <= 2.0 * f64::EPSILON); + }); + } + + #[test] + fn test_with_derivative() { + proptest!(|(f in polynomial(), x in 0.0..2.0)| { + let value = f.with_derivative(x); + let expected = f.with_derivative_exact(x); + assert_float_eq!(value.0, expected.0, rmax <= 2.0 * f64::EPSILON); + assert_float_eq!(value.1, expected.1, rmax <= 2.0 * f64::EPSILON); + }); + } + + #[test] + fn test_inverse() { + proptest!(|(f in polynomial(), x in 0.0..2.0)| { + let y = f.with_derivative_exact(x).0; + let value = f.inverse(y); + // There may be a multiple valid inverses, so instead of checking + // the answer directly by `value == x`, we check that `f(value) == f(x)`. + let y2 = f.with_derivative_exact(value).0; + assert_float_eq!(y, y2, rmax <= 2.0 * f64::EPSILON); + }); } #[test] - fn test_finite_difference_1() { + fn test_finite_difference() { let h = f64::EPSILON.powf(1.0 / 3.0); - let radial = polynomial_1(); - proptest!(|(r in 0.0..2.0)| { - let h = f64::max(h * 0.1, h * r); - let deriv = radial.derivative(r); - let approx = (radial.evaluate(r + h) - radial.evaluate(r - h)) / (2.0 * h); + proptest!(|(f in polynomial(), x in 0.0..2.0)| { + let h = f64::max(h * 0.1, h * x); + let deriv = f.derivative(x); + let approx = (f.evaluate(x + h) - f.evaluate(x - h)) / (2.0 * h); assert_float_eq!(deriv, approx, rmax <= 1e2 * h * h); }); } #[test] - fn test_roundtrip_forward_1() { - let radial = polynomial_1(); - proptest!(|(r in 0.0..2.0)| { - let eval = radial.evaluate(r); - let inv = radial.inverse(eval); - assert_float_eq!(inv, r, rmax <= 1e4 * f64::EPSILON); + fn test_roundtrip_forward() { + proptest!(|(f in polynomial(), x in 0.0..2.0)| { + let eval = f.evaluate(x); + let inv = f.inverse(eval); + assert_float_eq!(inv, x, rmax <= 1e4 * f64::EPSILON); }); } #[test] - fn test_roundtrip_reverse_1() { - let radial = polynomial_1(); - proptest!(|(r in 1.0..2.0)| { - let inv = radial.inverse(r); - let eval = radial.evaluate(inv); - assert_float_eq!(eval, r, rmax <= 1e4 * f64::EPSILON); + fn test_roundtrip_reverse() { + proptest!(|(f in polynomial(), x in 1.0..2.0)| { + let inv = f.inverse(x); + let eval = f.evaluate(inv); + assert_float_eq!(eval, x, rmax <= 1e4 * f64::EPSILON); }); } From 792805fd217df5ad7ac359c06d184877b48293a4 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Tue, 24 Nov 2020 23:08:18 -0800 Subject: [PATCH 19/26] Implement undistort --- cv-pinhole/Cargo.toml | 2 +- cv-pinhole/src/camera.rs | 440 ++++++++++-------- cv-pinhole/src/distortion_function/fisheye.rs | 3 +- .../src/distortion_function/identity.rs | 46 -- cv-pinhole/src/distortion_function/mod.rs | 45 +- .../src/distortion_function/polynomial.rs | 51 +- .../src/distortion_function/rational.rs | 80 +++- cv-pinhole/src/lib.rs | 16 + cv-pinhole/src/root.rs | 71 +++ 9 files changed, 470 insertions(+), 284 deletions(-) delete mode 100644 cv-pinhole/src/distortion_function/identity.rs create mode 100644 cv-pinhole/src/root.rs diff --git a/cv-pinhole/Cargo.toml b/cv-pinhole/Cargo.toml index 99d07734..ac9ed0a1 100644 --- a/cv-pinhole/Cargo.toml +++ b/cv-pinhole/Cargo.toml @@ -26,11 +26,11 @@ nalgebra = { version = "0.22.0", default-features = false } [dev-dependencies] cv-geom = { version = "0.7.0", path = "../cv-geom" } - criterion = "0.3.3" proptest = "0.10.1" static_assertions = "1.1.0" float_eq = "0.5.0" +rug = "1.11.0" [package.metadata.docs.rs] all-features = true diff --git a/cv-pinhole/src/camera.rs b/cv-pinhole/src/camera.rs index 05d88e1a..1f77ff51 100644 --- a/cv-pinhole/src/camera.rs +++ b/cv-pinhole/src/camera.rs @@ -1,8 +1,9 @@ -use crate::distortion_function::DistortionFunction; +use crate::distortion_function::{DistortionFunction, Fisheye}; +use crate::root; use crate::CameraIntrinsics; use crate::NormalizedKeyPoint; -use cv_core::nalgebra::{allocator::Allocator, DefaultAllocator, Dim, Matrix2, Vector2}; +use cv_core::nalgebra::{allocator::Allocator, DefaultAllocator, Matrix2, Point2, Vector2}; use cv_core::{CameraModel, ImagePoint, KeyPoint}; /// Realistic camera with distortions. @@ -72,6 +73,7 @@ where DefaultAllocator: Allocator, { linear: CameraIntrinsics, + fisheye: Fisheye, radial_distortion: R, tangential: [f64; 2], tangential_distortion: D, @@ -95,6 +97,7 @@ where ) -> Self { Self { linear, + fisheye: Fisheye::default(), radial_distortion, tangential: [0.0, 0.0], tangential_distortion, @@ -102,22 +105,84 @@ where } } - /// Computes $\vec f(\mathtt{point})$. - pub fn correct(&self, point: Vector2) -> Vector2 { - let r2 = point.norm_squared(); + /// Apply lens distortions to a point. + #[rustfmt::skip] + pub fn distort(&self, point: Point2) -> Point2 { + let r2 = point.coords.norm_squared(); let f_r = self.radial_distortion.evaluate(r2); - let f_t = 1.0; + let f_t = self.tangential_distortion.evaluate(r2); let f_px = self.prism_distortion[0].evaluate(r2); let f_py = self.prism_distortion[1].evaluate(r2); - let t = self.tangential[0] * point.x + self.tangential[1] * point.y; - let t_x = 2.0 * point.x * t + self.tangential[0] * r2; - let t_y = 2.0 * point.y * t + self.tangential[1] * r2; - Vector2::new( - point.x * f_r + t_x * f_t + f_px, - point.y * f_r + t_y * f_t + f_py, + let [p_1, p_2] = self.tangential; + let (x, y) = (point.x, point.y); + let t_x = 2.0 * p_1 * x * y + p_2 * (r2 + 2.0 * x * x); + let t_y = p_1 * (r2 + 2.0 * y * y) + 2.0 * p_2 * x * y; + Point2::new( + x * f_r + t_x * f_t + f_px, + y * f_r + t_y * f_t + f_py, ) } + /// Apply lens distortions to a point. + #[rustfmt::skip] + pub fn undistort(&self, point: Point2) -> Point2 { + // The radial distortion is a large effect. It is also a one-dimensional + // problem we can solve precisely. This will produce a good starting + // point for the two-dimensional problem. + let rd = point.coords.norm(); + // Find $r_u$ such that $r_d = r_u ⋅ f(r_u^2)$ + let ru = root(|ru| { + let (f, df) = self.radial_distortion.with_derivative(ru * ru); + (ru * f - rd, f + 2.0 * ru * ru * df) + }, 0.0, 10.0); + let mut pu = point * ru / rd; + + // Newton-Raphson iteration + const MAX_ITER: usize = 10; + let mut last_delta_norm = f64::MAX; + for iter in 0.. { + let (F, J) = self.with_jacobian(pu); + let delta = J.lu().solve(&(F - point)).unwrap(); + let delta_norm = delta.norm_squared(); + pu -= delta; + if delta_norm < pu.coords.norm_squared() * f64::EPSILON * f64::EPSILON { + // Converged to epsilon + break; + } + if delta_norm >= last_delta_norm { + // No progress + if delta_norm < 100.0 * pu.coords.norm_squared() * f64::EPSILON * f64::EPSILON { + // Still useful + break; + } else { + // Divergence + panic!(); + } + } + last_delta_norm = delta_norm; + if iter >= MAX_ITER { + // No convergence + panic!(); + break; + } + } + pu + } + + /// Convert from rectilinear to the camera projection. + pub fn project(&self, point: Vector2) -> Vector2 { + let r = point.norm(); + let rp = self.fisheye.evaluate(r); + point * rp / r + } + + /// Convert from camera projection to rectilinear. + pub fn unproject(&self, point: Vector2) -> Vector2 { + let rp = point.norm(); + let r = self.fisheye.inverse(rp); + point * r / rp + } + /// Computes $∇_{\vec x} \vec f\p{\vec x, \vec θ}$. /// /// $$ @@ -147,15 +212,31 @@ where /// \end{aligned} /// $$ /// - pub fn jacobian(&self, point: Vector2) -> Matrix2 { - let r2 = point.norm_squared(); + pub fn with_jacobian(&self, point: Point2) -> (Point2, Matrix2) { + let [x, y] = [point.coords[0], point.coords[1]]; + let r2 = x * x + y * y; + let r2dx = 2. * x; + let r2dy = 2. * y; let (f_r, df_r) = self.radial_distortion.with_derivative(r2); let (f_t, df_t) = self.tangential_distortion.with_derivative(r2); - let df_px = self.prism_distortion[0].derivative(r2); - let df_py = self.prism_distortion[1].derivative(r2); - - todo!() - // Matrix2::new() + let (f_px, df_px) = self.prism_distortion[0].with_derivative(r2); + let (f_py, df_py) = self.prism_distortion[1].with_derivative(r2); + let [t1, t2] = self.tangential; + let tx = 2.0 * t1 * x * y + t2 * (r2 + 2.0 * x * x); + let txdx = 2.0 * t1 * y + t2 * (r2dx + 4.0 * x); + let txdy = 2.0 * t1 * x + t2 * r2dy; + let ty = t1 * (r2 + 2.0 * y * y) + 2.0 * t2 * x * y; + let tydx = t1 * r2dx + 2.0 * t2 * y; + let tydy = t1 * (r2dy + 4.0 * y) + 2.0 * t2 * x; + let u = x * f_r + tx * f_t + f_px; + let udr2 = x * df_r + tx * df_t + df_px; + let udx = f_r + txdx * f_t + udr2 * r2dx; + let udy = txdy * f_t + udr2 * r2dy; + let v = y * f_r + ty * f_t + f_py; + let vdr2 = y * df_r + ty * df_t + df_py; + let vdx = tydx * f_t + vdr2 * r2dx; + let vdy = f_r + tydy * f_t + vdr2 * r2dy; + (Point2::new(u, v), Matrix2::new(udx, udy, vdx, vdy)) } } @@ -192,204 +273,173 @@ where pub mod camera { use super::Camera; - use crate::distortion_function::{Identity, Polynomial, Rational}; - use cv_core::nalgebra::{U1, U2, U3, U4}; + use crate::distortion_function::{self, Constant, DistortionFunction, Polynomial, Rational}; + use crate::CameraIntrinsics; + use cv_core::nalgebra::{Matrix3, Point2, Vector2, Vector3, VectorN, U1, U8}; + use cv_core::nalgebra::{U2, U3, U4}; + + pub type Adobe = Camera, Constant, Constant>; + + /// Generate Adobe rectilinear camera model + /// + /// $u_0, v_0, f_x, f_y, k_1, k_2, k_3, k_4, k_5$ + /// + /// # References + /// + /// http://download.macromedia.com/pub/labs/lensprofile_creator/lensprofile_creator_cameramodel.pdf + /// + /// # To do + /// + /// * Implement Geometric Distortion Model for Fisheye Lenses + /// * Implement Lateral Chromatic Aberration Model + /// * Implement Vignette Model + /// * Read/Write Lens Correction Profile File Format + pub fn adobe(parameters: [f64; 9]) -> Adobe { + todo!() + } - /// OpenCV Camera model with 4 parameters - pub type OpenCV4 = Camera, Identity, Identity>; + pub type OpenCV = Camera, Constant, Polynomial>; - pub type OpenCV5 = Camera, Identity, Identity>; + /// Generate OpenCV camera with up to twelve distortion coefficients. + pub fn opencv(cameraMatrix: Matrix3, distCoeffs: &[f64]) -> OpenCV { + assert!( + distCoeffs.len() <= 12, + "Up to 12 coefficients are supported." + ); - pub type OpenCV8 = Camera, Identity, Identity>; + // Zero pad coefficients + let mut coeffs = [0.0; 12]; + coeffs.copy_from_slice(distCoeffs); - pub type OpenCV12 = Camera, Identity, Polynomial>; + let intrinsic = CameraIntrinsics::from_matrix(cameraMatrix); + #[rustfmt::skip] + let radial = Rational::::from_parameters( + VectorN::::from_column_slice_generic(U8, U1, &[ + 1.0, coeffs[0], coeffs[1], coeffs[4], + 1.0, coeffs[5], coeffs[6], coeffs[7], + ]) + ); + let tangential = distortion_function::one(); + let prism = [ + Polynomial::::from_parameters(Vector3::new(0.0, coeffs[8], coeffs[9])), + Polynomial::::from_parameters(Vector3::new(0.0, coeffs[10], coeffs[11])), + ]; + + let mut camera = OpenCV::new(intrinsic, radial, tangential, prism); + camera.tangential = [coeffs[2], coeffs[3]]; + camera + } } #[cfg(test)] mod tests { use super::*; - use crate::distortion_function::{Identity, Polynomial, Rational}; - use cv_core::nalgebra::{Point2, Vector3, VectorN, U1, U2, U3, U4, U8}; + use crate::distortion_function::{self, Polynomial, Rational}; + use cv_core::nalgebra::{Matrix3, Point2, Vector3, VectorN, U1, U3, U4, U8}; + use float_eq::assert_float_eq; - #[test] - fn camera_1() { + #[rustfmt::skip] + const UNDISTORTED: &[[f64; 2]] = &[ + [-2.678325867593669 , -2.018833440108032 ], + [-0.8278750155570158 , -1.2269359155245612 ], + [-0.02009583871305682 , -1.0754641516207084 ], + [ 0.7696051515407775 , -1.1999092188198324 ], + [ 2.4240231486605044 , -1.8482850962222797 ], + [-2.00590387018105 , -0.7622775647671782 ], + [-0.6803517193354109 , -0.508744726065251 ], + [-0.018858004780545452, -0.45712295391460256 ], + [ 0.629841408485172 , -0.5002154916071485 ], + [ 1.852035193756623 , -0.7179892894252142 ], + [-1.8327338936360238 , -0.01592369353657851 ], + [-0.6414183824611562 , -0.01250548415320514 ], + [-0.01831155521928233 , -0.011537853508065351], + [ 0.5932054070590098 , -0.012346500967552635], + [ 1.6999857018318918 , -0.015398552060458103], + [-2.0026791707814664 , 0.7276895662410346 ], + [-0.6772278897320149 , 0.48030562044200287 ], + [-0.01882077458202271 , 0.4309830892825161 ], + [ 0.6268195195673317 , 0.4721702706067835 ], + [ 1.8474518498275845 , 0.6841838156568198 ], + [-2.7191252091392095 , 2.00821485753557 ], + [-0.8213459272712429 , 1.187222601515949 ], + [-0.020100153303183495, 1.0384037280790197 ], + [ 0.762958127238301 , 1.1607439280019045 ], + [ 2.446040772535413 , 1.8275603904250888 ], + ]; + + #[rustfmt::skip] + const DISTORTED: &[[f64; 2]] = &[ + [-1.1422163009074084 , -0.8548601705472734 ], + [-0.5802629659506781 , -0.8548601705472634 ], + [-0.018309630993960678, -0.8548601705472749 ], + [ 0.5436437039627523 , -0.8548601705472572 ], + [ 1.105597038919486 , -0.8548601705472729 ], + [-1.1422163009074044 , -0.4331982294741029 ], + [-0.5802629659506768 , -0.4331982294740987 ], + [-0.018309630993960376, -0.43319822947409947 ], + [ 0.5436437039627607 , -0.4331982294741025 ], + [ 1.1055970389194907 , -0.4331982294741061 ], + [-1.1422163009074042 , -0.011536288400935572], + [-0.58026296595067 , -0.011536288400935261], + [-0.018309630993960747, -0.011536288400935736], + [ 0.5436437039627555 , -0.011536288400935357], + [ 1.1055970389194847 , -0.011536288400935575], + [-1.1422163009074064 , 0.4101256526722325 ], + [-0.5802629659506844 , 0.4101256526722334 ], + [-0.018309630993960612, 0.41012565267223955 ], + [ 0.5436437039627673 , 0.4101256526722365 ], + [ 1.1055970389194787 , 0.41012565267223033 ], + [-1.1422163009074024 , 0.8317875937453983 ], + [-0.5802629659506786 , 0.8317875937453932 ], + [-0.018309630993960567, 0.8317875937453815 ], + [ 0.5436437039627544 , 0.8317875937453895 ], + [ 1.1055970389194876 , 0.8317875937454029 ], + ]; + + fn camera_1() -> camera::OpenCV { // Calibration parameters for a GoPro Hero 6 using OpenCV - let focal: [f64; 2] = [1.77950719e+0, 1.77867606e+03]; - let center: [f64; 2] = [2.03258212e+03, 1.52051932e+03]; + #[rustfmt::skip] + let cameraMatrix = Matrix3::from_row_slice(&[ + 1.77950719e+03, 0.00000000e+00, 2.03258212e+03, + 0.00000000e+00, 1.77867606e+03, 1.52051932e+03, + 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]); #[rustfmt::skip] let distortion: [f64; 12] = [ 2.56740016e+01, 1.52496764e+01, -5.01712057e-04, 1.09310463e-03, 6.72953083e-01, 2.59544797e+01, 2.24213453e+01, 3.04318306e+00, -3.23278793e-03, 9.53176056e-05, -9.35687185e-05, 2.96341863e-05]; - // The OpenCV - let distorted = [1718.3195, 1858.1052]; - let undistorted = [-0.17996894, 0.19362147]; + camera::opencv(cameraMatrix, &distortion) + } - let mut intrinsic = CameraIntrinsics::identity(); - intrinsic.focals = Vector2::new(focal[0], focal[1]); - intrinsic.principal_point = Point2::new(center[0], center[1]); - #[rustfmt::skip] - let radial = Rational::::from_parameters( - VectorN::::from_column_slice_generic(U8, U1, &[ - distortion[4], distortion[1], distortion[0], 1.0, - distortion[7], distortion[6], distortion[5], 1.0, - ]) - ); - let tangential = Identity; - let prism = [ - Polynomial::::from_parameters(Vector3::new(distortion[9], distortion[8], 0.0)), - Polynomial::::from_parameters(Vector3::new(distortion[11], distortion[10], 0.0)), - ]; + #[test] + fn test_distort_1() { + let camera = camera_1(); + for (undistorted, expected) in UNDISTORTED.iter().zip(DISTORTED.iter()) { + let [x, y] = *undistorted; + let [ex, ey] = *expected; - let camera = camera::OpenCV12::new(intrinsic, radial, tangential, prism); + let distorted = camera.distort(Point2::new(x, y)); + let (x, y) = (distorted[0], distorted[1]); - let distorted = Vector2::new(1718.3195, 1858.1052); - let undistorted = camera.correct(distorted); - let expected = Vector2::new(-0.17996894, 0.19362147); - println!("{:#?}", undistorted); + assert_float_eq!(x, ex, ulps <= 2); + assert_float_eq!(y, ey, ulps <= 2); + } + } - let distorted = Vector2::new(579.8596, 2575.0476); - let undistorted = camera.correct(distorted); - let expected = Vector2::new(-1.1464612, 0.8380858); - println!("{:#?}", undistorted); + #[test] + fn test_undistort_1() { + let camera = camera_1(); + for (expected, distorted) in UNDISTORTED.iter().zip(DISTORTED.iter()) { + let [x, y] = *distorted; + let [ex, ey] = *expected; - let source = [ - [-0.17996894, 0.19362147], - [-0.17968875, 0.24255648], - [-0.17944576, 0.29271868], - [-0.17916603, 0.34418374], - [-0.17893367, 0.39675304], - [-0.17863423, 0.45061454], - [-0.1782976, 0.5059651], - [-0.17797716, 0.56267387], - [-0.17769286, 0.62078154], - [-0.22691241, 0.1944314], - [-0.22716342, 0.24451172], - [-0.22748885, 0.29591268], - [-0.22788039, 0.3486148], - [-0.22820546, 0.4025184], - [-0.22855103, 0.4577957], - [-0.22891289, 0.51458186], - [-0.22923958, 0.572837], - [-0.22955456, 0.6325643], - [-0.2760499, 0.19522506], - [-0.27696946, 0.24657097], - [-0.27798527, 0.29921785], - [-0.27897915, 0.35328886], - [-0.27998206, 0.40862617], - [-0.28099003, 0.46533334], - [-0.28205746, 0.5236744], - [-0.2831492, 0.5835057], - [-0.28426304, 0.6449713], - [-0.3276202, 0.19608246], - [-0.3292424, 0.24866305], - [-0.3309349, 0.3026873], - [-0.33262616, 0.3581513], - [-0.33433127, 0.4149485], - [-0.33617917, 0.4732646], - [-0.33799866, 0.5331862], - [-0.33992943, 0.5947946], - [-0.34184524, 0.65808254], - [-0.38177538, 0.19692989], - [-0.38411814, 0.25095174], - [-0.3866268, 0.30635145], - [-0.38909492, 0.36327213], - [-0.3916721, 0.42171457], - [-0.39424962, 0.48160818], - [-0.39702195, 0.5432604], - [-0.39976382, 0.6066253], - [-0.40267637, 0.671886], - [-0.43867132, 0.19787468], - [-0.44195008, 0.2532662], - [-0.44519594, 0.31016332], - [-0.44858345, 0.3686112], - [-0.45198414, 0.42872244], - [-0.45556828, 0.49036437], - [-0.45910528, 0.55382824], - [-0.46299413, 0.6192026], - [-0.466843, 0.6863503], - [-0.49861795, 0.1987457], - [-0.5026725, 0.2557629], - [-0.5069558, 0.31417185], - [-0.51119965, 0.37434888], - [-0.5157122, 0.43618685], - [-0.5201786, 0.49963355], - [-0.5250113, 0.56500995], - [-0.529766, 0.63237023], - [-0.53485453, 0.70181614], - [-0.56156373, 0.19966456], - [-0.5667678, 0.25814596], - [-0.5719744, 0.31840706], - [-0.57744604, 0.38031867], - [-0.5828779, 0.4440139], - [-0.58866763, 0.50954485], - [-0.59443516, 0.5769114], - [-0.6006511, 0.64649254], - [-0.60675824, 0.71801007], - [-0.628279, 0.20040697], - [-0.6344018, 0.26069504], - [-0.64092815, 0.32267714], - [-0.6474141, 0.38664073], - [-0.6542724, 0.4522714], - [-0.6610637, 0.51989216], - [-0.6683662, 0.5895391], - [-0.6756135, 0.66126966], - [-0.6833462, 0.7354248], - [-0.6985042, 0.20141184], - [-0.7060343, 0.2632588], - [-0.71366554, 0.3273413], - [-0.7216309, 0.39327696], - [-0.7295911, 0.46094468], - [-0.7380902, 0.53080666], - [-0.7465825, 0.6027301], - [-0.7556527, 0.6770891], - [-0.7646206, 0.75370055], - [-0.7728095, 0.20244533], - [-0.78165317, 0.26625928], - [-0.7907952, 0.33220664], - [-0.8001839, 0.40018782], - [-0.8098858, 0.4701321], - [-0.8196281, 0.54215276], - [-0.83001417, 0.6167809], - [-0.8402761, 0.6934916], - [-0.8512289, 0.7733144], - [-0.8518392, 0.20366336], - [-0.862248, 0.26939756], - [-0.8729417, 0.33734974], - [-0.8838656, 0.40754512], - [-0.89505607, 0.4797963], - [-0.90671587, 0.5545365], - [-0.9185119, 0.63145286], - [-0.93089086, 0.71136534], - [-0.9433173, 0.7935529], - [-0.93565977, 0.20484428], - [-0.9476607, 0.27270162], - [-0.9600662, 0.3428219], - [-0.97279584, 0.41510573], - [-0.9859218, 0.49006754], - [-0.9992419, 0.56712496], - [-1.0130187, 0.6472003], - [-1.0271673, 0.72976106], - [-1.0417787, 0.8154801], - [-1.02476, 0.20601285], - [-1.038625, 0.27609447], - [-1.0529088, 0.34840867], - [-1.0676082, 0.42343187], - [-1.0825946, 0.50079226], - [-1.0980939, 0.5808948], - [-1.1138377, 0.6636289], - [-1.1300658, 0.7495205], - [-1.1464612, 0.8380858], - ]; - for i in 0..source.len() { - let [x, y] = source[i]; - let source = Vector2::new(x, y); - let destination = camera.correct(source); - let (x, y) = (destination[0], destination[1]); - let x = x * focal[0] + center[0]; - let y = y * focal[1] + center[1]; - println!("[{}, {}],", x, y); + let distorted = Point2::new(x, y); + let undistorted = camera.undistort(distorted); + let (x, y) = (undistorted[0], undistorted[1]); + + assert_float_eq!(x, ex, ulps <= 7); + assert_float_eq!(y, ey, ulps <= 7); } } } diff --git a/cv-pinhole/src/distortion_function/fisheye.rs b/cv-pinhole/src/distortion_function/fisheye.rs index b8612f88..f19b6868 100644 --- a/cv-pinhole/src/distortion_function/fisheye.rs +++ b/cv-pinhole/src/distortion_function/fisheye.rs @@ -32,10 +32,11 @@ use cv_core::nalgebra::{storage::Storage, Vector, Vector1, VectorN, U1}; /// [panotools]: https://wiki.panotools.org/Fisheye_Projection /// /// -#[derive(Clone, PartialEq, Debug)] +#[derive(Clone, PartialEq, PartialOrd, Debug)] pub struct Fisheye(f64); impl Default for Fisheye { + /// Defaults to rectilinear (non-Fisheye) projection. fn default() -> Self { Fisheye(1.0) } diff --git a/cv-pinhole/src/distortion_function/identity.rs b/cv-pinhole/src/distortion_function/identity.rs deleted file mode 100644 index 3a20fb8f..00000000 --- a/cv-pinhole/src/distortion_function/identity.rs +++ /dev/null @@ -1,46 +0,0 @@ -use super::DistortionFunction; -use cv_core::nalgebra::{ - allocator::Allocator, storage::Storage, zero, ArrayStorage, DefaultAllocator, Dim, DimName, - NamedDim, Vector, VectorN, U0, U1, -}; - -/// Identity distortion, i.e. no distortion at all. -/// -/// $$ -/// f(x) = x -/// $$ -/// -/// Refresh, maybe? -#[derive(Clone, PartialEq, Default, Debug)] -pub struct Identity; - -impl DistortionFunction for Identity { - type NumParameters = U0; - - fn from_parameters(parameters: Vector) -> Self - where - S: Storage, - { - Self - } - - fn parameters(&self) -> VectorN { - VectorN::zeros_generic(U0, U1) - } - - fn evaluate(&self, value: f64) -> f64 { - value - } - - fn derivative(&self, value: f64) -> f64 { - 0.0 - } - - fn inverse(&self, value: f64) -> f64 { - value - } - - fn gradient(&self, value: f64) -> VectorN { - todo!() - } -} diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs index f3a40c66..cade731b 100644 --- a/cv-pinhole/src/distortion_function/mod.rs +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -1,25 +1,17 @@ mod fisheye; -mod identity; mod polynomial; mod rational; use cv_core::nalgebra::{ - allocator::Allocator, storage::Storage, DefaultAllocator, Dim, Vector, VectorN, + allocator::Allocator, storage::Storage, DefaultAllocator, Dim, Vector, Vector1, Vector2, + VectorN, U1, U2, }; // Re-exports -#[doc(inline)] -pub use identity::Identity; - -#[doc(inline)] +pub use fisheye::Fisheye; pub use polynomial::Polynomial; - -#[doc(inline)] pub use rational::Rational; -#[doc(inline)] -pub use fisheye::Fisheye; - /// Trait for parameterized functions specifying 1D distortions. /// /// $$ @@ -74,12 +66,12 @@ where /// /// # Method /// - /// Uses a Newton-Bisection hybrid method based on [1] that is guaranteed to - /// converge to almost machine precision. + /// The default implementation uses a Newton-Bisection hybrid method based on [^1] + /// that is guaranteed to converge to almost machine precision. /// /// # Resources /// - /// Numerical Recipes 2nd edition. p. 365 + /// [^1]: Numerical Recipes 2nd edition. p. 365 /// /// /// @@ -107,6 +99,7 @@ where let mut dxold = (xl - xh).abs(); let mut dx = dxold; let (mut f, mut df) = self.with_derivative(rts); + f -= value; loop { if (((rts - xh) * df - f) * ((rts - xl) * df - f) > 0.0) || (2.0 * f.abs() > (dxold * df).abs()) @@ -142,3 +135,27 @@ where /// Parameter gradient $∇_{\vec β​} f(\mathtt{value}, \vec β)$. fn gradient(&self, value: f64) -> VectorN; } + +pub type Constant = Polynomial; + +pub type Identity = Polynomial; + +/// The constant zero function. +pub fn zero() -> Constant { + constant(0.0) +} + +/// The constant +pub fn one() -> Constant { + constant(1.0) +} + +/// Create a constant function. +pub fn constant(value: f64) -> Constant { + Constant::from_parameters(Vector1::new(value)) +} + +/// Create the identity function. +pub fn identity() -> Identity { + Identity::from_parameters(Vector2::new(0.0, 1.0)) +} diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index fcbd72b7..9744098b 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -70,6 +70,15 @@ where /// /// # Method /// + /// Uses Horner's method with it's algorithmic derivative. + /// + /// $$ + /// \begin{aligned} + /// y_{i+1} &= y_i ⋅ x + c_{n - i} \\\\ + /// y'_{i+1} &= y'_i ⋅ x + y_i + /// \end{aligned} + /// $$ + /// fn with_derivative(&self, value: f64) -> (f64, f64) { let mut result = 0.0; let mut derivative = 0.0; @@ -104,18 +113,22 @@ mod tests { where DefaultAllocator: Allocator, { - /// Use RUG + MPFR to compute the nearest f64s to the exact values. - pub fn with_derivative_exact(&self, x: f64) -> (f64, f64) { - const PREC: u32 = 1000; // Compute using 1000 bit accuracy - let x = Float::with_val(PREC, x); - let mut value = Float::new(PREC); - let mut derivative = Float::new(PREC); + pub fn with_derivative_float(&self, x: Float) -> (Float, Float) { + let mut value = Float::new(x.prec()); + let mut derivative = Float::new(x.prec()); for i in (0..self.0.nrows()).rev() { derivative *= &x; derivative += &value; value *= &x; value += self.0[i]; } + (value, derivative) + } + + pub fn with_derivative_exact(&self, x: f64) -> (f64, f64) { + // Compute with 1000 bits accuracy + let x = Float::with_val(1000, x); + let (value, derivative) = self.with_derivative_float(x); (value.to_f64(), derivative.to_f64()) } } @@ -124,19 +137,21 @@ mod tests { const POLYS: &[&[f64]] = &[ &[1.0, 25.67400161236561, 15.249676433312018, 0.6729530830603175], &[1.0, 25.95447969279203, 22.421345314744390, 3.0431830552169914], + // &[0.0, -3.23278793e-03, 9.53176056e-05, 0.0], + // &[0.0, -9.35687185e-05, 2.96341863e-05, 0.0], ]; - fn polynomials(index: usize) -> Polynomial { + fn functions(index: usize) -> Polynomial { Polynomial::from_parameters(Vector4::from_column_slice(POLYS[index])) } - fn polynomial() -> impl Strategy> { - (0_usize..POLYS.len()).prop_map(polynomials) + fn function() -> impl Strategy> { + (0_usize..POLYS.len()).prop_map(functions) } #[test] fn test_evaluate_literal() { - let f = polynomials(0); + let f = functions(0); let x = 0.06987809296337355; let value = f.evaluate(x); assert_float_eq!(value, 2.86874326561548, ulps <= 0); @@ -144,7 +159,7 @@ mod tests { #[test] fn test_evaluate() { - proptest!(|(f in polynomial(), x in 0.0..2.0)| { + proptest!(|(f in function(), x in 0.0..2.0)| { let value = f.evaluate(x); let expected = f.with_derivative_exact(x).0; assert_float_eq!(value, expected, rmax <= 2.0 * f64::EPSILON); @@ -153,17 +168,17 @@ mod tests { #[test] fn test_with_derivative() { - proptest!(|(f in polynomial(), x in 0.0..2.0)| { + proptest!(|(f in function(), x in 0.0..2.0)| { let value = f.with_derivative(x); let expected = f.with_derivative_exact(x); assert_float_eq!(value.0, expected.0, rmax <= 2.0 * f64::EPSILON); - assert_float_eq!(value.1, expected.1, rmax <= 2.0 * f64::EPSILON); + assert_float_eq!(value.1, expected.1, rmax <= 1e3 * f64::EPSILON); }); } #[test] fn test_inverse() { - proptest!(|(f in polynomial(), x in 0.0..2.0)| { + proptest!(|(f in function(), x in 0.0..2.0)| { let y = f.with_derivative_exact(x).0; let value = f.inverse(y); // There may be a multiple valid inverses, so instead of checking @@ -176,17 +191,17 @@ mod tests { #[test] fn test_finite_difference() { let h = f64::EPSILON.powf(1.0 / 3.0); - proptest!(|(f in polynomial(), x in 0.0..2.0)| { + proptest!(|(f in function(), x in 0.0..2.0)| { let h = f64::max(h * 0.1, h * x); let deriv = f.derivative(x); let approx = (f.evaluate(x + h) - f.evaluate(x - h)) / (2.0 * h); - assert_float_eq!(deriv, approx, rmax <= 1e2 * h * h); + assert_float_eq!(deriv, approx, rmax <= 1e3 * h * h); }); } #[test] fn test_roundtrip_forward() { - proptest!(|(f in polynomial(), x in 0.0..2.0)| { + proptest!(|(f in function(), x in 0.0..2.0)| { let eval = f.evaluate(x); let inv = f.inverse(eval); assert_float_eq!(inv, x, rmax <= 1e4 * f64::EPSILON); @@ -195,7 +210,7 @@ mod tests { #[test] fn test_roundtrip_reverse() { - proptest!(|(f in polynomial(), x in 1.0..2.0)| { + proptest!(|(f in function(), x in 1.0..2.0)| { let inv = f.inverse(x); let eval = f.evaluate(inv); assert_float_eq!(eval, x, rmax <= 1e4 * f64::EPSILON); diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index 85c6e90f..b5f2371d 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -162,10 +162,36 @@ mod tests { use cv_core::nalgebra::{VectorN, U4, U8}; use float_eq::assert_float_eq; use proptest::prelude::*; + use rug::ops::Pow; + use rug::Float; + + impl Rational + where + DP: DimName, + DQ: DimName, + DP: DimAdd, + DimSum: DimName, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator>, + { + pub fn with_derivative_float(&self, x: Float) -> (Float, Float) { + let (p, dp) = self.0.with_derivative_float(x.clone()); + let (q, dq) = self.1.with_derivative_float(x); + (p.clone() / &q, (dp * &q - p * dq) / q.pow(2)) + } + + pub fn with_derivative_exact(&self, x: f64) -> (f64, f64) { + // Compute with 1000 bits accuracy + let x = Float::with_val(1000, x); + let (value, derivative) = self.with_derivative_float(x); + (value.to_f64(), derivative.to_f64()) + } + } /// OpenCV12 radial distortion for a GoPro Hero 6 #[rustfmt::skip] - fn rational_1() -> Rational { + fn functions(_index: usize) -> Rational { Rational::from_parameters( VectorN::from_column_slice_generic(U8, U1, &[ 1.0, 25.67400161236561, 15.249676433312018, 0.6729530830603175, @@ -174,16 +200,52 @@ mod tests { ) } + fn function() -> impl Strategy> { + (0_usize..1).prop_map(functions) + } + + #[test] + fn test_evaluate_literal() { + let f = functions(0); + let x = 0.06987809296337355; + let value = f.evaluate(x); + assert_float_eq!(value, 0.9810452524397972, ulps <= 0); + } + #[test] - fn test_evaluate_1() { - let radial = rational_1(); - let result = radial.evaluate(0.06987809296337322); - assert_float_eq!(result, 0.9810452524397972, ulps <= 0); + fn test_evaluate() { + proptest!(|(f in function(), x in 0.0..2.0)| { + let value = f.evaluate(x); + let expected = f.with_derivative_exact(x).0; + assert_float_eq!(value, expected, rmax <= 2.0 * f64::EPSILON); + }); + } + + #[test] + fn test_with_derivative() { + proptest!(|(f in function(), x in 0.0..2.0)| { + let value = f.with_derivative(x); + let expected = f.with_derivative_exact(x); + assert_float_eq!(value.0, expected.0, rmax <= 2.0 * f64::EPSILON); + assert_float_eq!(value.1, expected.1, rmax <= 94.0 * f64::EPSILON); + }); + } + + #[test] + fn test_inverse() { + proptest!(|(f in function(), x in 0.0..2.0)| { + let y = f.with_derivative_exact(x).0; + let value = f.inverse(y); + // There may be a multiple valid inverses, so instead of checking + // the answer directly by `value == x`, we check that `f(value) == f(x)`. + let y2 = f.with_derivative_exact(value).0; + assert_float_eq!(y, y2, rmax <= 2.0 * f64::EPSILON); + }); } #[test] fn test_inverse_1() { - let radial = rational_1(); + let radial = functions(0); let result = radial.inverse(0.9810452524397972); assert_float_eq!(result, 0.06987809296337322, ulps <= 8); } @@ -191,7 +253,7 @@ mod tests { #[test] fn test_finite_difference_1() { let h = f64::EPSILON.powf(1.0 / 3.0); - let radial = rational_1(); + let radial = functions(0); proptest!(|(r in 0.0..2.0)| { let h = f64::max(h * 0.1, h * r); let deriv = radial.derivative(r); @@ -202,7 +264,7 @@ mod tests { #[test] fn test_roundtrip_forward_1() { - let radial = rational_1(); + let radial = functions(0); proptest!(|(r in 0.0..2.0)| { let eval = radial.evaluate(r); let inv = radial.inverse(eval); @@ -213,7 +275,7 @@ mod tests { #[test] fn test_roundtrip_reverse_1() { - let radial = rational_1(); + let radial = functions(0); proptest!(|(r in 0.7..1.0)| { let inv = radial.inverse(r); let eval = radial.evaluate(inv); diff --git a/cv-pinhole/src/lib.rs b/cv-pinhole/src/lib.rs index 0cb9577a..f43b8b29 100644 --- a/cv-pinhole/src/lib.rs +++ b/cv-pinhole/src/lib.rs @@ -11,9 +11,11 @@ extern crate alloc; mod camera; pub mod distortion_function; mod essential; +mod root; pub use camera::Camera; pub use essential::*; +pub(crate) use root::root; use cv_core::nalgebra::{Matrix3, Point2, Point3, Vector2, Vector3}; use cv_core::{ @@ -109,6 +111,20 @@ impl CameraIntrinsics { } } + pub fn from_matrix(matrix: Matrix3) -> Self { + assert_eq!( + matrix, + matrix.upper_triangle(), + "Camera matrix is not upper triangular" + ); + let s = 1.0 / matrix[(2, 2)]; + Self { + focals: Vector2::new(matrix[(0, 0)], matrix[(1, 1)]) * s, + principal_point: Point2::new(matrix[(0, 2)], matrix[(1, 2)]) * s, + skew: matrix[(0, 1)] * s, + } + } + pub fn focals(self, focals: Vector2) -> Self { Self { focals, ..self } } diff --git a/cv-pinhole/src/root.rs b/cv-pinhole/src/root.rs new file mode 100644 index 00000000..2323b919 --- /dev/null +++ b/cv-pinhole/src/root.rs @@ -0,0 +1,71 @@ +/// Find function root +/// +/// # Method +/// +/// The default implementation uses a Newton-Bisection hybrid method based on [^1] +/// that is guaranteed to converge to almost machine precision. +/// +/// # Resources +/// +/// [^1]: Numerical Recipes 2nd edition. p. 365 +/// +/// +/// +/// # Panics +/// +/// Panics when $f(a) ⋅ f(b) > 0$. +/// +pub(crate) fn root(func: F, a: f64, b: f64) -> f64 +where + F: Fn(f64) -> (f64, f64), +{ + let (mut xl, mut xh) = (a, b); + let (fl, _) = func(xl); + if fl == 0.0 { + return xl; + } + let (fh, _) = func(xh); + if fh == 0.0 { + return xh; + } + if fl * fh > 0.0 { + panic!("Inverse outside of bracket [a, b]."); + } + if fl > 0.0 { + std::mem::swap(&mut xl, &mut xh); + } + let mut rts = 0.5 * (xl + xh); + let mut dxold = (xl - xh).abs(); + let mut dx = dxold; + let (mut f, mut df) = func(rts); + loop { + if (((rts - xh) * df - f) * ((rts - xl) * df - f) > 0.0) + || (2.0 * f.abs() > (dxold * df).abs()) + { + // Bisection step + dxold = dx; + dx = 0.5 * (xh - xl); + rts = xl + dx; + if xl == rts || xh == rts { + return rts; + } + } else { + // Newton step + dxold = dx; + dx = f / df; + let tmp = rts; + rts -= dx; + if tmp == rts { + return rts; + } + } + let (nf, ndf) = func(rts); + f = nf; + df = ndf; + if f < 0.0 { + xl = rts; + } else { + xh = rts; + } + } +} From e599198bd8e5e2eb95d2e1b222e158aa1ab8789b Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Wed, 25 Nov 2020 09:05:34 -0800 Subject: [PATCH 20/26] Factor out solver --- cv-pinhole/src/camera.rs | 38 +++++++------------------------------- cv-pinhole/src/lib.rs | 2 +- cv-pinhole/src/root.rs | 38 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 46 insertions(+), 32 deletions(-) diff --git a/cv-pinhole/src/camera.rs b/cv-pinhole/src/camera.rs index 1f77ff51..9880d01d 100644 --- a/cv-pinhole/src/camera.rs +++ b/cv-pinhole/src/camera.rs @@ -1,7 +1,7 @@ use crate::distortion_function::{DistortionFunction, Fisheye}; -use crate::root; use crate::CameraIntrinsics; use crate::NormalizedKeyPoint; +use crate::{newton2, root}; use cv_core::nalgebra::{allocator::Allocator, DefaultAllocator, Matrix2, Point2, Vector2}; use cv_core::{CameraModel, ImagePoint, KeyPoint}; @@ -135,38 +135,14 @@ where let (f, df) = self.radial_distortion.with_derivative(ru * ru); (ru * f - rd, f + 2.0 * ru * ru * df) }, 0.0, 10.0); - let mut pu = point * ru / rd; + let pu = point * ru / rd; // Newton-Raphson iteration - const MAX_ITER: usize = 10; - let mut last_delta_norm = f64::MAX; - for iter in 0.. { - let (F, J) = self.with_jacobian(pu); - let delta = J.lu().solve(&(F - point)).unwrap(); - let delta_norm = delta.norm_squared(); - pu -= delta; - if delta_norm < pu.coords.norm_squared() * f64::EPSILON * f64::EPSILON { - // Converged to epsilon - break; - } - if delta_norm >= last_delta_norm { - // No progress - if delta_norm < 100.0 * pu.coords.norm_squared() * f64::EPSILON * f64::EPSILON { - // Still useful - break; - } else { - // Divergence - panic!(); - } - } - last_delta_norm = delta_norm; - if iter >= MAX_ITER { - // No convergence - panic!(); - break; - } - } - pu + let pu = newton2(|x| { + let (F, J) = self.with_jacobian(x.into()); + (F.coords - point.coords, J) + }, pu.coords).unwrap(); + pu.into() } /// Convert from rectilinear to the camera projection. diff --git a/cv-pinhole/src/lib.rs b/cv-pinhole/src/lib.rs index f43b8b29..92126da7 100644 --- a/cv-pinhole/src/lib.rs +++ b/cv-pinhole/src/lib.rs @@ -15,7 +15,7 @@ mod root; pub use camera::Camera; pub use essential::*; -pub(crate) use root::root; +pub(crate) use root::{newton2, root}; use cv_core::nalgebra::{Matrix3, Point2, Point3, Vector2, Vector3}; use cv_core::{ diff --git a/cv-pinhole/src/root.rs b/cv-pinhole/src/root.rs index 2323b919..e3640fa6 100644 --- a/cv-pinhole/src/root.rs +++ b/cv-pinhole/src/root.rs @@ -1,3 +1,5 @@ +use cv_core::nalgebra::{Matrix2, Vector2}; + /// Find function root /// /// # Method @@ -69,3 +71,39 @@ where } } } + +pub(crate) fn newton2(func: F, initial: Vector2) -> Option> +where + F: Fn(Vector2) -> (Vector2, Matrix2), +{ + // Newton-Raphson iteration + const MAX_ITER: usize = 10; + let mut x = initial; + let mut last_delta_norm = f64::MAX; + for iter in 0.. { + let (F, J) = func(x); + let delta = J.lu().solve(&F).unwrap(); + let delta_norm = delta.norm_squared(); + x -= delta; + if delta_norm < x.norm_squared() * f64::EPSILON * f64::EPSILON { + // Converged to epsilon + break; + } + if delta_norm >= last_delta_norm { + // No progress + if delta_norm < 100.0 * x.norm_squared() * f64::EPSILON * f64::EPSILON { + // Still useful + break; + } else { + // Divergence + return None; + } + } + last_delta_norm = delta_norm; + if iter >= MAX_ITER { + // No convergence + return None; + } + } + Some(x) +} From a25a3cca9ff944561765d40581d1d7151cf7d96f Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Fri, 27 Nov 2020 11:38:38 -0800 Subject: [PATCH 21/26] Refactor --- cv-pinhole/src/camera.rs | 278 +++++++++++------- cv-pinhole/src/distortion_function/fisheye.rs | 15 +- cv-pinhole/src/distortion_function/mod.rs | 53 ++++ .../src/distortion_function/polynomial.rs | 21 +- .../src/distortion_function/rational.rs | 41 ++- cv-pinhole/src/lib.rs | 8 +- cv-pinhole/src/root.rs | 4 +- 7 files changed, 273 insertions(+), 147 deletions(-) diff --git a/cv-pinhole/src/camera.rs b/cv-pinhole/src/camera.rs index 9880d01d..0aa7c4ab 100644 --- a/cv-pinhole/src/camera.rs +++ b/cv-pinhole/src/camera.rs @@ -3,8 +3,12 @@ use crate::CameraIntrinsics; use crate::NormalizedKeyPoint; use crate::{newton2, root}; -use cv_core::nalgebra::{allocator::Allocator, DefaultAllocator, Matrix2, Point2, Vector2}; +use cv_core::nalgebra::{ + allocator::Allocator, DefaultAllocator, DimAdd, DimMul, DimName, DimProd, DimSum, Matrix2, + Point2, Vector2, VectorN, U1, U2, +}; use cv_core::{CameraModel, ImagePoint, KeyPoint}; +use num_traits::Zero; /// Realistic camera with distortions. /// @@ -19,10 +23,30 @@ use cv_core::{CameraModel, ImagePoint, KeyPoint}; /// number of coefficients, the camera model uses generic [`DistortionFunction`]s, provided as a type /// parameter. /// -/// Given image coordinates $(x,y)$ and $r = x^2 + y^2$ the undistorted coordinates $(x', y')$ are +/// ## From model coordinates to image coordinates +/// +/// Given a point $(x, y, z)$ in camera relative coordinates such +/// that the camera is position at the origin and looking in the +z-direction. +/// +/// Given undistorted image coordinates $(x,y)$ and $r = x^2 + y^2$ the distorted coordinates $(x', y')$ are /// computed as: /// /// $$ +/// z' ⋅ \begin{bmatrix} x' \\\\ y' \\\ 1 \end{bmatrix} = +/// \begin{bmatrix} +/// f_x & s & c_x \\\\ +/// 0 & f_y & c_y \\\\ +/// 0 & 0 & 1 +/// \end{bmatrix} +/// \begin{bmatrix} x \\\\ y \\\\ z \end{bmatrix} +/// $$ +/// +/// $$ +/// \begin{bmatrix} x' \\\\ y' \end{bmatrix} = +/// \frac{f_p(r, \vec θ_p)}{r} ⋅ \begin{bmatrix} x \\\\ y \end{bmatrix} +/// $$ +/// +/// $$ /// \begin{bmatrix} x' \\\\ y' \end{bmatrix} = \begin{bmatrix} /// x ⋅ f_r(r^2, \vec θ_r) + \p{2⋅x⋅\p{t_1 ⋅ x + t_2 ⋅ y} + t_1 ⋅ r^2} ⋅ f_t(r^2, \vec θ_d) + f_{p}(r^2, \vec θ_{x}) /// \\\\ @@ -34,10 +58,14 @@ use cv_core::{CameraModel, ImagePoint, KeyPoint}; /// decentering distortion, with $f_t$ of type `D` and $(f_{px}, f_{py})$ specify the thin prism /// of type `T`. /// +/// ## From image coordinates to model coordinates +/// +/// ## Parameterization +/// /// The parameter vector of the distortion is /// /// $$ -/// \vec θ = \begin{bmatrix} \vec θ_r \\\\ t_1 \\\\ t_2 \\\\ \vec θ_d \\\\ \vec θ_x \\\\ \vec θ_y \end{bmatrix} +/// \vec θ = \begin{bmatrix} k & \vec θ_r^T & t_1 & t_2 & \vec θ_d^T & \vec θ_x^T & \vec θ_y^T \end{bmatrix}^T /// $$ /// /// # References @@ -57,54 +85,38 @@ use cv_core::{CameraModel, ImagePoint, KeyPoint}; /// /// # To do /// -/// * Chromatic abberation. -/// * Vigneting correction. +/// * Replace all the [`Dim`] madness with const generics once this hits stable. +/// * Adobe model fisheye distortion, chromatic abberation and vigneting correction. /// * Support [Tilt/Scheimpflug](https://en.wikipedia.org/wiki/Scheimpflug_principle) lenses. See [Louhichi et al.](https://iopscience.iop.org/article/10.1088/0957-0233/18/8/037) for a mathematical model. /// -#[derive(Clone, PartialEq, PartialOrd, Debug)] +#[derive(Clone, PartialEq, PartialOrd, Default, Debug)] // #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct Camera +pub struct Camera where R: DistortionFunction, - D: DistortionFunction, T: DistortionFunction, + P: DistortionFunction, DefaultAllocator: Allocator, - DefaultAllocator: Allocator, DefaultAllocator: Allocator, + DefaultAllocator: Allocator, { linear: CameraIntrinsics, fisheye: Fisheye, radial_distortion: R, tangential: [f64; 2], - tangential_distortion: D, - prism_distortion: [T; 2], + tangential_distortion: T, + prism_distortion: [P; 2], } -impl Camera +impl Camera where R: DistortionFunction, - D: DistortionFunction, T: DistortionFunction, + P: DistortionFunction, DefaultAllocator: Allocator, - DefaultAllocator: Allocator, DefaultAllocator: Allocator, + DefaultAllocator: Allocator, { - pub fn new( - linear: CameraIntrinsics, - radial_distortion: R, - tangential_distortion: D, - prism_distortion: [T; 2], - ) -> Self { - Self { - linear, - fisheye: Fisheye::default(), - radial_distortion, - tangential: [0.0, 0.0], - tangential_distortion, - prism_distortion, - } - } - /// Apply lens distortions to a point. #[rustfmt::skip] pub fn distort(&self, point: Point2) -> Point2 { @@ -136,58 +148,38 @@ where (ru * f - rd, f + 2.0 * ru * ru * df) }, 0.0, 10.0); let pu = point * ru / rd; - + // Newton-Raphson iteration let pu = newton2(|x| { - let (F, J) = self.with_jacobian(x.into()); - (F.coords - point.coords, J) + let (f, j) = self.with_jacobian(x.into()); + (f.coords - point.coords, j) }, pu.coords).unwrap(); pu.into() } /// Convert from rectilinear to the camera projection. - pub fn project(&self, point: Vector2) -> Vector2 { - let r = point.norm(); + pub fn project(&self, point: Point2) -> Point2 { + let r = point.coords.norm(); let rp = self.fisheye.evaluate(r); point * rp / r } /// Convert from camera projection to rectilinear. - pub fn unproject(&self, point: Vector2) -> Vector2 { - let rp = point.norm(); + pub fn unproject(&self, point: Point2) -> Point2 { + let rp = point.coords.norm(); let r = self.fisheye.inverse(rp); point * r / rp } - /// Computes $∇_{\vec x} \vec f\p{\vec x, \vec θ}$. + /// Computes $\p{f\p{\vec x, \vec θ}, ∇_{\vec x} \vec f\p{\vec x, \vec θ}}$. /// /// $$ - /// \mathbf{J}_{\vec f} = \begin{bmatrix} + /// ∇_{\vec x} \vec f\p{\vec x, \vec θ} = \begin{bmatrix} /// \frac{\partial f_x}{\partial x} & \frac{\partial f_x}{\partial y} \\\\[.8em] /// \frac{\partial f_y}{\partial x} & \frac{\partial f_y}{\partial y} /// \end{bmatrix} /// $$ /// - /// $$ - /// \begin{aligned} - /// \frac{\partial f_x}{\partial x} &= - /// f_r(r^2) + 2 ⋅ x^2 ⋅ f_r'(r^2) \\\\ &\phantom{=} - /// + \p{6⋅t_1 ⋅ x + 2 ⋅ t_2 ⋅ y} ⋅ f_t(r^2) \\\\ &\phantom{=} - /// + \p{2⋅x⋅\p{t_1 ⋅ x + t_2 ⋅ y} + t_1 ⋅ r^2} ⋅ 2 ⋅ x ⋅ f_t'(r^2) \\\\ &\phantom{=} - /// + 2 ⋅ x ⋅ f_{px}'(r^2) - /// \end{aligned} - /// $$ - /// - /// $$ - /// \begin{aligned} - /// \frac{\partial f_x}{\partial y} &= - /// 2 ⋅ x ⋅ y^2 ⋅ f_r'(r^2) \\\\ &\phantom{=} - /// + \p{2⋅t_1 ⋅ y + 2 ⋅ t_2 ⋅ x} ⋅ f_t(r^2) \\\\ &\phantom{=} - /// + \p{2⋅x⋅\p{t_1 ⋅ x + t_2 ⋅ y} + t_1 ⋅ r^2} ⋅ 2 ⋅ y ⋅ f_t'(r^2) \\\\ &\phantom{=} - /// + 2 ⋅ y ⋅ f_{px}'(r^2) - /// \end{aligned} - /// $$ - /// pub fn with_jacobian(&self, point: Point2) -> (Point2, Matrix2) { let [x, y] = [point.coords[0], point.coords[1]]; let r2 = x * x + y * y; @@ -214,6 +206,70 @@ where let vdy = f_r + tydy * f_t + vdr2 * r2dy; (Point2::new(u, v), Matrix2::new(udx, udy, vdx, vdy)) } + + /// Computes $∇_{\vec θ} \vec f\p{\vec x, \vec θ}$ + pub fn parameter_jacobian(&self, point: Point2) -> (Point2, Matrix2) { + todo!() + } +} + +type Dim

+where + P: DistortionFunction, + P::NumParameters: DimMul, + DimProd: DimAdd, += DimSum, U2>; + +impl Camera +where + R: DistortionFunction, + T: DistortionFunction, + P: DistortionFunction, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, + R::NumParameters: DimName, + T::NumParameters: DimName, + P::NumParameters: DimName + DimMul, + DimProd: DimName + DimAdd, + Dim

: DimName, + DefaultAllocator: Allocator>, +{ + pub fn parameters(&self) -> VectorN> { + let fisheye_params = self.fisheye.parameters(); + let radial_params = self.radial_distortion.parameters(); + let tangential_params = self.tangential_distortion.parameters(); + let prism_params_x = self.prism_distortion[0].parameters(); + let prism_params_y = self.prism_distortion[1].parameters(); + + let mut result = VectorN::>::zero(); + let mut offset = 0; + result + .fixed_slice_mut::(offset, 0) + .copy_from(&fisheye_params); + offset += fisheye_params.nrows(); + result + .fixed_slice_mut::(offset, 0) + .copy_from(&radial_params); + offset += radial_params.nrows(); + result[(offset, 0)] = self.tangential[0]; + offset += 1; + result[(offset, 0)] = self.tangential[1]; + offset += 1; + result + .fixed_slice_mut::(offset, 0) + .copy_from(&tangential_params); + offset += tangential_params.nrows(); + result + .fixed_slice_mut::(offset, 0) + .copy_from(&prism_params_x); + offset += prism_params_x.nrows(); + result + .fixed_slice_mut::(offset, 0) + .copy_from(&prism_params_y); + offset += prism_params_y.nrows(); + result + } } impl CameraModel for Camera @@ -228,33 +284,25 @@ where type Projection = NormalizedKeyPoint; fn calibrate(&self, point: Point) -> Self::Projection { - let NormalizedKeyPoint(distorted) = self.linear.calibrate(point); - let distorted_r = distorted.coords.norm(); - let corrected_r = self.radial_distortion.evaluate(distorted_r); - let r_factor = corrected_r / distorted_r; - let corrected = (distorted.coords * r_factor).into(); - - NormalizedKeyPoint(corrected) + let point = self.linear.calibrate(point).0; + let point = self.unproject(point); + let point = self.undistort(point); + NormalizedKeyPoint(point) } fn uncalibrate(&self, projection: Self::Projection) -> KeyPoint { - let NormalizedKeyPoint(corrected) = projection; - let corrected_r = corrected.coords.norm(); - let distorted_r = self.radial_distortion.inverse(corrected_r); - let r_factor = distorted_r / corrected_r; - let distorted = NormalizedKeyPoint((corrected.coords * r_factor).into()); - self.linear.uncalibrate(distorted).into() + let point = self.distort(projection.0); + let point = self.project(point); + self.linear.uncalibrate(NormalizedKeyPoint(point)) } } -pub mod camera { +pub mod models { use super::Camera; use crate::distortion_function::{self, Constant, DistortionFunction, Polynomial, Rational}; use crate::CameraIntrinsics; - use cv_core::nalgebra::{Matrix3, Point2, Vector2, Vector3, VectorN, U1, U8}; - use cv_core::nalgebra::{U2, U3, U4}; - - pub type Adobe = Camera, Constant, Constant>; + use cv_core::nalgebra::{Matrix3, Vector3, VectorN, U1, U8}; + use cv_core::nalgebra::{U3, U4}; /// Generate Adobe rectilinear camera model /// @@ -270,49 +318,57 @@ pub mod camera { /// * Implement Lateral Chromatic Aberration Model /// * Implement Vignette Model /// * Read/Write Lens Correction Profile File Format - pub fn adobe(parameters: [f64; 9]) -> Adobe { - todo!() + pub type Adobe = Camera, Constant, Constant>; + + impl Adobe { + pub fn adobe(_parameters: [f64; 9]) -> Adobe { + let mut camera = Adobe::default(); + todo!() + } } + /// OpenCV compatible model with up to 12 distortion coefficients pub type OpenCV = Camera, Constant, Polynomial>; - /// Generate OpenCV camera with up to twelve distortion coefficients. - pub fn opencv(cameraMatrix: Matrix3, distCoeffs: &[f64]) -> OpenCV { - assert!( - distCoeffs.len() <= 12, - "Up to 12 coefficients are supported." - ); - - // Zero pad coefficients - let mut coeffs = [0.0; 12]; - coeffs.copy_from_slice(distCoeffs); + impl OpenCV { + /// Generate OpenCV camera with up to twelve distortion coefficients. + pub fn opencv(camera_matrix: Matrix3, distortion_coefficients: &[f64]) -> OpenCV { + // Zero pad coefficients + assert!( + distortion_coefficients.len() <= 12, + "Up to 12 coefficients are supported." + ); + let mut coeffs = [0.0; 12]; + coeffs.copy_from_slice(distortion_coefficients); - let intrinsic = CameraIntrinsics::from_matrix(cameraMatrix); - #[rustfmt::skip] - let radial = Rational::::from_parameters( - VectorN::::from_column_slice_generic(U8, U1, &[ - 1.0, coeffs[0], coeffs[1], coeffs[4], - 1.0, coeffs[5], coeffs[6], coeffs[7], - ]) - ); - let tangential = distortion_function::one(); - let prism = [ - Polynomial::::from_parameters(Vector3::new(0.0, coeffs[8], coeffs[9])), - Polynomial::::from_parameters(Vector3::new(0.0, coeffs[10], coeffs[11])), - ]; - - let mut camera = OpenCV::new(intrinsic, radial, tangential, prism); - camera.tangential = [coeffs[2], coeffs[3]]; - camera + // Construct components + let mut camera = OpenCV::default(); + camera.linear = CameraIntrinsics::from_matrix(camera_matrix); + camera.radial_distortion = + Rational::from_parameters(VectorN::from_column_slice_generic( + U8, + U1, + &[ + 1.0, coeffs[0], coeffs[1], coeffs[4], 1.0, coeffs[5], coeffs[6], coeffs[7], + ], + )); + camera.tangential = [coeffs[2], coeffs[3]]; + camera.tangential_distortion = distortion_function::one(); + camera.prism_distortion = [ + Polynomial::::from_parameters(Vector3::new(0.0, coeffs[8], coeffs[9])), + Polynomial::::from_parameters(Vector3::new(0.0, coeffs[10], coeffs[11])), + ]; + camera + } } } #[cfg(test)] mod tests { use super::*; - use crate::distortion_function::{self, Polynomial, Rational}; - use cv_core::nalgebra::{Matrix3, Point2, Vector3, VectorN, U1, U3, U4, U8}; + use cv_core::nalgebra::{Matrix3, Point2}; use float_eq::assert_float_eq; + use models::OpenCV; #[rustfmt::skip] const UNDISTORTED: &[[f64; 2]] = &[ @@ -372,10 +428,10 @@ mod tests { [ 1.1055970389194876 , 0.8317875937454029 ], ]; - fn camera_1() -> camera::OpenCV { + fn camera_1() -> OpenCV { // Calibration parameters for a GoPro Hero 6 using OpenCV #[rustfmt::skip] - let cameraMatrix = Matrix3::from_row_slice(&[ + let camera_matrix = Matrix3::from_row_slice(&[ 1.77950719e+03, 0.00000000e+00, 2.03258212e+03, 0.00000000e+00, 1.77867606e+03, 1.52051932e+03, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]); @@ -385,7 +441,7 @@ mod tests { 1.09310463e-03, 6.72953083e-01, 2.59544797e+01, 2.24213453e+01, 3.04318306e+00, -3.23278793e-03, 9.53176056e-05, -9.35687185e-05, 2.96341863e-05]; - camera::opencv(cameraMatrix, &distortion) + OpenCV::opencv(camera_matrix, &distortion) } #[test] diff --git a/cv-pinhole/src/distortion_function/fisheye.rs b/cv-pinhole/src/distortion_function/fisheye.rs index f19b6868..8ff29873 100644 --- a/cv-pinhole/src/distortion_function/fisheye.rs +++ b/cv-pinhole/src/distortion_function/fisheye.rs @@ -16,8 +16,17 @@ use cv_core::nalgebra::{storage::Storage, Vector, Vector1, VectorN, U1}; /// /// Varying $k ∈ [-1, 1]$ will gradually transform from orthographic to rectilinear projection. In /// particular for $k = 1$ the equation simplifies to $r' = r$ representing the non-Fisheye -/// rectilinear projection. +/// rectilinear projection. Other named values are: /// +/// | $k$ | name | projection | +/// |------------:|---------------|-----------------------| +/// | $-1$ | orthographic | $r = \sin θ$ | +/// | $- 1/2$ | equisolid | $r = 2 \sin θ/2$ | +/// | $0$ | equidistant | $r = \tan θ$ | +/// | $1/2$ | stereographic | $r = 2 \tan θ/2$ | +/// | $1$ | rectilinear | $r = \tan θ$ | +/// +/// Wikipedia has an excellent [comparison][wiki] of the different projections. /// # References /// /// * Wikipedia Fisheye Lens. [link][wiki]. @@ -146,10 +155,10 @@ mod tests { proptest!(|(k in -1.0..1.0, r in 0.0..0.75)| { test(k, r); }); - proptest!(|(k in -1.0..1.0, r in 0.0..0.75)| { + proptest!(|(r in 0.0..0.75)| { test(0.0, r); }); - proptest!(|(k in -1.0..1.0, r in 0.0..0.75)| { + proptest!(|(r in 0.0..0.75)| { test(1.0, r); }); } diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs index cade731b..c14b2523 100644 --- a/cv-pinhole/src/distortion_function/mod.rs +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -159,3 +159,56 @@ pub fn constant(value: f64) -> Constant { pub fn identity() -> Identity { Identity::from_parameters(Vector2::new(0.0, 1.0)) } + +#[cfg(test)] +pub(crate) use test::TestFloat; +#[cfg(test)] +mod test { + use super::*; + use rug::Float; + + // Internal precision used to compute exact f64 values. + const INTERNAL_PRECISION: u32 = 1000; + + pub(crate) trait TestFloat: DistortionFunction + where + DefaultAllocator: Allocator, + { + fn evaluate_float(&self, x: &Float) -> Float; + + fn derivative_float(&self, x: &Float) -> Float { + let prec = x.prec(); + let epsilon = Float::with_val(prec, Float::i_exp(1, 1 - (prec as i32))); + + // Relative stepsize `h` is the cube root of epsilon + let h: Float = x * epsilon.root(3); + let xh = x + h.clone(); + let xl = x - h.clone(); + let deriv = (self.evaluate_float(&xh) - self.evaluate_float(&xl)) / (2 * h); + // `deriv` should be accurate to about 2/3 of `prec`. + deriv + } + + fn with_derivative_float(&self, x: &Float) -> (Float, Float) { + (self.evaluate_float(x), self.derivative_float(x)) + } + + fn evaluate_exact(&self, x: f64) -> f64 { + let x = Float::with_val(INTERNAL_PRECISION, x); + self.evaluate_float(&x).to_f64() + } + + fn derivative_exact(&self, x: f64) -> f64 { + let x = Float::with_val(INTERNAL_PRECISION, x); + self.derivative_float(&x).to_f64() + } + + fn with_derivative_exact(&self, x: f64) -> (f64, f64) { + let x = Float::with_val(INTERNAL_PRECISION, x); + let (value, derivative) = self.with_derivative_float(&x); + (value.to_f64(), derivative.to_f64()) + } + + // TODO: gradient_exact + } +} diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index 9744098b..4a27eb31 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -6,7 +6,7 @@ use super::DistortionFunction; use cv_core::nalgebra::{ allocator::Allocator, storage::Storage, DefaultAllocator, Dim, Vector, VectorN, U1, }; -use num_traits::{Float, Zero}; +use num_traits::Zero; /// Polynomial distortion function /// @@ -104,32 +104,23 @@ where #[cfg(test)] mod tests { use super::*; + use crate::distortion_function::TestFloat; use cv_core::nalgebra::{Vector4, U4}; use float_eq::assert_float_eq; use proptest::prelude::*; use rug::Float; - impl Polynomial + impl TestFloat for Polynomial where DefaultAllocator: Allocator, { - pub fn with_derivative_float(&self, x: Float) -> (Float, Float) { + fn evaluate_float(&self, x: &Float) -> Float { let mut value = Float::new(x.prec()); - let mut derivative = Float::new(x.prec()); for i in (0..self.0.nrows()).rev() { - derivative *= &x; - derivative += &value; - value *= &x; + value *= x; value += self.0[i]; } - (value, derivative) - } - - pub fn with_derivative_exact(&self, x: f64) -> (f64, f64) { - // Compute with 1000 bits accuracy - let x = Float::with_val(1000, x); - let (value, derivative) = self.with_derivative_float(x); - (value.to_f64(), derivative.to_f64()) + value } } diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index b5f2371d..3162513f 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -4,7 +4,7 @@ use cv_core::nalgebra::{ allocator::Allocator, storage::Storage, DefaultAllocator, Dim, DimAdd, DimName, DimSum, SliceStorage, Vector, VectorN, U1, }; -use num_traits::{Float, Zero}; +use num_traits::Zero; /// Rational distortion function. /// @@ -28,6 +28,23 @@ where DefaultAllocator: Allocator, DefaultAllocator: Allocator>; +impl Default for Rational +where + DP: DimName, + DQ: DimName, + DP: DimAdd, + DimSum: DimName, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator, + DefaultAllocator: Allocator>, + Polynomial: Default, + Polynomial: Default, +{ + fn default() -> Self { + Self(Polynomial::default(), Polynomial::default()) + } +} + impl DistortionFunction for Rational where DP: DimName, @@ -159,13 +176,14 @@ where #[cfg(test)] mod tests { use super::*; + use crate::distortion_function::TestFloat; use cv_core::nalgebra::{VectorN, U4, U8}; use float_eq::assert_float_eq; use proptest::prelude::*; use rug::ops::Pow; use rug::Float; - impl Rational + impl TestFloat for Rational where DP: DimName, DQ: DimName, @@ -174,18 +192,11 @@ mod tests { DefaultAllocator: Allocator, DefaultAllocator: Allocator, DefaultAllocator: Allocator>, + Polynomial: TestFloat, + Polynomial: TestFloat, { - pub fn with_derivative_float(&self, x: Float) -> (Float, Float) { - let (p, dp) = self.0.with_derivative_float(x.clone()); - let (q, dq) = self.1.with_derivative_float(x); - (p.clone() / &q, (dp * &q - p * dq) / q.pow(2)) - } - - pub fn with_derivative_exact(&self, x: f64) -> (f64, f64) { - // Compute with 1000 bits accuracy - let x = Float::with_val(1000, x); - let (value, derivative) = self.with_derivative_float(x); - (value.to_f64(), derivative.to_f64()) + fn evaluate_float(&self, x: &Float) -> Float { + self.0.evaluate_float(x) / self.1.evaluate_float(x) } } @@ -217,7 +228,7 @@ mod tests { proptest!(|(f in function(), x in 0.0..2.0)| { let value = f.evaluate(x); let expected = f.with_derivative_exact(x).0; - assert_float_eq!(value, expected, rmax <= 2.0 * f64::EPSILON); + assert_float_eq!(value, expected, rmax <= 3.0 * f64::EPSILON); }); } @@ -226,7 +237,7 @@ mod tests { proptest!(|(f in function(), x in 0.0..2.0)| { let value = f.with_derivative(x); let expected = f.with_derivative_exact(x); - assert_float_eq!(value.0, expected.0, rmax <= 2.0 * f64::EPSILON); + assert_float_eq!(value.0, expected.0, rmax <= 3.0 * f64::EPSILON); assert_float_eq!(value.1, expected.1, rmax <= 94.0 * f64::EPSILON); }); } diff --git a/cv-pinhole/src/lib.rs b/cv-pinhole/src/lib.rs index 92126da7..3ed4b3ca 100644 --- a/cv-pinhole/src/lib.rs +++ b/cv-pinhole/src/lib.rs @@ -13,7 +13,7 @@ pub mod distortion_function; mod essential; mod root; -pub use camera::Camera; +pub use camera::{models, Camera}; pub use essential::*; pub(crate) use root::{newton2, root}; @@ -157,6 +157,12 @@ impl CameraIntrinsics { } } +impl Default for CameraIntrinsics { + fn default() -> Self { + Self::identity() + } +} + impl CameraModel for CameraIntrinsics { type Projection = NormalizedKeyPoint; diff --git a/cv-pinhole/src/root.rs b/cv-pinhole/src/root.rs index e3640fa6..def07ba0 100644 --- a/cv-pinhole/src/root.rs +++ b/cv-pinhole/src/root.rs @@ -81,8 +81,8 @@ where let mut x = initial; let mut last_delta_norm = f64::MAX; for iter in 0.. { - let (F, J) = func(x); - let delta = J.lu().solve(&F).unwrap(); + let (f, j) = func(x); + let delta = j.lu().solve(&f).unwrap(); let delta_norm = delta.norm_squared(); x -= delta; if delta_norm < x.norm_squared() * f64::EPSILON * f64::EPSILON { From d29ad5ea3de7a61a738ddf1241e0f5cc4c42c263 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Fri, 27 Nov 2020 13:24:12 -0800 Subject: [PATCH 22/26] DRY the tests --- cv-pinhole/src/distortion_function/fisheye.rs | 77 +++++------------ cv-pinhole/src/distortion_function/mod.rs | 65 +++++++++++++-- .../src/distortion_function/polynomial.rs | 70 ++-------------- .../src/distortion_function/rational.rs | 83 ++----------------- 4 files changed, 98 insertions(+), 197 deletions(-) diff --git a/cv-pinhole/src/distortion_function/fisheye.rs b/cv-pinhole/src/distortion_function/fisheye.rs index 8ff29873..255ba3f0 100644 --- a/cv-pinhole/src/distortion_function/fisheye.rs +++ b/cv-pinhole/src/distortion_function/fisheye.rs @@ -100,68 +100,33 @@ impl DistortionFunction for Fisheye { #[cfg(test)] mod tests { use super::*; + use crate::distortion_function::test::TestFloat; + use crate::distortion_test_generate; use cv_core::nalgebra::Vector1; use float_eq::assert_float_eq; use proptest::prelude::*; + use rug::Float; - #[test] - fn test_finite_difference() { - let h = f64::EPSILON.powf(1.0 / 3.0); - let test = |k, r| { - let fisheye = Fisheye::from_parameters(Vector1::new(k)); - let h = f64::max(h * 0.1, h * r); - let deriv = fisheye.derivative(r); - let approx = (fisheye.evaluate(r + h) - fisheye.evaluate(r - h)) / (2.0 * h); - assert_float_eq!(deriv, approx, rmax <= 2e2 * h * h); - }; - proptest!(|(k in -1.0..1.0, r in 0.0..1.0)| { - test(k, r); - }); - proptest!(|(r in 0.0..1.0)| { - test(0.0, r); - }); - proptest!(|(r in 0.0..1.0)| { - test(1.0, r); - }); - } - - #[test] - fn test_roundtrip_forward() { - let test = |k, r| { - let fisheye = Fisheye::from_parameters(Vector1::new(k)); - let eval = fisheye.evaluate(r); - let inv = fisheye.inverse(eval); - assert_float_eq!(inv, r, rmax <= 1e1 * f64::EPSILON); - }; - proptest!(|(k in -1.0..1.0, r in 0.0..2.0)| { - test(k, r); - }); - proptest!(|(r in 0.0..1.0)| { - test(0.0, r); - }); - proptest!(|(r in 0.0..1.0)| { - test(1.0, r); - }); + impl TestFloat for Fisheye { + fn evaluate_float(&self, x: &Float) -> Float { + let x = x.clone(); + match self.0 { + k if k < 0.0 => (k * x.atan()).sin() / k, + k if k == 0.0 => x.atan(), + k if k < 1.0 => (k * x.atan()).tan() / k, + _ => x, + } + } } - #[test] - fn test_roundtrip_reverse() { - let test = |k, r| { - let fisheye = Fisheye::from_parameters(Vector1::new(k)); - let inv = fisheye.inverse(r); - let eval = fisheye.evaluate(inv); - assert_float_eq!(eval, r, rmax <= 1e1 * f64::EPSILON); - }; - proptest!(|(k in -1.0..1.0, r in 0.0..0.75)| { - test(k, r); - }); - proptest!(|(r in 0.0..0.75)| { - test(0.0, r); - }); - proptest!(|(r in 0.0..0.75)| { - test(1.0, r); - }); + fn function() -> impl Strategy { + (-1_f64..1.).prop_map(Fisheye) } - // TODO: Test parameter gradient using finite differences. + distortion_test_generate!( + function(), + evaluate_eps = 6.0, + derivative_eps = 9.0, + inverse_eps = 4.0, + ); } diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs index c14b2523..8c572ee8 100644 --- a/cv-pinhole/src/distortion_function/mod.rs +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -161,9 +161,7 @@ pub fn identity() -> Identity { } #[cfg(test)] -pub(crate) use test::TestFloat; -#[cfg(test)] -mod test { +pub(crate) mod test { use super::*; use rug::Float; @@ -181,9 +179,12 @@ mod test { let epsilon = Float::with_val(prec, Float::i_exp(1, 1 - (prec as i32))); // Relative stepsize `h` is the cube root of epsilon - let h: Float = x * epsilon.root(3); - let xh = x + h.clone(); - let xl = x - h.clone(); + let h: Float = (x * epsilon.clone().root(3)).max(&epsilon); + let x = if x > &h { x } else { &h }; + let (xl, xh) = (x - h.clone(), x + h.clone()); + assert!(xl >= 0); + assert!(xh > xl); + assert_eq!(xh.clone() - &xl, 2 * h.clone()); let deriv = (self.evaluate_float(&xh) - self.evaluate_float(&xl)) / (2 * h); // `deriv` should be accurate to about 2/3 of `prec`. deriv @@ -211,4 +212,56 @@ mod test { // TODO: gradient_exact } + + #[macro_export] + macro_rules! distortion_test_generate { + ( + $function:expr, + evaluate_eps = $eval:expr, + derivative_eps = $deriv:expr, + inverse_eps = $inv:expr, + ) => { + #[test] + fn test_evaluate() { + proptest!(|(f in $function, x in 0.0..2.0)| { + let value = f.evaluate(x); + let expected = f.evaluate_exact(x); + assert_float_eq!(value, expected, rmax <= $eval * f64::EPSILON); + }); + } + + #[test] + fn test_derivative() { + proptest!(|(f in $function, x in 0.0..2.0)| { + let value = f.derivative(x); + let expected = f.derivative_exact(x); + assert_float_eq!(value, expected, rmax <= $deriv * f64::EPSILON); + }); + } + + #[test] + fn test_with_derivative() { + proptest!(|(f in $function, x in 0.0..2.0)| { + let value = f.with_derivative(x); + let expected = f.with_derivative_exact(x); + assert_float_eq!(value.0, expected.0, rmax <= $eval * f64::EPSILON); + assert_float_eq!(value.1, expected.1, rmax <= $deriv * f64::EPSILON); + }); + } + + #[test] + fn test_inverse() { + proptest!(|(f in $function, x in 0.0..2.0)| { + let y = f.evaluate_exact(x); + let value = f.inverse(y); + // There may be a multiple valid inverses, so instead of checking + // the answer directly by `value == x`, we check that `f(value) == f(x)`. + let y2 = f.evaluate_exact(value); + assert_float_eq!(y, y2, rmax <= $inv * f64::EPSILON); + }); + } + + // TODO: Test parameter gradient using finite differences. + } + } } diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index 4a27eb31..9df39c04 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -104,7 +104,8 @@ where #[cfg(test)] mod tests { use super::*; - use crate::distortion_function::TestFloat; + use crate::distortion_function::test::TestFloat; + use crate::distortion_test_generate; use cv_core::nalgebra::{Vector4, U4}; use float_eq::assert_float_eq; use proptest::prelude::*; @@ -140,6 +141,13 @@ mod tests { (0_usize..POLYS.len()).prop_map(functions) } + distortion_test_generate!( + function(), + evaluate_eps = 2.0, + derivative_eps = 3.0, + inverse_eps = 2.0, + ); + #[test] fn test_evaluate_literal() { let f = functions(0); @@ -148,65 +156,5 @@ mod tests { assert_float_eq!(value, 2.86874326561548, ulps <= 0); } - #[test] - fn test_evaluate() { - proptest!(|(f in function(), x in 0.0..2.0)| { - let value = f.evaluate(x); - let expected = f.with_derivative_exact(x).0; - assert_float_eq!(value, expected, rmax <= 2.0 * f64::EPSILON); - }); - } - - #[test] - fn test_with_derivative() { - proptest!(|(f in function(), x in 0.0..2.0)| { - let value = f.with_derivative(x); - let expected = f.with_derivative_exact(x); - assert_float_eq!(value.0, expected.0, rmax <= 2.0 * f64::EPSILON); - assert_float_eq!(value.1, expected.1, rmax <= 1e3 * f64::EPSILON); - }); - } - - #[test] - fn test_inverse() { - proptest!(|(f in function(), x in 0.0..2.0)| { - let y = f.with_derivative_exact(x).0; - let value = f.inverse(y); - // There may be a multiple valid inverses, so instead of checking - // the answer directly by `value == x`, we check that `f(value) == f(x)`. - let y2 = f.with_derivative_exact(value).0; - assert_float_eq!(y, y2, rmax <= 2.0 * f64::EPSILON); - }); - } - - #[test] - fn test_finite_difference() { - let h = f64::EPSILON.powf(1.0 / 3.0); - proptest!(|(f in function(), x in 0.0..2.0)| { - let h = f64::max(h * 0.1, h * x); - let deriv = f.derivative(x); - let approx = (f.evaluate(x + h) - f.evaluate(x - h)) / (2.0 * h); - assert_float_eq!(deriv, approx, rmax <= 1e3 * h * h); - }); - } - - #[test] - fn test_roundtrip_forward() { - proptest!(|(f in function(), x in 0.0..2.0)| { - let eval = f.evaluate(x); - let inv = f.inverse(eval); - assert_float_eq!(inv, x, rmax <= 1e4 * f64::EPSILON); - }); - } - - #[test] - fn test_roundtrip_reverse() { - proptest!(|(f in function(), x in 1.0..2.0)| { - let inv = f.inverse(x); - let eval = f.evaluate(inv); - assert_float_eq!(eval, x, rmax <= 1e4 * f64::EPSILON); - }); - } - // TODO: Test parameter gradient using finite differences. } diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index 3162513f..1855f27b 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -176,7 +176,8 @@ where #[cfg(test)] mod tests { use super::*; - use crate::distortion_function::TestFloat; + use crate::distortion_function::test::TestFloat; + use crate::distortion_test_generate; use cv_core::nalgebra::{VectorN, U4, U8}; use float_eq::assert_float_eq; use proptest::prelude::*; @@ -215,6 +216,13 @@ mod tests { (0_usize..1).prop_map(functions) } + distortion_test_generate!( + function(), + evaluate_eps = 3.0, + derivative_eps = 25.0, + inverse_eps = 2.0, + ); + #[test] fn test_evaluate_literal() { let f = functions(0); @@ -222,77 +230,4 @@ mod tests { let value = f.evaluate(x); assert_float_eq!(value, 0.9810452524397972, ulps <= 0); } - - #[test] - fn test_evaluate() { - proptest!(|(f in function(), x in 0.0..2.0)| { - let value = f.evaluate(x); - let expected = f.with_derivative_exact(x).0; - assert_float_eq!(value, expected, rmax <= 3.0 * f64::EPSILON); - }); - } - - #[test] - fn test_with_derivative() { - proptest!(|(f in function(), x in 0.0..2.0)| { - let value = f.with_derivative(x); - let expected = f.with_derivative_exact(x); - assert_float_eq!(value.0, expected.0, rmax <= 3.0 * f64::EPSILON); - assert_float_eq!(value.1, expected.1, rmax <= 94.0 * f64::EPSILON); - }); - } - - #[test] - fn test_inverse() { - proptest!(|(f in function(), x in 0.0..2.0)| { - let y = f.with_derivative_exact(x).0; - let value = f.inverse(y); - // There may be a multiple valid inverses, so instead of checking - // the answer directly by `value == x`, we check that `f(value) == f(x)`. - let y2 = f.with_derivative_exact(value).0; - assert_float_eq!(y, y2, rmax <= 2.0 * f64::EPSILON); - }); - } - - #[test] - fn test_inverse_1() { - let radial = functions(0); - let result = radial.inverse(0.9810452524397972); - assert_float_eq!(result, 0.06987809296337322, ulps <= 8); - } - - #[test] - fn test_finite_difference_1() { - let h = f64::EPSILON.powf(1.0 / 3.0); - let radial = functions(0); - proptest!(|(r in 0.0..2.0)| { - let h = f64::max(h * 0.1, h * r); - let deriv = radial.derivative(r); - let approx = (radial.evaluate(r + h) - radial.evaluate(r - h)) / (2.0 * h); - assert_float_eq!(deriv, approx, rmax <= 1e4 * h * h); - }); - } - - #[test] - fn test_roundtrip_forward_1() { - let radial = functions(0); - proptest!(|(r in 0.0..2.0)| { - let eval = radial.evaluate(r); - let inv = radial.inverse(eval); - let eval2 = radial.evaluate(inv); - assert_float_eq!(eval, eval2, rmax <= 3.0 * f64::EPSILON); - }); - } - - #[test] - fn test_roundtrip_reverse_1() { - let radial = functions(0); - proptest!(|(r in 0.7..1.0)| { - let inv = radial.inverse(r); - let eval = radial.evaluate(inv); - assert_float_eq!(eval, r, rmax <= 3.0 * f64::EPSILON); - }); - } - - // TODO: Test parameter gradient using finite differences. } From 69138caa9b7169c15945ffda2630d91484f1b1c9 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Fri, 27 Nov 2020 13:33:04 -0800 Subject: [PATCH 23/26] Fix lints --- cv-pinhole/src/camera.rs | 14 ++++---------- cv-pinhole/src/distortion_function/fisheye.rs | 2 -- cv-pinhole/src/distortion_function/mod.rs | 18 +++++++++--------- cv-pinhole/src/distortion_function/rational.rs | 1 - 4 files changed, 13 insertions(+), 22 deletions(-) diff --git a/cv-pinhole/src/camera.rs b/cv-pinhole/src/camera.rs index 0aa7c4ab..1eba3320 100644 --- a/cv-pinhole/src/camera.rs +++ b/cv-pinhole/src/camera.rs @@ -5,7 +5,7 @@ use crate::{newton2, root}; use cv_core::nalgebra::{ allocator::Allocator, DefaultAllocator, DimAdd, DimMul, DimName, DimProd, DimSum, Matrix2, - Point2, Vector2, VectorN, U1, U2, + Point2, VectorN, U1, U2, }; use cv_core::{CameraModel, ImagePoint, KeyPoint}; use num_traits::Zero; @@ -208,17 +208,12 @@ where } /// Computes $∇_{\vec θ} \vec f\p{\vec x, \vec θ}$ - pub fn parameter_jacobian(&self, point: Point2) -> (Point2, Matrix2) { + pub fn parameter_jacobian(&self, _point: Point2) -> (Point2, Matrix2) { todo!() } } -type Dim

-where - P: DistortionFunction, - P::NumParameters: DimMul, - DimProd: DimAdd, -= DimSum, U2>; +type Dim

= DimSum::NumParameters, U2>, U2>; impl Camera where @@ -267,7 +262,6 @@ where result .fixed_slice_mut::(offset, 0) .copy_from(&prism_params_y); - offset += prism_params_y.nrows(); result } } @@ -322,7 +316,7 @@ pub mod models { impl Adobe { pub fn adobe(_parameters: [f64; 9]) -> Adobe { - let mut camera = Adobe::default(); + let mut _camera = Adobe::default(); todo!() } } diff --git a/cv-pinhole/src/distortion_function/fisheye.rs b/cv-pinhole/src/distortion_function/fisheye.rs index 255ba3f0..522b98b4 100644 --- a/cv-pinhole/src/distortion_function/fisheye.rs +++ b/cv-pinhole/src/distortion_function/fisheye.rs @@ -102,8 +102,6 @@ mod tests { use super::*; use crate::distortion_function::test::TestFloat; use crate::distortion_test_generate; - use cv_core::nalgebra::Vector1; - use float_eq::assert_float_eq; use proptest::prelude::*; use rug::Float; diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs index 8c572ee8..55a5f0e1 100644 --- a/cv-pinhole/src/distortion_function/mod.rs +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -223,41 +223,41 @@ pub(crate) mod test { ) => { #[test] fn test_evaluate() { - proptest!(|(f in $function, x in 0.0..2.0)| { + proptest::proptest!(|(f in $function, x in 0.0..2.0)| { let value = f.evaluate(x); let expected = f.evaluate_exact(x); - assert_float_eq!(value, expected, rmax <= $eval * f64::EPSILON); + float_eq::assert_float_eq!(value, expected, rmax <= $eval * f64::EPSILON); }); } #[test] fn test_derivative() { - proptest!(|(f in $function, x in 0.0..2.0)| { + proptest::proptest!(|(f in $function, x in 0.0..2.0)| { let value = f.derivative(x); let expected = f.derivative_exact(x); - assert_float_eq!(value, expected, rmax <= $deriv * f64::EPSILON); + float_eq::assert_float_eq!(value, expected, rmax <= $deriv * f64::EPSILON); }); } #[test] fn test_with_derivative() { - proptest!(|(f in $function, x in 0.0..2.0)| { + proptest::proptest!(|(f in $function, x in 0.0..2.0)| { let value = f.with_derivative(x); let expected = f.with_derivative_exact(x); - assert_float_eq!(value.0, expected.0, rmax <= $eval * f64::EPSILON); - assert_float_eq!(value.1, expected.1, rmax <= $deriv * f64::EPSILON); + float_eq::assert_float_eq!(value.0, expected.0, rmax <= $eval * f64::EPSILON); + float_eq::assert_float_eq!(value.1, expected.1, rmax <= $deriv * f64::EPSILON); }); } #[test] fn test_inverse() { - proptest!(|(f in $function, x in 0.0..2.0)| { + proptest::proptest!(|(f in $function, x in 0.0..2.0)| { let y = f.evaluate_exact(x); let value = f.inverse(y); // There may be a multiple valid inverses, so instead of checking // the answer directly by `value == x`, we check that `f(value) == f(x)`. let y2 = f.evaluate_exact(value); - assert_float_eq!(y, y2, rmax <= $inv * f64::EPSILON); + float_eq::assert_float_eq!(y, y2, rmax <= $inv * f64::EPSILON); }); } diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index 1855f27b..2a90d55e 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -181,7 +181,6 @@ mod tests { use cv_core::nalgebra::{VectorN, U4, U8}; use float_eq::assert_float_eq; use proptest::prelude::*; - use rug::ops::Pow; use rug::Float; impl TestFloat for Rational From 888d93c55ce2d833576c857ec965584249989c97 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Fri, 27 Nov 2020 14:19:58 -0800 Subject: [PATCH 24/26] Fix rational test --- cv-pinhole/src/distortion_function/mod.rs | 7 +++++-- cv-pinhole/src/distortion_function/rational.rs | 2 +- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs index 55a5f0e1..2108012a 100644 --- a/cv-pinhole/src/distortion_function/mod.rs +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -179,13 +179,16 @@ pub(crate) mod test { let epsilon = Float::with_val(prec, Float::i_exp(1, 1 - (prec as i32))); // Relative stepsize `h` is the cube root of epsilon - let h: Float = (x * epsilon.clone().root(3)).max(&epsilon); + let h: Float = epsilon.clone().root(3); + let h: Float = (x * epsilon.clone().root(3)).max(&h); let x = if x > &h { x } else { &h }; let (xl, xh) = (x - h.clone(), x + h.clone()); assert!(xl >= 0); assert!(xh > xl); assert_eq!(xh.clone() - &xl, 2 * h.clone()); - let deriv = (self.evaluate_float(&xh) - self.evaluate_float(&xl)) / (2 * h); + let yl = self.evaluate_float(&xl); + let yh = self.evaluate_float(&xh); + let deriv = (yh - yl) / (xh - xl); // `deriv` should be accurate to about 2/3 of `prec`. deriv } diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index 2a90d55e..118da735 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -218,7 +218,7 @@ mod tests { distortion_test_generate!( function(), evaluate_eps = 3.0, - derivative_eps = 25.0, + derivative_eps = 120.0, inverse_eps = 2.0, ); From 368dec3f6153b26122d0f1593613c2d14482bcaf Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Sat, 28 Nov 2020 11:34:43 -0800 Subject: [PATCH 25/26] Test gradients --- cv-pinhole/src/distortion_function/mod.rs | 39 ++++++++++++++++++- .../src/distortion_function/polynomial.rs | 6 ++- 2 files changed, 41 insertions(+), 4 deletions(-) diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs index 2108012a..1ac5db2f 100644 --- a/cv-pinhole/src/distortion_function/mod.rs +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -163,6 +163,8 @@ pub fn identity() -> Identity { #[cfg(test)] pub(crate) mod test { use super::*; + use cv_core::nalgebra::DimName; + use num_traits::Zero; use rug::Float; // Internal precision used to compute exact f64 values. @@ -170,6 +172,7 @@ pub(crate) mod test { pub(crate) trait TestFloat: DistortionFunction where + Self::NumParameters: DimName, DefaultAllocator: Allocator, { fn evaluate_float(&self, x: &Float) -> Float; @@ -213,7 +216,30 @@ pub(crate) mod test { (value.to_f64(), derivative.to_f64()) } - // TODO: gradient_exact + fn gradient_exact(&self, x: f64) -> VectorN { + let x = Float::with_val(INTERNAL_PRECISION, x); + let theta = self.parameters(); + let mut gradient = VectorN::::zero(); + for i in 0..gradient.nrows() { + let p = theta[(i, 0)]; + let h = if p.is_normal() { + p * f64::EPSILON + } else { + f64::EPSILON * f64::EPSILON + }; + let tl = p - h; + let th = p + h; + let mut theta_l = theta.clone(); + theta_l[(i, 0)] = tl; + let mut theta_h = theta.clone(); + theta_h[(i, 0)] = th; + let yl = Self::from_parameters(theta_l).evaluate_float(&x); + let yh = Self::from_parameters(theta_h).evaluate_float(&x); + let deriv = (yh - yl) / (th - tl); + gradient[(i, 0)] = deriv.to_f64(); + } + gradient + } } #[macro_export] @@ -264,7 +290,16 @@ pub(crate) mod test { }); } - // TODO: Test parameter gradient using finite differences. + #[test] + fn test_gradient() { + proptest::proptest!(|(f in $function, x in 0.0..2.0)| { + let value = f.gradient(x); + let expected = f.gradient_exact(x); + for (value, expected) in value.iter().zip(expected.iter()) { + float_eq::assert_float_eq!(value, expected, rmax <= $deriv * f64::EPSILON); + } + }); + } } } } diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index 9df39c04..3622a99b 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -106,13 +106,14 @@ mod tests { use super::*; use crate::distortion_function::test::TestFloat; use crate::distortion_test_generate; - use cv_core::nalgebra::{Vector4, U4}; + use cv_core::nalgebra::{DimName, Vector4, U4}; use float_eq::assert_float_eq; use proptest::prelude::*; use rug::Float; - impl TestFloat for Polynomial + impl TestFloat for Polynomial where + Degree: Dim + DimName, DefaultAllocator: Allocator, { fn evaluate_float(&self, x: &Float) -> Float { @@ -153,6 +154,7 @@ mod tests { let f = functions(0); let x = 0.06987809296337355; let value = f.evaluate(x); + let expected = f.gradient(x); assert_float_eq!(value, 2.86874326561548, ulps <= 0); } From 51de89584e798b6247d17f71383992ff2e80b235 Mon Sep 17 00:00:00 2001 From: Remco Bloemen Date: Sat, 28 Nov 2020 13:32:08 -0800 Subject: [PATCH 26/26] Implement fisheye gradient --- cv-pinhole/src/distortion_function/fisheye.rs | 18 ++++++++++++++++-- cv-pinhole/src/distortion_function/mod.rs | 6 +++++- .../src/distortion_function/polynomial.rs | 2 +- cv-pinhole/src/distortion_function/rational.rs | 3 ++- 4 files changed, 24 insertions(+), 5 deletions(-) diff --git a/cv-pinhole/src/distortion_function/fisheye.rs b/cv-pinhole/src/distortion_function/fisheye.rs index 522b98b4..1354adc9 100644 --- a/cv-pinhole/src/distortion_function/fisheye.rs +++ b/cv-pinhole/src/distortion_function/fisheye.rs @@ -92,8 +92,21 @@ impl DistortionFunction for Fisheye { } } - fn gradient(&self, _value: f64) -> VectorN { - todo!() + fn gradient(&self, value: f64) -> VectorN { + Vector1::new(match self.0 { + k if k < 0.0 => { + let theta = value.atan(); + let kt = k * theta; + (theta * kt.cos() - kt.sin() / k) / k + } + k if k == 0.0 => 0.0, + k if k < 1.0 => { + let theta = value.atan(); + let kt = k * theta; + (theta * (1.0 + kt.tan().powi(2)) - kt.tan() / k) / k + } + _ => value, + }) } } @@ -126,5 +139,6 @@ mod tests { evaluate_eps = 6.0, derivative_eps = 9.0, inverse_eps = 4.0, + gradient_eps = 1e10, ); } diff --git a/cv-pinhole/src/distortion_function/mod.rs b/cv-pinhole/src/distortion_function/mod.rs index 1ac5db2f..97a8b452 100644 --- a/cv-pinhole/src/distortion_function/mod.rs +++ b/cv-pinhole/src/distortion_function/mod.rs @@ -249,6 +249,7 @@ pub(crate) mod test { evaluate_eps = $eval:expr, derivative_eps = $deriv:expr, inverse_eps = $inv:expr, + gradient_eps = $grad:expr, ) => { #[test] fn test_evaluate() { @@ -296,7 +297,10 @@ pub(crate) mod test { let value = f.gradient(x); let expected = f.gradient_exact(x); for (value, expected) in value.iter().zip(expected.iter()) { - float_eq::assert_float_eq!(value, expected, rmax <= $deriv * f64::EPSILON); + float_eq::assert_float_eq!(value, expected, + rmax <= $grad * f64::EPSILON, + abs <= f64::EPSILON * f64::EPSILON + ); } }); } diff --git a/cv-pinhole/src/distortion_function/polynomial.rs b/cv-pinhole/src/distortion_function/polynomial.rs index 3622a99b..6cae3689 100644 --- a/cv-pinhole/src/distortion_function/polynomial.rs +++ b/cv-pinhole/src/distortion_function/polynomial.rs @@ -147,6 +147,7 @@ mod tests { evaluate_eps = 2.0, derivative_eps = 3.0, inverse_eps = 2.0, + gradient_eps = 1.0, ); #[test] @@ -154,7 +155,6 @@ mod tests { let f = functions(0); let x = 0.06987809296337355; let value = f.evaluate(x); - let expected = f.gradient(x); assert_float_eq!(value, 2.86874326561548, ulps <= 0); } diff --git a/cv-pinhole/src/distortion_function/rational.rs b/cv-pinhole/src/distortion_function/rational.rs index 118da735..03ed0d13 100644 --- a/cv-pinhole/src/distortion_function/rational.rs +++ b/cv-pinhole/src/distortion_function/rational.rs @@ -218,8 +218,9 @@ mod tests { distortion_test_generate!( function(), evaluate_eps = 3.0, - derivative_eps = 120.0, + derivative_eps = 135.0, inverse_eps = 2.0, + gradient_eps = 4.0, ); #[test]