) {
+ todo!()
+ }
+}
+
+type Dim = DimSum::NumParameters, U2>, 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);
+ result
+ }
+}
+
+impl CameraModel for Camera
+where
+ R: DistortionFunction,
+ T: DistortionFunction,
+ P: DistortionFunction,
+ DefaultAllocator: Allocator,
+ DefaultAllocator: Allocator,
+ DefaultAllocator: Allocator,
+{
+ type Projection = NormalizedKeyPoint;
+
+ fn calibrate(&self, point: Point) -> Self::Projection {
+ 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 point = self.distort(projection.0);
+ let point = self.project(point);
+ self.linear.uncalibrate(NormalizedKeyPoint(point))
+ }
+}
+
+pub mod models {
+ use super::Camera;
+ use crate::distortion_function::{self, Constant, DistortionFunction, Polynomial, Rational};
+ use crate::CameraIntrinsics;
+ use cv_core::nalgebra::{Matrix3, Vector3, VectorN, U1, U8};
+ use cv_core::nalgebra::{U3, U4};
+
+ /// 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 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>;
+
+ 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);
+
+ // 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 cv_core::nalgebra::{Matrix3, Point2};
+ use float_eq::assert_float_eq;
+ use models::OpenCV;
+
+ #[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() -> OpenCV {
+ // Calibration parameters for a GoPro Hero 6 using OpenCV
+ #[rustfmt::skip]
+ 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]);
+ #[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];
+ OpenCV::opencv(camera_matrix, &distortion)
+ }
+
+ #[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 distorted = camera.distort(Point2::new(x, y));
+ let (x, y) = (distorted[0], distorted[1]);
+
+ assert_float_eq!(x, ex, ulps <= 2);
+ assert_float_eq!(y, ey, ulps <= 2);
+ }
+ }
+
+ #[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 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
new file mode 100644
index 00000000..1354adc9
--- /dev/null
+++ b/cv-pinhole/src/distortion_function/fisheye.rs
@@ -0,0 +1,144 @@
+use super::DistortionFunction;
+use cv_core::nalgebra::{storage::Storage, Vector, Vector1, VectorN, U1};
+
+/// Parametric fisheye radial distortion function.
+///
+/// Implements various projection functions using the generalized relation with a single
+/// parameter $k$.
+///
+/// $$
+/// 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. 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].
+/// * Lensfun documentation. [link][lensfun].
+/// * PTGui v11 Support question 3.28. [link][ptgui].
+/// * Panotools wiki. [link][panotools].
+///
+/// [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
+/// [panotools]: https://wiki.panotools.org/Fisheye_Projection
+///
+///
+#[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)
+ }
+}
+
+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 {
+ 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 {
+ 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,
+ }
+ }
+
+ 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 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,
+ })
+ }
+}
+
+#[cfg(test)]
+mod tests {
+ use super::*;
+ use crate::distortion_function::test::TestFloat;
+ use crate::distortion_test_generate;
+ use proptest::prelude::*;
+ use rug::Float;
+
+ 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,
+ }
+ }
+ }
+
+ fn function() -> impl Strategy {
+ (-1_f64..1.).prop_map(Fisheye)
+ }
+
+ distortion_test_generate!(
+ function(),
+ 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
new file mode 100644
index 00000000..97a8b452
--- /dev/null
+++ b/cv-pinhole/src/distortion_function/mod.rs
@@ -0,0 +1,309 @@
+mod fisheye;
+mod polynomial;
+mod rational;
+
+use cv_core::nalgebra::{
+ allocator::Allocator, storage::Storage, DefaultAllocator, Dim, Vector, Vector1, Vector2,
+ VectorN, U1, U2,
+};
+
+// Re-exports
+pub use fisheye::Fisheye;
+pub use polynomial::Polynomial;
+pub use rational::Rational;
+
+/// 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,
+{
+ /// 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;
+
+ /// Evaluate $f(\mathtt{value}, \vec β)$.
+ fn evaluate(&self, value: f64) -> f64;
+
+ /// Evaluate the derivative $f'(\mathtt{value}, \vec β)$ where $f' = \frac{\d}{\d x} f$.
+ fn derivative(&self, value: f64) -> f64;
+
+ /// 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))
+ }
+
+ /// Evaluate the inverse $f^{-1}(\mathtt{value}, \vec β)$.
+ ///
+ /// # 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 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);
+ f -= value;
+ 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;
+}
+
+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))
+}
+
+#[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.
+ const INTERNAL_PRECISION: u32 = 1000;
+
+ pub(crate) trait TestFloat: DistortionFunction
+ where
+ Self::NumParameters: DimName,
+ 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 = 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 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
+ }
+
+ 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())
+ }
+
+ 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]
+ macro_rules! distortion_test_generate {
+ (
+ $function:expr,
+ evaluate_eps = $eval:expr,
+ derivative_eps = $deriv:expr,
+ inverse_eps = $inv:expr,
+ gradient_eps = $grad:expr,
+ ) => {
+ #[test]
+ fn test_evaluate() {
+ proptest::proptest!(|(f in $function, x in 0.0..2.0)| {
+ let value = f.evaluate(x);
+ let expected = f.evaluate_exact(x);
+ float_eq::assert_float_eq!(value, expected, rmax <= $eval * f64::EPSILON);
+ });
+ }
+
+ #[test]
+ fn test_derivative() {
+ proptest::proptest!(|(f in $function, x in 0.0..2.0)| {
+ let value = f.derivative(x);
+ let expected = f.derivative_exact(x);
+ float_eq::assert_float_eq!(value, expected, rmax <= $deriv * f64::EPSILON);
+ });
+ }
+
+ #[test]
+ fn test_with_derivative() {
+ proptest::proptest!(|(f in $function, x in 0.0..2.0)| {
+ let value = f.with_derivative(x);
+ let expected = f.with_derivative_exact(x);
+ 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::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);
+ float_eq::assert_float_eq!(y, y2, rmax <= $inv * f64::EPSILON);
+ });
+ }
+
+ #[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 <= $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
new file mode 100644
index 00000000..6cae3689
--- /dev/null
+++ b/cv-pinhole/src/distortion_function/polynomial.rs
@@ -0,0 +1,162 @@
+// 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, storage::Storage, DefaultAllocator, Dim, Vector, VectorN, U1,
+};
+use num_traits::Zero;
+
+/// Polynomial distortion function
+///
+/// $$
+/// f(x, \vec β) = β_0 + β_1 ⋅ x + β_2 ⋅ x^2 + ⋯ + β_n ⋅ x^n
+/// $$
+///
+/// # To do
+///
+/// * Make it work with `Degree = Dynamic`.
+///
+#[derive(Clone, PartialEq, Debug)]
+pub struct Polynomial(VectorN)
+where
+ DefaultAllocator: Allocator;
+
+impl Default for Polynomial
+where
+ DefaultAllocator: Allocator,
+ VectorN: Zero,
+{
+ fn default() -> Self {
+ Self(VectorN::zero())
+ }
+}
+
+impl DistortionFunction for Polynomial
+where
+ 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.
+ // multiplication is redundant.
+ let mut result = 0.0;
+ for i in (0..self.0.nrows()).rev() {
+ result *= value;
+ result += self.0[i];
+ }
+ result
+ }
+
+ fn derivative(&self, value: f64) -> f64 {
+ self.with_derivative(value).1
+ }
+
+ /// Simultaneously compute value and first derivative.
+ ///
+ /// # 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;
+ for i in (0..self.0.nrows()).rev() {
+ derivative *= value;
+ derivative += result;
+ result *= value;
+ result += self.0[i];
+ }
+ (result, derivative)
+ }
+
+ fn gradient(&self, value: f64) -> VectorN {
+ let mut factor = 1.0;
+ VectorN::from_fn_generic(self.0.data.shape().0, U1, move |_, _| {
+ let coefficient = factor;
+ factor *= value;
+ coefficient
+ })
+ }
+}
+
+#[cfg(test)]
+mod tests {
+ use super::*;
+ use crate::distortion_function::test::TestFloat;
+ use crate::distortion_test_generate;
+ use cv_core::nalgebra::{DimName, Vector4, U4};
+ use float_eq::assert_float_eq;
+ use proptest::prelude::*;
+ use rug::Float;
+
+ impl TestFloat for Polynomial
+ where
+ Degree: Dim + DimName,
+ DefaultAllocator: Allocator,
+ {
+ fn evaluate_float(&self, x: &Float) -> Float {
+ let mut value = Float::new(x.prec());
+ for i in (0..self.0.nrows()).rev() {
+ value *= x;
+ value += self.0[i];
+ }
+ value
+ }
+ }
+
+ #[rustfmt::skip]
+ 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 functions(index: usize) -> Polynomial {
+ Polynomial::from_parameters(Vector4::from_column_slice(POLYS[index]))
+ }
+
+ fn function() -> impl Strategy> {
+ (0_usize..POLYS.len()).prop_map(functions)
+ }
+
+ distortion_test_generate!(
+ function(),
+ evaluate_eps = 2.0,
+ derivative_eps = 3.0,
+ inverse_eps = 2.0,
+ gradient_eps = 1.0,
+ );
+
+ #[test]
+ fn test_evaluate_literal() {
+ let f = functions(0);
+ let x = 0.06987809296337355;
+ let value = f.evaluate(x);
+ assert_float_eq!(value, 2.86874326561548, ulps <= 0);
+ }
+
+ // 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
new file mode 100644
index 00000000..03ed0d13
--- /dev/null
+++ b/cv-pinhole/src/distortion_function/rational.rs
@@ -0,0 +1,233 @@
+use super::polynomial::Polynomial;
+use super::DistortionFunction;
+use cv_core::nalgebra::{
+ allocator::Allocator, storage::Storage, DefaultAllocator, Dim, DimAdd, DimName, DimSum,
+ SliceStorage, Vector, VectorN, U1,
+};
+use num_traits::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$.
+///
+/// See [`impl DistortionFunction`](#impl-DistortionFunction).
+///
+///
+#[derive(Clone, PartialEq, Debug)]
+pub struct Rational(Polynomial, Polynomial)
+where
+ DP: DimAdd,
+ DefaultAllocator: Allocator,
+ 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,
+ DQ: DimName,
+ DP: DimAdd,
+ DimSum: DimName,
+ DefaultAllocator: Allocator,
+ DefaultAllocator: Allocator,
+ DefaultAllocator: Allocator>,
+{
+ 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
+ }
+
+ /// Numerically invert the function.
+ ///
+ /// # Method
+ ///
+ /// Starting with the function definition:
+ ///
+ /// $$
+ /// y = \frac{P(x)}{Q(x)}
+ /// $$
+ ///
+ /// $$
+ /// \frac{P'(x)}{Q(x)} - \frac{P(x)}{Q(x)^2} ⋅ Q'(x)
+ /// $$
+ ///
+ /// $$
+ /// \frac{P'(x) ⋅ Q(x) - P(x) ⋅ Q'(x)}{Q(x)^2}
+ /// $$
+ ///
+ 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 {
+ 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(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);
+ 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,
+) -> 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
+}
+
+/// 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>,
+) -> (
+ Vector>,
+ Vector>,
+)
+where
+ D1: DimName,
+ D2: DimName,
+ S: Storage>,
+ D1: DimAdd,
+ DimSum: DimName,
+{
+ (
+ vector.fixed_slice::(0, 0),
+ vector.fixed_slice::(D1::dim(), 0),
+ )
+}
+
+#[cfg(test)]
+mod tests {
+ use super::*;
+ 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::*;
+ use rug::Float;
+
+ impl TestFloat for Rational
+ where
+ DP: DimName,
+ DQ: DimName,
+ DP: DimAdd,
+ DimSum: DimName,
+ DefaultAllocator: Allocator,
+ DefaultAllocator: Allocator,
+ DefaultAllocator: Allocator>,
+ Polynomial: TestFloat,
+ Polynomial: TestFloat,
+ {
+ fn evaluate_float(&self, x: &Float) -> Float {
+ self.0.evaluate_float(x) / self.1.evaluate_float(x)
+ }
+ }
+
+ /// OpenCV12 radial distortion for a GoPro Hero 6
+ #[rustfmt::skip]
+ fn functions(_index: usize) -> Rational {
+ Rational::from_parameters(
+ VectorN::from_column_slice_generic(U8, U1, &[
+ 1.0, 25.67400161236561, 15.249676433312018, 0.6729530830603175,
+ 1.0, 25.95447969279203, 22.421345314744390, 3.0431830552169914,
+ ])
+ )
+ }
+
+ fn function() -> impl Strategy> {
+ (0_usize..1).prop_map(functions)
+ }
+
+ distortion_test_generate!(
+ function(),
+ evaluate_eps = 3.0,
+ derivative_eps = 135.0,
+ inverse_eps = 2.0,
+ gradient_eps = 4.0,
+ );
+
+ #[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);
+ }
+}
diff --git a/cv-pinhole/src/lib.rs b/cv-pinhole/src/lib.rs
index 592416b6..3ed4b3ca 100644
--- a/cv-pinhole/src/lib.rs
+++ b/cv-pinhole/src/lib.rs
@@ -3,14 +3,19 @@
//! 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;
+pub mod distortion_function;
mod essential;
+mod root;
+pub use camera::{models, Camera};
pub use essential::*;
+pub(crate) use root::{newton2, root};
use cv_core::nalgebra::{Matrix3, Point2, Point3, Vector2, Vector3};
use cv_core::{
@@ -106,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 }
}
@@ -138,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
new file mode 100644
index 00000000..def07ba0
--- /dev/null
+++ b/cv-pinhole/src/root.rs
@@ -0,0 +1,109 @@
+use cv_core::nalgebra::{Matrix2, Vector2};
+
+/// 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;
+ }
+ }
+}
+
+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)
+}