fivePointAlgorithm.h

Go to the documentation of this file.

#ifndef DLR_COMPUTERVISION_FIVEPOINTALGORITHM_H
#define DLR_COMPUTERVISION_FIVEPOINTALGORITHM_H

#include <dlrNumeric/array1D.h>
#include <dlrNumeric/array2D.h>
#include <dlrNumeric/transform3D.h>
#include <dlrNumeric/vector2D.h>
#include <dlrRandom/pseudoRandom.h>


namespace dlr {

  namespace computerVision {

    template<class Iterator>
    std::vector< dlr::numeric::Array2D<double> >
    fivePointAlgorithm(Iterator sequence0Begin, Iterator sequence0End,
                       Iterator sequence1Begin);


    template<class Iterator>
    dlr::numeric::Array2D<double>
    fivePointAlgorithmRobust(Iterator sequence0Begin, Iterator sequence0End,
                             Iterator sequence1Begin,
                             size_t iterations,
                             double inlierProportion,
                             double& score,
                             dlr::random::PseudoRandom pRandom
                             = dlr::random::PseudoRandom());


    template<class Iterator>
    void
    fivePointAlgorithmRobust(Iterator sequence0Begin, Iterator sequence0End,
                             Iterator sequence1Begin,
                             Iterator sequence2Begin,
                             size_t iterations,
                             double inlierProportion,
                             dlr::numeric::Array2D<double>& cam2Ecam0,
                             dlr::numeric::Transform3D& cam0Tcam2,
                             dlr::numeric::Transform3D& cam1Tcam2,
                             double& score,
                             dlr::random::PseudoRandom pRandom
                             = dlr::random::PseudoRandom());
    
    
    // Return value is residual in pix^2.
    double
    checkEpipolarConstraint(dlr::numeric::Array2D<double> const& fundamentalMx,
                            dlr::numeric::Vector2D& point0,
                            dlr::numeric::Vector2D& point1);


    // WARNING(xxx): We have observed at least one case in which the
    // return value from this function is fishy.  We currently do not
    // trust it.
    // 
    // Warning Think of this function as being "private."  Gets cam1Tcam0.
    // Might throw if test points are on parallel rays.
    dlr::numeric::Transform3D
    getCameraMotionFromEssentialMatrix(
      dlr::numeric::Array2D<double> const& EE,
      dlr::numeric::Vector2D const& testPointCamera0,
      dlr::numeric::Vector2D const& testPointCamera1);


    // Returns point in CS of camera 0.  Note that argument is the
    // inverse of what's returned by
    // getCameraMotionFromEssentialMatrix().  Might throw if test
    // points are on parallel rays.
    dlr::numeric::Vector3D
    triangulateCalibratedImagePoint(
      dlr::numeric::Transform3D const& c0Tc1,
      dlr::numeric::Vector2D const& testPointCamera0,
      dlr::numeric::Vector2D const& testPointCamera1);
    

    dlr::numeric::Array2D<double>
    generateFivePointConstraintMatrix(
      dlr::numeric::Array2D<double> const& E0Array,
      dlr::numeric::Array2D<double> const& E1Array,
      dlr::numeric::Array2D<double> const& E2Array,
      dlr::numeric::Array2D<double> const& E3Array);

  } // namespace computerVision
    
} // namespace dlr


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


#include <cmath>
#include <complex>
#include <limits>
#include <dlrComputerVision/cameraIntrinsicsPinhole.h>
#include <dlrComputerVision/threePointAlgorithm.h>
#include <dlrGeometry/ray2D.h>
#include <dlrGeometry/utilities2D.h>
#include <dlrLinearAlgebra/linearAlgebra.h>
#include <dlrNumeric/subArray2D.h>
#include <dlrNumeric/utilities.h>

namespace dlr {

  namespace computerVision {


    template<class Iterator>
    std::vector< dlr::numeric::Array2D<double> >
    fivePointAlgorithm(Iterator sequence0Begin, Iterator sequence0End,
                       Iterator sequence1Begin)
    {
      // The five (or more) pairs of input points give at least five
      // linear constraints on the essential matrix, E.  Since E has
      // nine elements, this means there's a four-dimentional null
      // space of these constraints.  We solve for an orthogonal basis
      // of this null space here.

      // For each pair of points q, q', the essential matrix, E,
      // satisfies the equation:
      //
      //   transpose(q') * E * q = 0
      //
      // where the points q are drawn from sequence0, and the points
      // q' are drawn from sequence1.
      // 
      // We rearrange this equation to get
      //
      //   ||e_00*q_x*q'_x + e_01*q_y*q'_x + e_02*q'_x
      //     + e_10*q_x*q'_y + e_11*q_y*q'_y + e_12*q'_y
      //     + e_20*q_x + e_21*q_y + e_22|| = 0
      //
      // where e_ij is the element of E in the i^th row and the j^th
      // column, q_x & q_y are the x & y components of q,
      // respectively, and q'_x & q'_y are the x & y components of q',
      // respectively.
      //
      // or,
      // 
      //   ||A * vec(E)|| = 0
      //
      // With the matrix A as specified in the code below.
      size_t numberOfCorrespondences = sequence0End - sequence0Begin;
      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::Vector2D& qq = *sequence0Begin;
        const dlr::numeric::Vector2D& qPrime = *sequence1Begin;
        currentRow[0] = qq.x() * qPrime.x();
        currentRow[1] = qq.y() * qPrime.x();
        currentRow[2] = qPrime.x();
        currentRow[3] = qq.x() * qPrime.y();
        currentRow[4] = qq.y() * qPrime.y();
        currentRow[5] = qPrime.y();
        currentRow[6] = qq.x();
        currentRow[7] = qq.y();
        currentRow[8] = 1.0;
        ++sequence0Begin;
        ++sequence1Begin;
      }

      // Following [1], we solve for the four dimensional null space
      // using SVD.
      dlr::numeric::Array2D<double> uArray;
      dlr::numeric::Array1D<double> sigmaArray;
      dlr::numeric::Array2D<double> vTransposeArray;
      dlr::linearAlgebra::singularValueDecomposition(
        AMatrix, uArray, sigmaArray, vTransposeArray, true);
      dlr::numeric::Array2D<double> E0Array(3, 3);
      dlr::numeric::Array2D<double> E1Array(3, 3);
      dlr::numeric::Array2D<double> E2Array(3, 3);
      dlr::numeric::Array2D<double> E3Array(3, 3);
      std::copy(vTransposeArray.getRow(5).begin(),
                vTransposeArray.getRow(5).end(),
                E0Array.begin());
      std::copy(vTransposeArray.getRow(6).begin(),
                vTransposeArray.getRow(6).end(),
                E1Array.begin());
      std::copy(vTransposeArray.getRow(7).begin(),
                vTransposeArray.getRow(7).end(),
                E2Array.begin());
      std::copy(vTransposeArray.getRow(8).begin(),
                vTransposeArray.getRow(8).end(),
                E3Array.begin());

      // Let E = x*E0 + y*E1 + z*E2 + w*E3, and assume w == 1 (Yuck.
      // We will fix this soon).  We have (equations (2) and (3) of [1])
      //
      //   det(E) = 0
      //
      // and
      // 
      //   2 * E * transpose(E) * E - trace(E * transpose(E)) * E = 0
      //
      // These two equations give us ten cubic constraints on x, y,
      // and z.  As described in the paper, we write these constraints
      // as the matrix product of a 10x20 coefficient matrix, M, and a
      // monomials vector X.
      //
      //   M * X = [0]
      //
      //   X = [x^3, x^2*y, x^2*z, x*y^2, x*y*z, x*z^2, y^3, y^2*z, y*z^2, z^3,
      //        x^2, x*y, x*z, y^2, y*z, z^2, x, y, z, 1]
      //
      // Although computing M just involves doing a bunch of matrix
      // multiplications, tranposes, determinants, etc., the
      // computation is quite tedious.  In fact, even typing in the
      // result is quite tedious.  For this reason, we use
      // automatically generated C code exported from the computer
      // algebra system Maxima.
      dlr::numeric::Array2D<double> M = generateFivePointConstraintMatrix(
        E0Array, E1Array, E2Array, E3Array);

#if 0
      dlr::numeric::Array2D<double> diff = M - MnL;
      dlr::numeric::Array2D<bool> flags(diff.rows(), diff.columns());
      for(size_t nn = 0; nn < diff.size(); ++nn) {
        flags[nn] = std::fabs(diff[nn]) < 1.0E-9;
      }

      std::cout << "===================" << std::endl;
      double target = MnL(0, 0);
      for(size_t mm = 0; mm < M.rows(); ++mm) {
        for(size_t nn = 0; nn < M.columns(); ++nn) {
          if(std::fabs(M(mm, nn) - target) < 1.0E-9) {
            std::cout << " [" << mm << ", " << nn << "] matches." << std::endl;
          }
        }
      }
      std::cout << "===================" << std::endl;
#endif
      
      // The paper calls for Gauss-Jordan elimination to reduce the
      // first 10 columns of M to the identity matrix, so that the
      // rows of the eliminated matrix form a Groebner basis of the
      // ideal of the 10 original cubic constraints (see [3] for more
      // information on Groebner bases, ideals, and algebraic geometry
      // in general).  We don't have a Gauss-Jordan elimination
      // routine coded up, so we use what we have available.

      // Extract the first 10 columns of M.
      dlr::numeric::Array2D<double> M0 = dlr::numeric::subArray(
        M, dlr::numeric::Slice(), dlr::numeric::Slice(0, 10));

      // Invert them (presumably using Gauss-Jordan elimination.
      dlr::numeric::Array2D<double> M0Inv = dlr::linearAlgebra::inverse(M0);

      // Extract the 10th through 20th columns of M.
      dlr::numeric::Array2D<double> M1 = dlr::numeric::subArray(
        M, dlr::numeric::Slice(), dlr::numeric::Slice(10, 0));

      // And manipulate them so that they look like they went through
      // the same elimination process.  Following the paper, we call
      // the result of this manipulation B, although the real Brobner
      // basis is the set of polynomials whose coefficients are drawn
      // from the 10x20 matrix resulting from the elimination (first
      // 10 columns are 10x10 identity matrix, remaining columns equal
      // to our 10x10 matrix B.
      dlr::numeric::Array2D<double> B = dlr::numeric::matrixMultiply(M0Inv, M1);

      // Since we eliminated the left half of M, we're safe to assume
      // that the leading terms of the Groebner basis we just computed
      // do not include the monomials x, y, or z.  This means that x,
      // y, and z will be in the quotient ring of complex third-order
      // polynomials over the ideal of the polynomials expressed in M.
      // 
      // Theorem xxx in [3] gives us that, at the solutions, x, of the
      // polynomials in M,
      //
      //   f(x) * transpose(u(x)) = transpose(u(x)) * A_f
      // 
      // Where f(x) is an arbitrary polynomial in the quotient ring,
      // u(x) is the vector of basis monomials of the quotient ring,
      // and A_f is the action matrix associated with f(x).
      //
      // This is useful because it means that, at the solutions to our
      // system of third order polynomials, the values of the
      // monomials in u(x) correspond to the left eigenvectors of A_f.
      // But we've already agreed that x, y, and z are three of the
      // monomials in u(x), so if we can just find A_f for an
      // arbitrary polynomial in the quotient ring, then we can take
      // its left eigenvectors and grab the elements corresponding to
      // x, y, and z to find our solutions!
      //
      // We'll use the action matrix for the polynomial x, and
      // transpose it so that we can use our normal right-eigenvector
      // solver to find the eigenvectors.

      // Warning(xxx): We'll use the action matrix from Stewenius &
      // Nister's paper, which doesn't appear to correspond to
      // suggested degree-then-lexicographic order of monomials.  The
      // right solution to this is to generate the correct action
      // matrix below, but instead we temporarily shuffle the columns
      // of our constraint matrix to match the action matrix.  This
      // shuffling happens under the hood in the call to
      // generateFivePointConstraintMatrix(), above.
      dlr::numeric::Array2D<double> At(10, 10);
      At = 0.0;
      for(size_t ii = 0; ii < 10; ++ii) {
        At(0, ii) = -B(0, ii);
        At(1, ii) = -B(1, ii);
        At(2, ii) = -B(2, ii);
        At(3, ii) = -B(4, ii);
        At(4, ii) = -B(5, ii);
        At(5, ii) = -B(7, ii);
      }
      At(6, 0) = 1.0;
      At(7, 1) = 1.0;
      At(8, 3) = 1.0;
      At(9, 6) = 1.0;

      // Solutions for x, y, z can be found from the 10 eigenvectors
      // of this (tranposed) action matrix.  We normalize the final
      // element of each eigenvector to 1.0.
      dlr::numeric::Array1D< std::complex<Float64> > eigenvalues;
      dlr::numeric::Array2D< std::complex<Float64> > eigenvectors;
      dlr::linearAlgebra::eigenvectors(At, eigenvalues, eigenvectors);
      
      // Now that we have solutions for x, y, and z, we can go back
      // and use them to reconstruct potential solutions for the
      // essential matrix.
      std::vector< dlr::numeric::Array2D<double> > result;
      for(size_t ii = 0; ii < 10; ++ii) {
        if(eigenvalues[ii].imag() == 0) {
          double xx = eigenvectors(6, ii).real() / eigenvectors(9, ii).real();
          double yy = eigenvectors(7, ii).real() / eigenvectors(9, ii).real();
          double zz = eigenvectors(8, ii).real() / eigenvectors(9, ii).real();
          Array2D<double> solution =
            (xx * E0Array) + (yy * E1Array) + (zz * E2Array) + E3Array;
          result.push_back(solution);
        }
      }

      return result;
    }


    template<class Iterator>
    dlr::numeric::Array2D<double>
    fivePointAlgorithmRobust(Iterator sequence0Begin, Iterator sequence0End,
                             Iterator sequence1Begin,
                             size_t iterations,
                             double inlierProportion,
                             double& score,
                             dlr::random::PseudoRandom pRandom)
    {
      // State variables so we'll remember the correct essential
      // matrix once we find it.
      double bestErrorSoFar = std::numeric_limits<double>::max();
      dlr::numeric::Array2D<double> selectedCandidate(3, 3);
      selectedCandidate = 0.0;
      
      // Copy input points into local buffers.
      size_t numberOfPoints = sequence0End - sequence0Begin;
      std::vector<dlr::numeric::Vector2D> qVector(numberOfPoints);
      std::vector<dlr::numeric::Vector2D> qPrimeVector(numberOfPoints);
      std::copy(sequence0Begin, sequence0End, qVector.begin());
      std::copy(sequence1Begin, sequence1Begin + numberOfPoints,
                qPrimeVector.begin());

      for(size_t ii = 0; ii < iterations; ++ii) {
        // Select five points.
        for(size_t jj = 0; jj < 5; ++jj) {
          int selectedIndex = pRandom.uniformInt(jj, numberOfPoints);
          if(selectedIndex != static_cast<int>(jj)) {
            std::swap(qVector[jj], qVector[selectedIndex]);
            std::swap(qPrimeVector[jj], qPrimeVector[selectedIndex]);
          }
        }

        // Get candidate essential matrices.
        std::vector< dlr::numeric::Array2D<double> > EVector =
          fivePointAlgorithm(qVector.begin(), qVector.begin() + 5,
                             qPrimeVector.begin());

        // Test each candidate.
        for(size_t jj = 0; jj < EVector.size(); ++jj) {
          Array2D<double> EE = EVector[jj];

          // Compute residuals for all input points.
          std::vector<double> residualVector(numberOfPoints);
          for(size_t kk = 0; kk < numberOfPoints; ++kk) {
            residualVector[kk] = checkEpipolarConstraint(
              EE, qVector[kk], qPrimeVector[kk]);
          }

          // Compute robust error statistic.
          //
          // Note(xxx): Better not to sort here, since it changes the
          // algorithm to O(NlogN).
          std::sort(residualVector.begin(), residualVector.end());
          int testIndex = static_cast<int>(
            inlierProportion * residualVector.size() + 0.5);
          double errorValue = residualVector[testIndex];

          // Remember candidate if it's the best so far.
          if(errorValue < bestErrorSoFar) {
            selectedCandidate = EE;
            bestErrorSoFar = errorValue;
          }
        }
      }
      score = bestErrorSoFar;
      return selectedCandidate;
    }

    
    template<class Iterator>
    void
    fivePointAlgorithmRobust(Iterator sequence0Begin, Iterator sequence0End,
                             Iterator sequence1Begin,
                             Iterator sequence2Begin,
                             size_t iterations,
                             double inlierProportion,
                             dlr::numeric::Array2D<double>& cam2Ecam0,
                             dlr::numeric::Transform3D& cam0Tcam2,
                             dlr::numeric::Transform3D& cam1Tcam2,
                             double& score,
                             dlr::random::PseudoRandom pRandom)
    {
      // Since we're using calibrated image points, all cameras have
      // the same intrinsics.
      CameraIntrinsicsPinhole intrinsics(1, 1, 1.0, 1.0, 1.0, 0.0, 0.0);
      
      // State variables so we'll remember the correct essential
      // matrices once we find them.
      double bestErrorSoFar = std::numeric_limits<double>::max();
      dlr::numeric::Array2D<double> selectedCam2Ecam0(3, 3);
      dlr::numeric::Transform3D selectedCam0Tcam2;
      dlr::numeric::Transform3D selectedCam1Tcam2;
      selectedCam2Ecam0 = 0.0;
      
      // Copy input points into local buffers.
      size_t numberOfPoints = sequence0End - sequence0Begin;
      std::vector<dlr::numeric::Vector2D> points2D_cam0(numberOfPoints);
      std::vector<dlr::numeric::Vector2D> points2D_cam1(numberOfPoints);
      std::vector<dlr::numeric::Vector2D> points2D_cam2(numberOfPoints);
      std::copy(sequence0Begin, sequence0End, points2D_cam0.begin());
      std::copy(sequence1Begin, sequence1Begin + numberOfPoints,
                points2D_cam1.begin());
      std::copy(sequence2Begin, sequence2Begin + numberOfPoints,
                points2D_cam2.begin());

      // Allocate storage for temporary values prior to starting loop.
      std::vector<dlr::numeric::Vector3D> points3D_cam0(numberOfPoints);
      std::vector<dlr::numeric::Vector3D> points3D_cam1(numberOfPoints);
      std::vector<dlr::numeric::Vector3D> points3D_cam2(numberOfPoints);
      std::vector<double> residualVector(numberOfPoints);

      // Loop over many sets of random samples.
      for(size_t ii = 0; ii < iterations; ++ii) {

        // Select five points.
        for(size_t jj = 0; jj < 5; ++jj) {
          int selectedIndex = pRandom.uniformInt(jj, numberOfPoints);
          if(selectedIndex != static_cast<int>(jj)) {
            std::swap(points2D_cam0[jj], points2D_cam0[selectedIndex]);
            std::swap(points2D_cam1[jj], points2D_cam1[selectedIndex]);
            std::swap(points2D_cam2[jj], points2D_cam2[selectedIndex]);
          }
        }

        // Get candidate essential matrices.
        std::vector< dlr::numeric::Array2D<double> > EVector =
          fivePointAlgorithm(points2D_cam0.begin(), points2D_cam0.begin() + 5,
                             points2D_cam2.begin());

        // Test each candidate.
        for(size_t jj = 0; jj < EVector.size(); ++jj) {
          Array2D<double> EE = EVector[jj];

          // Recover relative motion between cameras, assuming EE is
          // correct.
          dlr::numeric::Transform3D c2Tc0;
          try {
            c2Tc0 = getCameraMotionFromEssentialMatrix(
              EE, points2D_cam0[0], points2D_cam2[0]);
          } catch(dlr::common::ValueException&) {
            // Input points were on parallel rays!  No point in evaluating
            // this candidate.
            continue;
          }

          // Given relative motion, recover 3D position of each input
          // point in camera 2 coordinates.
          for(size_t kk = 0; kk < numberOfPoints; ++kk) {
            points3D_cam2[kk] = triangulateCalibratedImagePoint(
              c2Tc0, points2D_cam2[kk], points2D_cam0[kk]);
          }

          // Recover 3D position and orientation of camera 1.
          double internalScore;
          dlr::numeric::Transform3D c1Tc2 = threePointAlgorithmRobust(
            points3D_cam2.begin(), points3D_cam2.begin() + 5,
            points2D_cam1.begin(), intrinsics, 1, 1.0, internalScore, pRandom);

          // We expect the model to fit these five points better than
          // it fits other points in the set.  If internalScore
          // doesn't beat the average residual over all points for our
          // best so far, then there's no point in continuing with
          // this candidate.
          if(internalScore >= bestErrorSoFar) {
            continue;
          }
          
          // Compute the 3D position of each input point in camera 1
          // coordinates.
          for(size_t kk = 0; kk < numberOfPoints; ++kk) {
            points3D_cam1[kk] = c1Tc2 * points3D_cam2[kk];
          }
          
          // Recover 3D position of each of the input points in camera
          // 0 coordinates.
          dlr::numeric::Transform3D c0Tc2 = c2Tc0.invert();
          for(size_t kk = 0; kk < numberOfPoints; ++kk) {
            points3D_cam0[kk] = c0Tc2 * points3D_cam2[kk];
          }
          
          // Project 3D points into each image, and compute residual.
          for(size_t kk = 0; kk < numberOfPoints; ++kk) {
            dlr::numeric::Vector2D residualVec0(
              points3D_cam0[kk].x() / points3D_cam0[kk].z(),
              points3D_cam0[kk].y() / points3D_cam0[kk].z());
            residualVec0 -= points2D_cam0[kk];
            dlr::numeric::Vector2D residualVec1(
              points3D_cam1[kk].x() / points3D_cam1[kk].z(),
              points3D_cam1[kk].y() / points3D_cam1[kk].z());
            residualVec1 -= points2D_cam1[kk];
            dlr::numeric::Vector2D residualVec2(
              points3D_cam2[kk].x() / points3D_cam2[kk].z(),
              points3D_cam2[kk].y() / points3D_cam2[kk].z());
            residualVec2 -= points2D_cam2[kk];

            residualVector[kk] =
              (dlr::numeric::magnitudeSquared(residualVec0)
               + dlr::numeric::magnitudeSquared(residualVec1)
               + dlr::numeric::magnitudeSquared(residualVec2)) / 3.0;
          }

          // Compute robust error statistic.
          //
          // Note(xxx): Better not to sort here, since it changes the
          // algorithm to O(NlogN).
          std::sort(residualVector.begin(), residualVector.end());
          int testIndex = static_cast<int>(
            inlierProportion * residualVector.size() + 0.5);
          double errorValue = residualVector[testIndex];

          // Remember candidate if it's the best so far.
          if(errorValue < bestErrorSoFar) {
            selectedCam2Ecam0 = EE;
            selectedCam0Tcam2 = c0Tc2;
            selectedCam1Tcam2 = c1Tc2;
            bestErrorSoFar = errorValue;
          }
        }
      }
      score = bestErrorSoFar;
      cam2Ecam0 = selectedCam2Ecam0;
      cam0Tcam2 = selectedCam0Tcam2;
      cam1Tcam2 = selectedCam1Tcam2;
    }

  } // namespace computerVision
    
} // namespace dlr

#endif /* #ifndef DLR_COMPUTERVISION_FIVEPOINTALGORITHM_H */