# Camera Calibration

## Coordinate System

This camera calibration follows the coordinate system below.

x: World coordinate, longitudinal. Forward direction is positive.

y: World coordinate, lateral. Leftward direction is positive.

z: World coordinate, vertical. Upward direction is positive.

u: Image coordinate, lateral. Rightward direction is positive.

v: Image coordinate, vertical. Downward direction is positive.



## Distortion Model

This camera calibration takes only barrel distortion parameters $\left( \kappa_1, \kappa_2, \kappa_3, \kappa_4, \kappa_5, \kappa_6 \right)$ into account.

Let $\left( u, v \right)$ be a point in undistorted image. Then the undistorted point is normalized as follows.

$$ \bar{u} = \frac{u - cu}{f_u} $$

$$ \bar{v} = \frac{v - cv}{f_v} $$

$$ r^2 = \bar{u}^2 + \bar{v}^2 $$

The ratio of distortion with respect to radial radius from the image center $\left( cu, cv \right)$ is computed.

$$ ratio = \frac{1.0 + \kappa_1 r^2 + \kappa_2 r^4 \kappa_3 r^6} {1.0 + \kappa_4 r^2 + \kappa_5 r^4 \kappa_6 r^6} $$

The corresponding point in distorted image $\left( u_d, v_d \right)$ is computed as follows.

$$ u_d = f_u \cdot ratio \cdot \bar{u} + cu $$

$$ u_v = f_v \cdot ratio \cdot \bar{v} + cv $$

## Homography Matrix

The homography matrix is defined as follows.

$$ H = K \cdot P \cdot \begin{bmatrix} R & R\cdot t \end{bmatrix}$$

where 

$$ K = \begin{bmatrix} f_u & 0 & cu \\ 0 & f_v & cv \\ 0 & 0 & 1 \end{bmatrix} $$

$$ P = \begin{bmatrix} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \end{bmatrix} $$

$$ R = R_z R_y R_x $$

$$ t = \begin{bmatrix} t_x \\ t_y \\ t_z \end{bmatrix} $$

## Initial value

The homography matrix is 3x4 matrix in which intrinsic camera parameters and extrinsic camera parameters cannot be linearly separated. To solve these parameters, initial values of the parameters are computed with the following assumption. Then these values will be refined by non-linear optimization.

The assumptions are as follows.
- Camera image has no distortion i.e. $\left( \kappa_n = 0 \right)$.
- $\left(cu, cv \right)$ is at the axact center of the image.
- Angle of camera is small enough to apply small angle approximation.


```python
"""
Direct Linear Transform for computing initial values.
"""
import sympy as sp
from IPython.display import display

roll, pitch, yaw = sp.symbols('roll pitch yaw')
tx, ty, tz = sp.symbols('tx ty tz')
fu, fv, cu, cv = sp.symbols('fu fv cu cv')

cosr = 1.0  #sp.cos(roll)
sinr = roll #sp.sin(roll)
Rx = sp.Matrix([[1, 0,     0,  ],
                [0, cosr, -sinr],
                [0, sinr,  cosr]])

# pitch
cosp = 1.0   #sp.cos(pitch)
sinp = pitch #sp.sin(pitch)
Ry = sp.Matrix([[cosp,  0, sinp],
                [0,     1,    0],
                [-sinp, 0, cosp]])

# yaw
cosy = 1.0  #sp.cos(yaw)
siny = yaw  #sp.sin(yaw)
Rz = sp.Matrix([[cosy, -siny, 0],
                [siny,  cosy, 0],
                [0,        0, 1]])

# translation
T = sp.Matrix([[1, 0, 0, tx],
               [0, 1, 0, ty],
               [0, 0, 1, tz]])

# intrinsic camera parameter matrix
K = sp.Matrix([[fu, 0,  cu],
               [0,  fv, cv],
               [0,  0,  1,]])

# permutation matrix
P = sp.Matrix([[0, -1,  0],
               [0,  0, -1],
               [1,  0,  0]])


# R = Rz*Ry*Rx
# print('rotation matrix with small angle approximation')
# display(R)


R = sp.Matrix([[1.0, -yaw, pitch],
               [yaw, 1.0, -roll],
               [-pitch, roll, 1.0]])
# print('rotation matrix with small angle approximation 2')
# display(R)

RT = R*T

H  = K * P * RT
display(H)

#sp.latex(H)

H3 = H[:,3]
h3_dx = H3.diff(tx)
display(h3_dx)
h3_dy = H3.diff(ty)
display(h3_dy)
h3_dz = H3.diff(tz)
display(h3_dz)

```


$\displaystyle \left[\begin{matrix}1.0 cu - fu yaw & - cu yaw - 1.0 fu & cu pitch + fu roll & cu \left(pitch tz + 1.0 tx - ty yaw\right) - fu \left(- roll tz + tx yaw + 1.0 ty\right)\\1.0 cv + fv pitch & - cv yaw - fv roll & cv pitch - 1.0 fv & cv \left(pitch tz + 1.0 tx - ty yaw\right) - fv \left(- pitch tx + roll ty + 1.0 tz\right)\\1.0 & - yaw & pitch & pitch tz + 1.0 tx - ty yaw\end{matrix}\right]$



$\displaystyle \left[\begin{matrix}1.0 cu - fu yaw\\1.0 cv + fv pitch\\1.0\end{matrix}\right]$



$\displaystyle \left[\begin{matrix}- cu yaw - 1.0 fu\\- cv yaw - fv roll\\- yaw\end{matrix}\right]$



$\displaystyle \left[\begin{matrix}cu pitch + fu roll\\cv pitch - 1.0 fv\\pitch\end{matrix}\right]$


From the approximated homography equation, initial guess of parameters can be obtained as follows.

$$ H = \begin{bmatrix} h_{00} & h_{01} & h_{02} & h_{03} \\ h_{10} & h_{11} & h_{12} & h_{13} \\ 1 & h_{21} & h_{22} & h_{23} \end{bmatrix} $$

$$ cu = 0.5 \cdot width $$

$$ cv = 0.5 \cdot height $$

$$ yaw = -h_{21} $$

$$ pitch = h_{22} $$

$$ fu = -\frac{f_{00} - cu}{yaw} $$

$$ fv = -\frac{f_{10} - cv}{pitch} $$

$$ roll = \frac{h_{02} - cu \cdot pitch}{fu} $$

Note that pitch and yaw angle must not be zeros.

Translation vector can be obtained by the simultaneous equations

$$ \begin{bmatrix} cu - f_u \cdot yaw & -cu \cdot yaw - fu & cu \cdot pitch + f_u \cdot roll \\ cv + f_v \cdot pitch & -cv \cdot yaw - f_v \cdot roll & cv \cdot pitch - f_v \\ 1.0 & -yaw & pitch \end{bmatrix} \begin{bmatrix}t_x \\ t_y \\ t_z\end{bmatrix} = \begin{bmatrix}h_{03} \\ h_{13} \\ h_{23} \end{bmatrix} $$

## Direct Linear Transform


```python
import sympy as sp
from IPython.display import display

roll, pitch, yaw = sp.symbols('roll pitch yaw')
tx, ty, tz = sp.symbols('tx ty tz')
fu, fv, cu, cv = sp.symbols('fu fv cu cv')

cosr = sp.cos(roll)
sinr = sp.sin(roll)
Rx = sp.Matrix([[1, 0,     0,  ],
                [0, cosr, -sinr],
                [0, sinr,  cosr]])

# pitch
cosp = sp.cos(pitch)
sinp = sp.sin(pitch)
Ry = sp.Matrix([[cosp,  0, sinp],
                [0,     1,    0],
                [-sinp, 0, cosp]])

# yaw
cosy = sp.cos(yaw)
siny = sp.sin(yaw)
Rz = sp.Matrix([[cosy, -siny, 0],
                [siny,  cosy, 0],
                [0,        0, 1]])

# translation
T = sp.Matrix([[1, 0, 0, tx],
               [0, 1, 0, ty],
               [0, 0, 1, tz]])

# intrinsic camera parameter matrix
K = sp.Matrix([[fu, 0,  cu],
               [0,  fv, cv],
               [0,  0,  1,]])

# permutation matrix
P = sp.Matrix([[0, -1,  0],
               [0,  0, -1],
               [1,  0,  0]])


R = Rz*Ry*Rx
display(R)

RT = R*T
display(RT)

H  = K * P * RT
display(H)
```


$\displaystyle \left[\begin{matrix}\cos{\left(pitch \right)} \cos{\left(yaw \right)} & \sin{\left(pitch \right)} \sin{\left(roll \right)} \cos{\left(yaw \right)} - \sin{\left(yaw \right)} \cos{\left(roll \right)} & \sin{\left(pitch \right)} \cos{\left(roll \right)} \cos{\left(yaw \right)} + \sin{\left(roll \right)} \sin{\left(yaw \right)}\\\sin{\left(yaw \right)} \cos{\left(pitch \right)} & \sin{\left(pitch \right)} \sin{\left(roll \right)} \sin{\left(yaw \right)} + \cos{\left(roll \right)} \cos{\left(yaw \right)} & \sin{\left(pitch \right)} \sin{\left(yaw \right)} \cos{\left(roll \right)} - \sin{\left(roll \right)} \cos{\left(yaw \right)}\\- \sin{\left(pitch \right)} & \sin{\left(roll \right)} \cos{\left(pitch \right)} & \cos{\left(pitch \right)} \cos{\left(roll \right)}\end{matrix}\right]$



$\displaystyle \left[\begin{matrix}\cos{\left(pitch \right)} \cos{\left(yaw \right)} & \sin{\left(pitch \right)} \sin{\left(roll \right)} \cos{\left(yaw \right)} - \sin{\left(yaw \right)} \cos{\left(roll \right)} & \sin{\left(pitch \right)} \cos{\left(roll \right)} \cos{\left(yaw \right)} + \sin{\left(roll \right)} \sin{\left(yaw \right)} & tx \cos{\left(pitch \right)} \cos{\left(yaw \right)} + ty \left(\sin{\left(pitch \right)} \sin{\left(roll \right)} \cos{\left(yaw \right)} - \sin{\left(yaw \right)} \cos{\left(roll \right)}\right) + tz \left(\sin{\left(pitch \right)} \cos{\left(roll \right)} \cos{\left(yaw \right)} + \sin{\left(roll \right)} \sin{\left(yaw \right)}\right)\\\sin{\left(yaw \right)} \cos{\left(pitch \right)} & \sin{\left(pitch \right)} \sin{\left(roll \right)} \sin{\left(yaw \right)} + \cos{\left(roll \right)} \cos{\left(yaw \right)} & \sin{\left(pitch \right)} \sin{\left(yaw \right)} \cos{\left(roll \right)} - \sin{\left(roll \right)} \cos{\left(yaw \right)} & tx \sin{\left(yaw \right)} \cos{\left(pitch \right)} + ty \left(\sin{\left(pitch \right)} \sin{\left(roll \right)} \sin{\left(yaw \right)} + \cos{\left(roll \right)} \cos{\left(yaw \right)}\right) + tz \left(\sin{\left(pitch \right)} \sin{\left(yaw \right)} \cos{\left(roll \right)} - \sin{\left(roll \right)} \cos{\left(yaw \right)}\right)\\- \sin{\left(pitch \right)} & \sin{\left(roll \right)} \cos{\left(pitch \right)} & \cos{\left(pitch \right)} \cos{\left(roll \right)} & - tx \sin{\left(pitch \right)} + ty \sin{\left(roll \right)} \cos{\left(pitch \right)} + tz \cos{\left(pitch \right)} \cos{\left(roll \right)}\end{matrix}\right]$



$\displaystyle \left[\begin{matrix}cu \cos{\left(pitch \right)} \cos{\left(yaw \right)} - fu \sin{\left(yaw \right)} \cos{\left(pitch \right)} & cu \left(\sin{\left(pitch \right)} \sin{\left(roll \right)} \cos{\left(yaw \right)} - \sin{\left(yaw \right)} \cos{\left(roll \right)}\right) - fu \left(\sin{\left(pitch \right)} \sin{\left(roll \right)} \sin{\left(yaw \right)} + \cos{\left(roll \right)} \cos{\left(yaw \right)}\right) & cu \left(\sin{\left(pitch \right)} \cos{\left(roll \right)} \cos{\left(yaw \right)} + \sin{\left(roll \right)} \sin{\left(yaw \right)}\right) - fu \left(\sin{\left(pitch \right)} \sin{\left(yaw \right)} \cos{\left(roll \right)} - \sin{\left(roll \right)} \cos{\left(yaw \right)}\right) & cu \left(tx \cos{\left(pitch \right)} \cos{\left(yaw \right)} + ty \left(\sin{\left(pitch \right)} \sin{\left(roll \right)} \cos{\left(yaw \right)} - \sin{\left(yaw \right)} \cos{\left(roll \right)}\right) + tz \left(\sin{\left(pitch \right)} \cos{\left(roll \right)} \cos{\left(yaw \right)} + \sin{\left(roll \right)} \sin{\left(yaw \right)}\right)\right) - fu \left(tx \sin{\left(yaw \right)} \cos{\left(pitch \right)} + ty \left(\sin{\left(pitch \right)} \sin{\left(roll \right)} \sin{\left(yaw \right)} + \cos{\left(roll \right)} \cos{\left(yaw \right)}\right) + tz \left(\sin{\left(pitch \right)} \sin{\left(yaw \right)} \cos{\left(roll \right)} - \sin{\left(roll \right)} \cos{\left(yaw \right)}\right)\right)\\cv \cos{\left(pitch \right)} \cos{\left(yaw \right)} + fv \sin{\left(pitch \right)} & cv \left(\sin{\left(pitch \right)} \sin{\left(roll \right)} \cos{\left(yaw \right)} - \sin{\left(yaw \right)} \cos{\left(roll \right)}\right) - fv \sin{\left(roll \right)} \cos{\left(pitch \right)} & cv \left(\sin{\left(pitch \right)} \cos{\left(roll \right)} \cos{\left(yaw \right)} + \sin{\left(roll \right)} \sin{\left(yaw \right)}\right) - fv \cos{\left(pitch \right)} \cos{\left(roll \right)} & cv \left(tx \cos{\left(pitch \right)} \cos{\left(yaw \right)} + ty \left(\sin{\left(pitch \right)} \sin{\left(roll \right)} \cos{\left(yaw \right)} - \sin{\left(yaw \right)} \cos{\left(roll \right)}\right) + tz \left(\sin{\left(pitch \right)} \cos{\left(roll \right)} \cos{\left(yaw \right)} + \sin{\left(roll \right)} \sin{\left(yaw \right)}\right)\right) - fv \left(- tx \sin{\left(pitch \right)} + ty \sin{\left(roll \right)} \cos{\left(pitch \right)} + tz \cos{\left(pitch \right)} \cos{\left(roll \right)}\right)\\\cos{\left(pitch \right)} \cos{\left(yaw \right)} & \sin{\left(pitch \right)} \sin{\left(roll \right)} \cos{\left(yaw \right)} - \sin{\left(yaw \right)} \cos{\left(roll \right)} & \sin{\left(pitch \right)} \cos{\left(roll \right)} \cos{\left(yaw \right)} + \sin{\left(roll \right)} \sin{\left(yaw \right)} & tx \cos{\left(pitch \right)} \cos{\left(yaw \right)} + ty \left(\sin{\left(pitch \right)} \sin{\left(roll \right)} \cos{\left(yaw \right)} - \sin{\left(yaw \right)} \cos{\left(roll \right)}\right) + tz \left(\sin{\left(pitch \right)} \cos{\left(roll \right)} \cos{\left(yaw \right)} + \sin{\left(roll \right)} \sin{\left(yaw \right)}\right)\end{matrix}\right]$



```python

```

ca9fcc238dec09e7b4d1786e7f573874cbb844ca
ipynb
Jupyter Notebook
calibration.ipynb
trip2eee/camera_calibration
ed8b7b8bcf0c6b3bcf9249f1524f8e5d818c83e0
MIT
calibration.ipynb
trip2eee/camera_calibration
ed8b7b8bcf0c6b3bcf9249f1524f8e5d818c83e0
MIT
calibration.ipynb
trip2eee/camera_calibration
ed8b7b8bcf0c6b3bcf9249f1524f8e5d818c83e0
MIT
Qwen/Qwen-72B
1. YES
2. YES