Computing a homography between 3D scene and image

      #include <gandalf/vision/affine34_fit.h>

Pose estimation is the procedure to compute the position of a camera relative to a known scene. In projective terms it means estimating the $3\times 4$ homography matrix $P$ representing the projection from the 3D scene into the 2D image. Here we assume a point-cloud representation, so the scene is represented by $n$ 3D points ${\bf X}_i$ in homogeneous coordinates, $i=1,\ldots,n$. The relationship between the ${\bf X}_i$ and points ${\bf x}_i$ in an image of the same (part of the) scene is a simple linear projective transformation or homography:

$${\bf x}_i = \lambda_i P {\bf X}_i$$ (5.5)

$P$ is a $3\times 4$ homography matrix and $\lambda$ is a scale factor. This equation also assumes that the camera employed in projecting the points onto the image is linear, but if the camera is non-linear AND the camera parameters are known, the distortion can be removed first by applying the function gan_camera_remove_distortion_[qi]() to the image points ${\bf x}_i$ as described in Section 5.1. Given six or more point correspondences (in general 3D position) in two images, the homography matrix $P$ can be computed. This can be done by first eliminating $\lambda$ to obtain homogeneous linear equations for the elements of $P$. Given that ${\bf X}=(X\;Y\;Z\:W)^{\top}$ and ${\bf x}=(x\;y\;z_h)^{\top}$, we can obtain the equations

$$x {\bf P}_3.{\bf X}- z_h {\bf P}_1.{\bf X}= 0,\;\;\; y {\bf P}_3.{\bf X}- z_h {\bf P}_2.{\bf X}= 0$$ (5.6)

where $P$ is separated into rows as

$$P = \left(\!\!\begin{array}{c} {\bf P}_1^{\top}\ {\bf P}_2^{\top}\ {\bf P}_3^{\top}\end{array}\!\!\right)$$

From six points we get twelve such equations, which allows $P$ to be computed up to a scale factor^5.2 using the same symmetric eigensystem routines as are used to solve for the fundamental and essential matrices in Section 5.2.

To start the calculation, define an accumulated symmetric matrix eigensystem structure and initialise it using the following routine:

      Gan_SymMatEigenStruct SymEigen;

      /* initialise eigensystem matrix */
      gan_homog34_init ( &SymEigen );

Then for each point correspondence, build the equations 5.6 and increment the accumulated symmetric eigensystem matrix by calling the following function:

      int iEqCount=0, iCount;
      Gan_Vector4 v4X; /* declare scene point X */
      Gan_Vector3 v3x; /* declare image point x */

      for ( iCount = 0; iCount < 100; iCount++ )
      {
         /* ... build scene and image point coordinates into X and x ... */

         /* increment matrix using point correspondence */
         gan_homog34_increment_p ( &SymEigen, &v4X, &v3x, 1.0, &iEqCount );
      }

The fourth argument 1.0 is a weighting factor for the equations as described in Section 3.2.2.15. The last argument iEqCount is a running count of the total number of equations processed thus far, to be passed below to the function to solve for $P$.

Once the point correspondences have been processed in this way, you can solve the equations using

      Gan_Matrix34 m34P; /* homography matrix P */

      gan_homog34_solve ( &SymEigen, iEqCount, &m34P );

to compute the homography $P$. If you want to repeat the calculation of a homography with new data, you can start again by calling

      gan_homog34_reset ( &SymEigen );

At the end of the homography calculation(s) you can free the eigensystem structure using the function

      gan_homog34_free ( &SymEigen );

If line matches are available, and the endpoints of the 3D line are approximately known, the line information can also be incorporated into the calculation. Since the scene line will normally be created from previous matching of image lines, which are line segments, the endpoints ${\bf X}_1$ and ${\bf X}_2$ of the line in scene coordinates should indeed be known. Note that we don't depend on locating the actual endpoints of the line accurately, which is a notoriously difficult problem. You should think of the two points ${\bf X}_1$ and ${\bf X}_2$ instead as representative points on the line. We note that ${\bf X}_1$ and ${\bf X}_2$ should project onto the image line ${\bf l}$, and so we obtain the equations

$${\bf l}.(P{\bf X}_1) = 0,\;\;\;\;{\bf l}.(P{\bf X}_2) = 0$$

These are homogeneous linear equations in the elements of $P$ which can be directly fed into the accumulated matrix calculation for $P$, using the routine

      Gan_Vector4 v4X1, v4X2; /* declare scene line endpoints X1 & X2 */
      Gan_Vector3 v3l; /* image line homogeneous coordinates l */

      /* ... set X1, X2 and l for corresponding scene line and image line ... */

      /* add equations for two endpoints */
      gan_homog34_increment_le ( &SymEigen, &v4X1, &v3l, 1.0, &iEqCount );
      gan_homog34_increment_le ( &SymEigen, &v4X2, &v3l, 1.0, &iEqCount );

Error detection: gan_homog34_init() returns a pointer to the initialised structure, and returns NULL on error. All the other routines except the void routine gan_homog34_free() return a boolean value, which is GAN_FALSE on error. The Gandalf error handler is invoked when an error occurs.