eightPointAlgorithm.h

Go to the documentation of this file.

#ifndef DLR_COMPUTERVISION_EIGHTPOINTALGORITHM_H
#define DLR_COMPUTERVISION_EIGHTPOINTALGORITHM_H

#include <dlrNumeric/array1D.h>
#include <dlrNumeric/array2D.h>
#include <dlrNumeric/vector2D.h>

namespace dlr {

  namespace computerVision {

    template<class Iterator>
    dlr::numeric::Array2D<double>
    eightPointAlgorithm(Iterator sequence0Begin, Iterator sequence0End,
                        Iterator sequence1Begin);
    
    
    template<class Iterator>
    dlr::numeric::Array2D<double>
    eightPointAlgorithm(Iterator sequence0Begin, Iterator sequence0End,
                        Iterator sequence1Begin,
                        dlr::numeric::Array1D<double>& eigenvalues);



    void
    normalizePointSequence(dlr::numeric::Array2D<double> const& inputPoints,
                           dlr::numeric::Array2D<double>& outputPoints,
                           dlr::numeric::Array2D<double>& transform);

  } // namespace computerVision
    
} // namespace dlr


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


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


namespace dlr {

  namespace computerVision {


    template<class Iterator>
    dlr::numeric::Array2D<double>
    eightPointAlgorithm(Iterator sequence0Begin, Iterator sequence0End,
                        Iterator sequence1Begin)
    {
      dlr::numeric::Array1D<double> eigenvalues;
      return eightPointAlgorithm(
        sequence0Begin, sequence0End, sequence1Begin, eigenvalues);
    }
    
    
    template<class Iterator>
    dlr::numeric::Array2D<double>
    eightPointAlgorithm(Iterator sequence0Begin, Iterator sequence0End,
                        Iterator sequence1Begin,
                        dlr::numeric::Array1D<double>& eigenvalues)
    {
      // Find out how many points we have.  Even if this subtraction
      // is O(N), it will be dominated by the matrix multiplication
      // below.
      size_t numberOfCorrespondences = sequence0End - sequence0Begin;

      
      // Following Hartley, precondition the data by translating and
      // scaling (independently for each image) so that the points
      // roughly form a unit circle.  This greatly improves the
      // stability of the math below.
      dlr::numeric::Array2D<double> inputArray(numberOfCorrespondences, 3);
      dlr::numeric::Array2D<double> inputPrimeArray(numberOfCorrespondences, 3);
      Iterator begin0 = sequence0Begin;
      Iterator begin1 = sequence1Begin;
      size_t pointIndex = 0;
      while(begin0 != sequence0End) {
        dlr::numeric::Array1D<double> tmpRow = inputArray.getRow(pointIndex);
        tmpRow[0] = begin0->x();
        tmpRow[1] = begin0->y();
        tmpRow[2] = 1.0;

        tmpRow = inputPrimeArray.getRow(pointIndex);
        tmpRow[0] = begin1->x();
        tmpRow[1] = begin1->y();
        tmpRow[2] = 1.0;

        ++pointIndex;
        ++begin0;
        ++begin1;
      }

      dlr::numeric::Array2D<double> KKInv;
      dlr::numeric::Array2D<double> normalizedPoints;
      normalizePointSequence(inputArray, normalizedPoints, KKInv);

      dlr::numeric::Array2D<double> KPrimeInv;
      dlr::numeric::Array2D<double> normalizedPrimePoints;
      normalizePointSequence(inputPrimeArray, normalizedPrimePoints, KPrimeInv);

      // For each pair of points u, u', the fundamental matrix, F,
      // satisfies the equation:
      //
      //   transpose(u') * F * u = 0
      //
      // where the points u are drawn from sequence0, and the points
      // u' are drawn from sequence1.
      // 
      // We rearrange this equation to get
      //
      //   ||f_00*u_x*u'_x + f_01*u_y*u'_x + f_02*u'_x
      //     + f_10*u_x*u'_y + f_11*u_y*u'_y + f_12*u'_y
      //     + f_20*u_x + f_21*u_y + f_22|| = 0
      //
      // where f_ij is the element of F in the i^th row and the j^th
      // column, u_x & u_y are the x & y components of u,
      // respectively, and u'_x & u'_y are the x & y components of u',
      // respectively.
      //
      // or,
      // 
      //   ||A * vec(F)|| = 0
      //
      // With the matrix A as specified in the code below.
        
      dlr::numeric::Array2D<double> AMatrix(numberOfCorrespondences, 9);
      for(size_t rowIndex = 0; rowIndex < numberOfCorrespondences; ++rowIndex) {
        dlr::numeric::Array1D<double> currentRow = AMatrix.getRow(rowIndex);
        const dlr::numeric::Array1D<double>& uu =
          normalizedPoints.getRow(rowIndex);
        const dlr::numeric::Array1D<double>& uPrime =
          normalizedPrimePoints.getRow(rowIndex);
        currentRow[0] = uu[0] * uPrime[0];
        currentRow[1] = uu[1] * uPrime[0];
        currentRow[2] = uu[2] * uPrime[0];
        currentRow[3] = uu[0] * uPrime[1];
        currentRow[4] = uu[1] * uPrime[1];
        currentRow[5] = uu[2] * uPrime[1];
        currentRow[6] = uu[0] * uPrime[2];
        currentRow[7] = uu[1] * uPrime[2];
        currentRow[8] = uu[2] * uPrime[2];
      }

      // Solve for the F that minimizes the residual in the least
      // squares sense.
      dlr::numeric::Array2D<double> ATA =
        dlr::numeric::matrixMultiply(AMatrix.transpose(), AMatrix);
      dlr::numeric::Array2D<double> eigenvectors;
      dlr::linearAlgebra::eigenvectorsSymmetric(ATA, eigenvalues, eigenvectors);
      dlr::numeric::Array2D<double> FMatrix(3, 3);
      FMatrix[0] = eigenvectors(0, 8);
      FMatrix[1] = eigenvectors(1, 8);
      FMatrix[2] = eigenvectors(2, 8);
      FMatrix[3] = eigenvectors(3, 8);
      FMatrix[4] = eigenvectors(4, 8);
      FMatrix[5] = eigenvectors(5, 8);
      FMatrix[6] = eigenvectors(6, 8);
      FMatrix[7] = eigenvectors(7, 8);
      FMatrix[8] = eigenvectors(8, 8);

      // Good.  Now we have an estimate for F.  Here we enforce that F
      // must not be full rank.
      dlr::numeric::Array2D<double> uArray;
      dlr::numeric::Array1D<double> sigmaArray;
      dlr::numeric::Array2D<double> vTransposeArray;
      dlr::linearAlgebra::singularValueDecomposition(
        FMatrix, uArray, sigmaArray, vTransposeArray);
      uArray[2] = 0.0;
      for(size_t ii = 0; ii < sigmaArray.size(); ++ii) {
        vTransposeArray.getRow(ii) *= sigmaArray(ii);
      }
      FMatrix = matrixMultiply(uArray, vTransposeArray);

      // Transform back to unnormalized coordinates.
      FMatrix =
        dlr::numeric::matrixMultiply(
          dlr::numeric::matrixMultiply(KPrimeInv.transpose(), FMatrix), KKInv);
      
      return FMatrix;
    }

  } // namespace computerVision
    
} // namespace dlr

#endif /* #ifndef DLR_COMPUTERVISION_EIGHTPOINTALGORITHM_H */