I have a camera mounted on a rotational joint. I need to calibrate the extrinsics of this camera. I can fix the camera at an estimated angle (facing the ceiling). Then I want to get the real angle.

For that I track key-points in the ceiling while moving my robot forward. Supposing that odometry is perfect, I will see a difference between real key-points shift and estimated shift from the odometry.

I thought about using Levenberg Marquardt to find the optimal solution which is the angle and of my camera in the robot frame but what would my equation look like?


I assume you're provided with the following quantities:

  • The intrinsic parameters of the pinhole camera model in the conventional matrix format $\Pi \in \mathbb{R}^{3\times4}$.
  • $n$ 3-D stationary points $P_i=(x_i,y_i,z_i)'$ you can track in the environment that are given in the fixed reference frame $O$ used by the odometry, whose projections in the camera image plane are $\pi_i(t_j)$. The camera is supposed to be rigidly attached to a moving robot (we keep the joint fixed), thus $\pi_i$ varies with the observations taken at discrete time instants $t_j$.
  • $n$ 2-D points $p_i(t_j)$ estimated within the image plane by the tracking algorithm at the discrete instants $t_j$.
  • The homogeneous transformation $H(x(t_j),\theta+\delta\theta) \in \mathbb{R}^{4\times4}$ describing the location of the camera frame mounted on the robot with respect to the root frame $O$, as provided by the odometry. $H$ depends on the location $x$ (comprising the translation and the rotation of the base) that varies over time so as the joint angle $\theta+\delta\theta$ which is kept fixed, instead, representing $\theta$ the initial estimate of the joint angle and $\delta\theta$ the unknown offset to be sought.

We aim then at minimizing the following cost function: $$ E=\frac{1}{2}\sum_{i=1}^{n} \sum_{t_j} \left\| p_i(t_j)-\tilde{\pi}_i(t_j) \right\|^2, $$ where $$ \pi_i(t_j)=\Pi\cdot H^{-1}(x(t_j),\theta+\delta\theta)\cdot P_i. $$

It turns out that $\pi_i \in \mathbb{R}^3$ and specifically $\pi_i=\lambda_i\cdot(u_i,v_i,1)'$, so that we can set $\tilde{\pi}_i=(u_i,v_i)'$ to be used in $E$.

The unknowns are $\delta\theta$ and $P_i$; the total number of key-points $n$ can be kept low (around 4$\div$5 non-coplanar points) as we benefit from the significantly higher number of observations at discrete instants $t_j$. The form of the cost function $E$ is suitable for the LM algorithm since the expression for the gradient $J$ is easy to be retrieved (we know how to write down $H$).

A possible knowledge of the locations of the key-points $P_i$ specified in $O$ can be conveniently put inside $E$ to speed up convergence.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.