ov_core::CamEqui class

Fisheye / equadistant model pinhole camera model class.

Contents

As fisheye or wide-angle lenses are widely used in practice, we here provide mathematical derivations of such distortion model as in OpenCV fisheye.

\begin{align*} \begin{bmatrix} u \\ v \end{bmatrix}:= \mathbf{z}_k &= \mathbf h_d(\mathbf{z}_{n,k}, ~\boldsymbol\zeta) = \begin{bmatrix} f_x * x + c_x \\ f_y * y + c_y \end{bmatrix}\\[1em] \empty {\rm where}~~ x &= \frac{x_n}{r} * \theta_d \\ y &= \frac{y_n}{r} * \theta_d \\ \theta_d &= \theta (1 + k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8) \\ \quad r^2 &= x_n^2 + y_n^2 \\ \theta &= atan(r) \end{align*}

where $ \mathbf{z}_{n,k} = [ x_n ~ y_n ]^\top$ are the normalized coordinates of the 3D feature and u and v are the distorted image coordinates on the image plane. Clearly, the following distortion intrinsic parameters are used in the above model:

\begin{align*} \boldsymbol\zeta = \begin{bmatrix} f_x & f_y & c_x & c_y & k_1 & k_2 & k_3 & k_4 \end{bmatrix}^\top \end{align*}

In analogy to the previous radial distortion (see ov_core::CamRadtan) case, the following Jacobian for these parameters is needed for intrinsic calibration:

\begin{align*} \frac{\partial \mathbf h_d (\cdot)}{\partial \boldsymbol\zeta} = \begin{bmatrix} x_n & 0 & 1 & 0 & f_x*(\frac{x_n}{r}\theta^3) & f_x*(\frac{x_n}{r}\theta^5) & f_x*(\frac{x_n}{r}\theta^7) & f_x*(\frac{x_n}{r}\theta^9) \\[5pt] 0 & y_n & 0 & 1 & f_y*(\frac{y_n}{r}\theta^3) & f_y*(\frac{y_n}{r}\theta^5) & f_y*(\frac{y_n}{r}\theta^7) & f_y*(\frac{y_n}{r}\theta^9) \end{bmatrix} \end{align*}

Similarly, with the chain rule of differentiation, we can compute the following Jacobian with respect to the normalized coordinates:

\begin{align*} \frac{\partial \mathbf h_d(\cdot)}{\partial \mathbf{z}_{n,k}} &= \frac{\partial uv}{\partial xy}\frac{\partial xy}{\partial x_ny_n}+ \frac{\partial uv}{\partial xy}\frac{\partial xy}{\partial r}\frac{\partial r}{\partial x_ny_n}+ \frac{\partial uv}{\partial xy}\frac{\partial xy}{\partial \theta_d}\frac{\partial \theta_d}{\partial \theta}\frac{\partial \theta}{\partial r}\frac{\partial r}{\partial x_ny_n} \\[1em] \empty {\rm where}~~~~ \frac{\partial uv}{\partial xy} &= \begin{bmatrix} f_x & 0 \\ 0 & f_y \end{bmatrix} \\ \empty \frac{\partial xy}{\partial x_ny_n} &= \begin{bmatrix} \theta_d/r & 0 \\ 0 & \theta_d/r \end{bmatrix} \\ \empty \frac{\partial xy}{\partial r} &= \begin{bmatrix} -\frac{x_n}{r^2}\theta_d \\ -\frac{y_n}{r^2}\theta_d \end{bmatrix} \\ \empty \frac{\partial r}{\partial x_ny_n} &= \begin{bmatrix} \frac{x_n}{r} & \frac{y_n}{r} \end{bmatrix} \\ \empty \frac{\partial xy}{\partial \theta_d} &= \begin{bmatrix} \frac{x_n}{r} \\ \frac{y_n}{r} \end{bmatrix} \\ \empty \frac{\partial \theta_d}{\partial \theta} &= \begin{bmatrix} 1 + 3k_1 \theta^2 + 5k_2 \theta^4 + 7k_3 \theta^6 + 9k_4 \theta^8\end{bmatrix} \\ \empty \frac{\partial \theta}{\partial r} &= \begin{bmatrix} \frac{1}{r^2+1} \end{bmatrix} \end{align*}

To equate this to one of Kalibr's models, this is what you would use for pinhole-equi.

Base classes

class CamBase
Base pinhole camera model class.

Constructors, destructors, conversion operators

CamEqui(int width, int height)
Default constructor.

Public functions

auto undistort_f(const Eigen::Vector2f& uv_dist) -> Eigen::Vector2f override
Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coords.
auto distort_f(const Eigen::Vector2f& uv_norm) -> Eigen::Vector2f override
Given a normalized uv coordinate this will distort it to the raw image plane.
void compute_distort_jacobian(const Eigen::Vector2d& uv_norm, Eigen::MatrixXd& H_dz_dzn, Eigen::MatrixXd& H_dz_dzeta) override
Computes the derivative of raw distorted to normalized coordinate.

Function documentation

ov_core::CamEqui::CamEqui(int width, int height)

Default constructor.

Parameters
width Width of the camera (raw pixels)
height Height of the camera (raw pixels)

Eigen::Vector2f ov_core::CamEqui::undistort_f(const Eigen::Vector2f& uv_dist) override

Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coords.

Parameters
uv_dist Raw uv coordinate we wish to undistort
Returns 2d vector of normalized coordinates

Eigen::Vector2f ov_core::CamEqui::distort_f(const Eigen::Vector2f& uv_norm) override

Given a normalized uv coordinate this will distort it to the raw image plane.

Parameters
uv_norm Normalized coordinates we wish to distort
Returns 2d vector of raw uv coordinate

void ov_core::CamEqui::compute_distort_jacobian(const Eigen::Vector2d& uv_norm, Eigen::MatrixXd& H_dz_dzn, Eigen::MatrixXd& H_dz_dzeta) override

Computes the derivative of raw distorted to normalized coordinate.

Parameters
uv_norm Normalized coordinates we wish to distort
H_dz_dzn Derivative of measurement z in respect to normalized
H_dz_dzeta Derivative of measurement z in respect to intrinic parameters