import sympy as sp

Pinhole Camera Model

We'll start with a relatively simple example. Borrowing from the camera calibration example, we'll use the Pinhole Camera Model. This model maps points in 3D to pixels. $f$ is the focal length, $c_x, c_y$ are the camera's principal point, $X, Y, Z$ is a point in 3D, and $u, v$ are the resulting pixel locations. Check out the camera calibration blog post for a refresher on these parameters.

$$ \begin{bmatrix}u_{pix} \\v_{pix}\end{bmatrix} =\begin{bmatrix}f \frac{X}{Z} + c_x \\ f \frac{Y}{Z} + c_y\end{bmatrix} $$

In keeping with the optimization theme, we'll convert this into a residual function. The residual here is there difference between an observed pixel location and the expected pixel location we get by passing the corresponding 3D point through the model. To perform camera calibration as described in the Camera Calibration from Scratch blog post, we iteratively change the camera parameters ($f, c_x, c_y$) until the sum of squared residuals is minimized.

$$ r(u, v, f, c_x, c_y, X, Y, Z) = \begin{bmatrix}u_{obs} \\v_{obs}\end{bmatrix} - \begin{bmatrix}f \frac{X}{Z} + c_x \\ f \frac{Y}{Z} + c_y\end{bmatrix} $$

Performing this minimization requires providing residuals and Jacobians of those residuals to the optimization algorithm. We've just provided the expression for the residual, so now let's focus on the Jacobian.

First we'll start by enumerating the symbols in the function in question. We pass to the symbols() function a list of symbol names we'll be using and it'll return handles to those names. The symbol names and python variable names don't need to be the same even though they are here.

# u, v are optional since we don't optimize them and they're linearly separate terms
u, v, f, cx, cy, X, Y, Z = sp.symbols('u, v, f, c_x, c_y, X, Y, Z')
display(f, cx, cy)

$\displaystyle f$

$\displaystyle c_{x}$

$\displaystyle c_{y}$

Next we'll build the residual vector expression. We'll do by creating a Matrix.

resid = sp.Matrix([u - (f*X/Z+cx), v - (f*Y/Z+cy)])
display(resid)

$\displaystyle \left[\begin{matrix}- \frac{X f}{Z} - c_{x} + u\\- \frac{Y f}{Z} - c_{y} + v\end{matrix}\right]$

Now we build the parameter vector, i.e. the list of parameters we wish optimize. Since this is a camera calibration example, this is just $ f, c_x, c_y$, the focal length and principal point of our camera. The other parameters are observations or constants we don't optimize.

parameter_vector = sp.Matrix([f, cx, cy])

Now we just call the jacobian() method on the residual vector, passing the parameter vector. This will generate the Jacobian of the residual expression with respect to the parameters we passed. Note the dimensions. The residual has two rows and thus so does the Jacobian. There are three input parameters and thus the Jacobian has three columns.

resid.jacobian(parameter_vector)

$\displaystyle \left[\begin{matrix}- \frac{X}{Z} & -1 & 0\\- \frac{Y}{Z} & 0 & -1\end{matrix}\right]$

If we instead wanted to optimize the 3D point($ \begin{bmatrix}X & Y & Z\end{bmatrix} $) like one might see in iterative PnP or Bundle Adjustment problems, we would change the parameter vector to contain those points.

parameter_vector_point = sp.Matrix([X, Y, Z])
resid.jacobian(parameter_vector_point)

$\displaystyle \left[\begin{matrix}- \frac{f}{Z} & 0 & \frac{X f}{Z^{2}}\\0 & - \frac{f}{Z} & \frac{Y f}{Z^{2}}\end{matrix}\right]$

Brown Conrady Distortion Model

Now let's try something more complicated. The Pinhole model can be extended to better model cameras with lens distortion). The Brown-Conrady model is one of the most common of such models; OpenCV implements it in its calib3d library. The model has additional coefficients ($k1, k2, k3, p1, p2$), with $k$ coefficients modeling radial distortion and $p$ coefficients modeling tangential distortion. We'll use the same notation conventions as with the Pinhole model and show how a 3D point gets mapped to a pixel in the presence of distortion:

$$ \begin{bmatrix}x' \\y'\end{bmatrix} = \begin{bmatrix}X/Z \\Y/Z\end{bmatrix} $$$$ r^2 = x'^2 + y'^2 $$$$ \begin{bmatrix} x'' \\ y'' \end{bmatrix} = \begin{bmatrix} x' ( 1+ k_1r^2 + k_2 r^4 + k_3 r^6) + 2p_1 x' y' + p_2 ( r^2 + 2 x' ^2) \\ y' ( 1+ k_1r^2 + k_2 r^4 + k_3 r^6) + p_1 ( r^2 + 2 y' ^2) + 2 p_2 x' y' \end{bmatrix} $$$$ \begin{bmatrix}u_{calc} \\v_{calc}\end{bmatrix} = \begin{bmatrix}f x'' + c_x \\ f y'' + c_y\end{bmatrix} $$

And as a residual: $$ r(u, v, f, c_x, c_y, X, Y, Z, k_1, k_2, k_3, p_1, p_2) = \begin{bmatrix}u_{obs} \\v_{obs}\end{bmatrix} - \begin{bmatrix}f x'' + c_x \\ f y'' + c_y\end{bmatrix} $$

We'll start by modeling the distortion in SymPy:

from IPython.display import Math
k1, k2, k3, p1, p2 = sp.symbols('k1, k2, k3, p1, p2')

# [X', Y']
xp = X/Z;
yp = Y/Z;

# r = x'^2 + y'^2
xpyp = sp.Matrix([xp, yp])
r2 = xpyp.dot(xpyp)

# [x'' y'']
xpp = xp * (1+ k1*r2 + k2*r2*r2 + k3*r2*r2*r2) + 2*p1*xp*yp + p2*(r2 + 2*xp*xp)
ypp = yp * (1+ k1*r2 + k2*r2*r2 + k3*r2*r2*r2) + p1*(r2 + 2*yp*yp) + 2*p2*xp*yp
display(Math(r'x^{\prime\prime}: '))
display(xpp)
display(Math(r'y^{\prime\prime}: '))
display(ypp)

$\displaystyle x^{\prime\prime}: $

$\displaystyle \frac{2 X Y p_{1}}{Z^{2}} + \frac{X \left(k_{1} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right) + k_{2} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{2} + k_{3} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{3} + 1\right)}{Z} + p_{2} \left(\frac{3 X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)$

$\displaystyle y^{\prime\prime}: $

$\displaystyle \frac{2 X Y p_{2}}{Z^{2}} + \frac{Y \left(k_{1} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right) + k_{2} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{2} + k_{3} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{3} + 1\right)}{Z} + p_{1} \left(\frac{X^{2}}{Z^{2}} + \frac{3 Y^{2}}{Z^{2}}\right)$

Now we'll construct the matrix residual expression:

resid_bc = sp.Matrix([u - (f*xpp+cx), v - (f*ypp+cy)])
display(resid_bc)

$\displaystyle \left[\begin{matrix}- c_{x} - f \left(\frac{2 X Y p_{1}}{Z^{2}} + \frac{X \left(k_{1} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right) + k_{2} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{2} + k_{3} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{3} + 1\right)}{Z} + p_{2} \left(\frac{3 X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)\right) + u\\- c_{y} - f \left(\frac{2 X Y p_{2}}{Z^{2}} + \frac{Y \left(k_{1} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right) + k_{2} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{2} + k_{3} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{3} + 1\right)}{Z} + p_{1} \left(\frac{X^{2}}{Z^{2}} + \frac{3 Y^{2}}{Z^{2}}\right)\right) + v\end{matrix}\right]$

And calculate the Jacobian:

parameter_vector_bc = sp.Matrix([f, cx, cy, k1, k2, k3, p1, p2]);
bc_jacobian = resid_bc.jacobian(parameter_vector_bc)
display(bc_jacobian)

$\displaystyle \left[\begin{matrix}- \frac{2 X Y p_{1}}{Z^{2}} - \frac{X \left(k_{1} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right) + k_{2} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{2} + k_{3} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{3} + 1\right)}{Z} - p_{2} \left(\frac{3 X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right) & -1 & 0 & - \frac{X f \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)}{Z} & - \frac{X f \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{2}}{Z} & - \frac{X f \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{3}}{Z} & - \frac{2 X Y f}{Z^{2}} & - f \left(\frac{3 X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)\\- \frac{2 X Y p_{2}}{Z^{2}} - \frac{Y \left(k_{1} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right) + k_{2} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{2} + k_{3} \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{3} + 1\right)}{Z} - p_{1} \left(\frac{X^{2}}{Z^{2}} + \frac{3 Y^{2}}{Z^{2}}\right) & 0 & -1 & - \frac{Y f \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)}{Z} & - \frac{Y f \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{2}}{Z} & - \frac{Y f \left(\frac{X^{2}}{Z^{2}} + \frac{Y^{2}}{Z^{2}}\right)^{3}}{Z} & - f \left(\frac{X^{2}}{Z^{2}} + \frac{3 Y^{2}}{Z^{2}}\right) & - \frac{2 X Y f}{Z^{2}}\end{matrix}\right]$

Great! we've got our Jacobian. It just looks a little hairy. SymPy can further simplify expressions and also perform common subexpression elimination which is useful to reduce the amount of transcription necessary to use this elsewhere. In this case we can also just simplify some of this by inspection. In the above definition of the model we defined a few intermediate variables ($x', y', x'', y'', r^2$) for convenience. We can substitute those back into the equation. This isn't just for having a cleaner visual output; it's likely that you'll have to compute these subexpressions to evaluate the residual function. We'll use the subs() function to perform these substituions. Unfortunately this function won't recognized the subexpressions (like xpp) generated heretofor. Instead we'll have to generate some dummy symbols and replace the corresponding expressions with those dummies.

r2 = sp.Dummy('r^2')
r4 = sp.Dummy('r^4')
r6 = sp.Dummy('r^6')

simp_r2 = bc_jacobian.subs({(X*X)/(Z*Z)+(Y*Y)/(Z*Z): r2, r2*r2:r4, r2*r2*r2:r6})

# x_p, y_p are x', y'
xp = sp.Dummy('x_p')
yp = sp.Dummy('y_p')
simp_xpyp = simp_r2.subs({X/Z:xp, Y/Z: yp})

xpp = sp.Dummy('x_pp')
ypp = sp.Dummy('y_pp')
final = simp_xpyp.subs({simp_xpyp[(0,0)]:xpp, simp_xpyp[(1,0)]:ypp})
display(final)

$\displaystyle \left[\begin{matrix}x_{pp} & -1 & 0 & - r^{2} x_{p} f & - r^{4} x_{p} f & - r^{6} x_{p} f & - 2 x_{p} y_{p} f & - f \left(3 x_{p}^{2} + y_{p}^{2}\right)\\y_{pp} & 0 & -1 & - r^{2} y_{p} f & - r^{4} y_{p} f & - r^{6} y_{p} f & - f \left(x_{p}^{2} + 3 y_{p}^{2}\right) & - 2 x_{p} y_{p} f\end{matrix}\right]$

Conclusion

And there you have a it: a nice, clean, simplified, expression for the Jacobian of our residual function with respect to the camera parameters ($f, c_x, c_y, k_1, k_2, k_3, p_1, p_2$). Using the techniques in this notebook, you should be able to come up with Jacobians for your own function using SymPy.