calibrationTools.h

Go to the documentation of this file.

#ifndef DLR_COMPUTERVISION_CALIBRATIONTOOLS_H
#define DLR_COMPUTERVISION_CALIBRATIONTOOLS_H

#include <dlrComputerVision/cameraIntrinsicsPinhole.h>

namespace dlr {

  namespace computerVision {


    template <class Iter3D, class Iter2D>
    CameraIntrinsicsPinhole
    estimateCameraIntrinsicsPinhole(size_t numPixelsX, size_t numPixelsY,
                                    Iter3D points3DBegin, Iter3D points3DEnd,
                                    Iter2D points2DBegin);
    
  } // namespace computerVision
  
} // namespace dlr


/* ============ Definitions of inline & template functions ============ */


#include <dlrNumeric/utilities.h>
#include <dlrLinearAlgebra/linearAlgebra.h>

namespace dlr {

  namespace computerVision {
    
    // This function estimates pinhole camera intrinsic parameters
    // based on corresponding points in 2D image coordinates and 3D
    // camera coordinates.
    template <class Iter3D, class Iter2D>
    CameraIntrinsicsPinhole
    estimateCameraIntrinsicsPinhole(size_t numPixelsX, size_t numPixelsY,
                                    Iter3D points3DBegin, Iter3D points3DEnd,
                                    Iter2D points2DBegin)
    {
      // Depending on the type of iterator, this might be O(N), but the
      // rest of the function will dominate regardless.
      size_t numberOfInputPoints = points3DEnd - points3DBegin;

      // For each 2D point, w_i = [u_i, v_i, 1]^T, expressed in 2D
      // homogeneous coordinates, and corresponding 3D point, d_i =
      // [x_i, y_i, z_i, 1]^T, expressed in 3D homogeneous coordinates,
      // the projection equation is
      //
      //   alpha * w_i = P * d_i
      // 
      // where alpha is an arbitrary scale factor, and P is the pinhole
      // projection matrix.
      // 
      //       | k_x, 0.0, u_0, 0.0 |
      //   P = | 0.0, k_y, v_0, 0.0 |
      //       | 0.0, 0.0, 1.0, 0.0 |
      //
      // This implies that w_i and (P * d_i) are parallel,
      // which implies that their cross product has zero magnitude:
      //
      //   cross(w_i, P * d_i) = [0.0, 0.0, 0.0]^T
      //
      // This cross product can be computed as
      //
      //   cross(w_i, P * d_i) = skewSymmetric(w_i) * (P * d_i)
      //
      // We can rearrange the matrix*vector product P * d_i to get
      // Equation 1:
      //
      //   cross(w_i, P * d_i)
      //     = skewSymmetric(w_i) * equivalentMatrix(d_i) * vec(P)
      //     = [0.0, 0.0, 0.0]^T
      //    
      // We get one of these sets of linear equations in the elements of
      // P for each pair of input points.  We can assemble the equations
      // into a linear system and solve for vec(P).
      // 
      //   A * vec(P) = [0.0, 0.0, ..., 0.0]
      //
      // In practice, though, we see that P is very sparse, and has one
      // element itentically equal to zero.  The first of these two
      // observations means that we must drop many of the columns of
      // equivalentMatrix(d_i).  Rather than use the prefab functino
      // "equivalentMatrix()" and ignore remove unused columns, we
      // hand-roll our own sparse equivalentMatrix below.
      //
      // The second observation (that the [2, 3] element of P is equal
      // to 1.0) lets us rearrange the matrix equation above.  Here is
      // the P * d_i product (the equivalentMatrix(d_i) * vec(P)
      // product), written out by individual elements:
      //
      //   | x_i, 0.0, z_i, 0.0 |   | k_x |   | 0.0 |
      //   | 0.0, y_i, 0.0, z_i | * | k_y | + | 0.0 | 
      //   | 0.0, 0.0, 0.0, 0.0 |   | u_0 |   | z_i |
      //                            | v_0 |
      //
      // Here is skewSymmetric(w_i) written out:
      //
      //                        |  0.0, -1.0,  v_i |
      //   skewSymmetric(w_i) = |  1.0,  0.0, -u_i |
      //                        | -v_i,  u_i,  0.0 |
      //
      // Substituting the previous two equations into Eq. 1, we have:
      //
      //   |      0.0,    -y_i,      0.0,   -z_i |   | k_x |   |  v_i * z_i |
      //   |      x_i,     0.0,      z_i,    0.0 | * | k_y | + | -u_i * z_i |
      //   | -v_i*x_i, u_i*y_i, -v_i*z_i, u_i*z_i|   | u_0 |   |     0.0    |
      //                                             | v_0 |
      //
      //          | 0.0 |
      //        = | 0.0 |
      //          | 0.0 |
      //
      // Subtracting from both sides the vector that does not depend on
      // our unknown parameters gives:
      //
      //   |      0.0,    -y_i,      0.0,   -z_i |   | k_x |   | -v_i * z_i |
      //   |      x_i,     0.0,      z_i,    0.0 | * | k_y | = |  u_i * z_i |
      //   | -v_i*x_i, u_i*y_i, -v_i*z_i, u_i*z_i|   | u_0 |   |     0.0    |
      //                                             | v_0 |
      // These are the equations implemented below.
      numeric::Array2D<double> AMatrix(3 * numberOfInputPoints, 4);
      numeric::Array1D<double> bVector(3 * numberOfInputPoints);
      unsigned int rowIndex = 0;
      while(points3DBegin != points3DEnd) {
        AMatrix(rowIndex, 0) = 0.0;
        AMatrix(rowIndex, 1) = -(points3DBegin->y());
        AMatrix(rowIndex, 2) = 0.0;
        AMatrix(rowIndex, 3) = -(points3DBegin->z());
        AMatrix(rowIndex + 1, 0) = points3DBegin->x();
        AMatrix(rowIndex + 1, 1) = 0.0;
        AMatrix(rowIndex + 1, 2) = points3DBegin->z();
        AMatrix(rowIndex + 1, 3) = 0.0;
        AMatrix(rowIndex + 2, 0) = -(points2DBegin->y()) * points3DBegin->x();
        AMatrix(rowIndex + 2, 1) = points2DBegin->x() * points3DBegin->y();
        AMatrix(rowIndex + 2, 2) = -(points2DBegin->y()) * points3DBegin->z();
        AMatrix(rowIndex + 2, 3) = points2DBegin->x() * points3DBegin->z();
        bVector(rowIndex) = AMatrix(rowIndex + 2, 2);
        bVector(rowIndex + 1) = AMatrix(rowIndex + 2, 3);
        bVector(rowIndex + 2) = 0.0;

        rowIndex += 3;
        ++points2DBegin;
        ++points3DBegin;
      }

      // Now we can solve for our unknow parameters.
      numeric::Array2D<double> ATranspose = AMatrix.transpose();
      numeric::Array2D<double> ATA = numeric::matrixMultiply(
        ATranspose, AMatrix);
      numeric::Array1D<double> ATb = numeric::matrixMultiply(
        ATranspose, bVector);
      linearAlgebra::linearSolveInPlace(ATA, ATb);

      // ...which are returned in ATb.
      double k_x = ATb[0];
      double k_y = ATb[1];
      double u_0 = ATb[2];
      double v_0 = ATb[3];

      return CameraIntrinsicsPinhole(
        numPixelsX, numPixelsY, 1, 1.0 / k_x, 1.0 / k_y, u_0, v_0);
    }

  } // namespace computerVision
  
} // namespace dlr

#endif /* #ifndef DLR_COMPUTERVISION_CALIBRATIONTOOLS_H */