from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from stereocomplex.synthetic.parallel_plate import pinhole_ray_from_pixel
def _as_flat_pixels(u: np.ndarray, v: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
uu, vv = np.broadcast_arrays(np.asarray(u, dtype=np.float64), np.asarray(v, dtype=np.float64))
return uu.reshape(-1), vv.reshape(-1)
[docs]
@dataclass(frozen=True)
class CentralPinholeModel:
"""Central pinhole rayfield used as the zero-parameter baseline."""
K: np.ndarray
name: str = "central_pinhole"
@property
def n_parameters(self) -> int:
"""Number of free parameters in this model (0 for pinhole, 5 for Brown-Conrady)."""
return 0
[docs]
def parameter_vector(self) -> np.ndarray:
"""Pack model parameters into a flat vector for optimisation."""
return np.zeros(0, dtype=np.float64)
[docs]
@classmethod
def from_parameter_vector(cls, x: np.ndarray, **kwargs) -> CentralPinholeModel:
"""Reconstruct model from a parameter vector. K must be passed via kwargs."""
arr = np.asarray(x, dtype=np.float64).reshape(-1)
if arr.size != 0:
raise ValueError("CentralPinholeModel expects zero parameters")
return cls(K=np.asarray(kwargs["K"], dtype=np.float64).reshape(3, 3))
[docs]
def parameter_dict(self) -> dict[str, float]:
"""Model parameters as a dict keyed by coefficient name."""
return {}
[docs]
def ray(self, u: np.ndarray, v: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
"""Compute ray (origin at camera centre, distorted direction) for pixels."""
uf, vf = _as_flat_pixels(u, v)
d = pinhole_ray_from_pixel(uf, vf, self.K).reshape(-1, 3)
origins = np.zeros_like(d)
return origins, d
[docs]
def project_point(self, X: np.ndarray) -> tuple[np.ndarray, bool]:
"""Analytic projection: 3-D point → pixel via perspective division."""
K = np.asarray(self.K, dtype=np.float64).reshape(3, 3)
x = np.asarray(X, dtype=np.float64).reshape(3)
uv_h = K @ x
if abs(uv_h[2]) < 1e-12:
return np.full(2, np.nan), False
u, v = uv_h[0] / uv_h[2], uv_h[1] / uv_h[2]
W = 2 * K[0, 2]
H = 2 * K[1, 2]
return np.array([u, v]), bool(0 <= u <= W - 1 and 0 <= v <= H - 1)
[docs]
def brown_conrady_distort_normalized(
x: np.ndarray,
y: np.ndarray,
k1: float,
k2: float,
p1: float,
p2: float,
k3: float,
) -> tuple[np.ndarray, np.ndarray]:
"""Apply Brown-Conrady distortion to normalised camera coordinates.
Uses the standard OpenCV convention:
xd = x * (1 + k1*r2 + k2*r4 + k3*r6) + 2*p1*x*y + p2*(r2 + 2*x^2)
yd = y * (1 + k1*r2 + k2*r4 + k3*r6) + p1*(r2 + 2*y^2) + 2*p2*x*y
where r2 = x^2 + y^2.
Parameters
----------
x, y : ndarray
Undistorted normalised image coordinates (unitless).
k1, k2, k3 : float
Radial distortion coefficients (k3 is optional, often 0).
p1, p2 : float
Tangential distortion coefficients.
Returns
-------
xd, yd : ndarray
Distorted normalised coordinates (unitless).
"""
r2 = x * x + y * y
radial = 1.0 + float(k1) * r2 + float(k2) * r2 * r2 + float(k3) * r2 * r2 * r2
xy2 = 2.0 * x * y
xd = x * radial + float(p1) * xy2 + float(p2) * (r2 + 2.0 * x * x)
yd = y * radial + float(p1) * (r2 + 2.0 * y * y) + float(p2) * xy2
return xd, yd
[docs]
def undistort_brown_normalized(
xd: np.ndarray,
yd: np.ndarray,
k1: float,
k2: float,
p1: float,
p2: float,
k3: float,
n_iter: int = 10,
) -> tuple[np.ndarray, np.ndarray]:
"""Invert Brown-Conrady distortion by fixed-point iteration.
The model-selection use case only needs a stable central candidate, not a
high-performance undistorter. The iteration is vectorized and initialized at
the distorted normalized coordinates.
"""
x = np.asarray(xd, dtype=np.float64).copy()
y = np.asarray(yd, dtype=np.float64).copy()
xd_arr = np.asarray(xd, dtype=np.float64)
yd_arr = np.asarray(yd, dtype=np.float64)
for _ in range(int(n_iter)):
r2 = x * x + y * y
radial = 1.0 + float(k1) * r2 + float(k2) * r2 * r2 + float(k3) * r2 * r2 * r2
radial = np.where(np.abs(radial) < 1e-12, 1e-12, radial)
xy2 = 2.0 * x * y
dx_tan = float(p1) * xy2 + float(p2) * (r2 + 2.0 * x * x)
dy_tan = float(p1) * (r2 + 2.0 * y * y) + float(p2) * xy2
x = (xd_arr - dx_tan) / radial
y = (yd_arr - dy_tan) / radial
return x, y
[docs]
@dataclass(frozen=True)
class CentralBrownConradyModel:
"""Central Brown-Conrady optical candidate.
It can bend pixel directions, but all rays still share one camera center.
Therefore it is intentionally unable to represent a pixel-dependent origin
field generated by an inclined parallel plate.
"""
K: np.ndarray
k1: float = 0.0
k2: float = 0.0
p1: float = 0.0
p2: float = 0.0
k3: float = 0.0
name: str = "central_brown_conrady"
@property
def n_parameters(self) -> int:
"""Number of free parameters in this model (0 for pinhole, 5 for Brown-Conrady)."""
return 5
[docs]
def parameter_vector(self) -> np.ndarray:
"""Pack model parameters into a flat vector for optimisation."""
return np.array([self.k1, self.k2, self.p1, self.p2, self.k3], dtype=np.float64)
[docs]
@classmethod
def from_parameter_vector(cls, x: np.ndarray, **kwargs) -> CentralBrownConradyModel:
"""Reconstruct model from a parameter vector. K must be passed via kwargs."""
arr = np.asarray(x, dtype=np.float64).reshape(-1)
if arr.size != 5:
raise ValueError("CentralBrownConradyModel expects five parameters")
return cls(
K=np.asarray(kwargs["K"], dtype=np.float64).reshape(3, 3),
k1=float(arr[0]),
k2=float(arr[1]),
p1=float(arr[2]),
p2=float(arr[3]),
k3=float(arr[4]),
)
[docs]
def parameter_dict(self) -> dict[str, float]:
"""Model parameters as a dict keyed by coefficient name."""
return {
"k1": float(self.k1),
"k2": float(self.k2),
"p1": float(self.p1),
"p2": float(self.p2),
"k3": float(self.k3),
}
[docs]
def ray(self, u: np.ndarray, v: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
"""Compute ray (origin at camera centre, distorted direction) for pixels."""
K = np.asarray(self.K, dtype=np.float64).reshape(3, 3)
uf, vf = _as_flat_pixels(u, v)
xd = (uf - K[0, 2]) / K[0, 0]
yd = (vf - K[1, 2]) / K[1, 1]
x, y = undistort_brown_normalized(xd, yd, self.k1, self.k2, self.p1, self.p2, self.k3)
d = np.column_stack([x, y, np.ones_like(x)])
d = d / np.linalg.norm(d, axis=1, keepdims=True)
origins = np.zeros_like(d)
return origins, d
[docs]
def project_point(self, X: np.ndarray) -> tuple[np.ndarray, bool]:
"""Analytic projection: 3-D point → pixel with Brown distortion."""
K = np.asarray(self.K, dtype=np.float64).reshape(3, 3)
x = np.asarray(X, dtype=np.float64).reshape(3)
if abs(x[2]) < 1e-12:
return np.full(2, np.nan), False
xn, yn = x[0] / x[2], x[1] / x[2]
xd, yd = brown_conrady_distort_normalized(
np.array([xn]), np.array([yn]),
self.k1, self.k2, self.p1, self.p2, self.k3,
)
u = float(xd[0] * K[0, 0] + K[0, 2])
v = float(yd[0] * K[1, 1] + K[1, 2])
W = 2 * K[0, 2]
H = 2 * K[1, 2]
return np.array([u, v]), bool(0 <= u <= W - 1 and 0 <= v <= H - 1)
__all__ = [
"CentralBrownConradyModel",
"CentralPinholeModel",
"brown_conrady_distort_normalized",
"undistort_brown_normalized",
]