AbsoluteOrientation.cpp

#include "AbsoluteOrientation.hh"
#include <Base/Math/Matrix3x3.hh>
#include <Base/Math/Matrix.hh>
#include <Base/Math/Vector.hh>
#include <Base/Geometry/Quaternion.hh>
#include <MathAlgo/SVD.hh>
#include <MathAlgo/SVD3x3.hh>
#include <iostream>

using namespace std;
using namespace BIAS;

const unsigned int AbsoluteOrientation::MIN_CORRESPONDENCES = 3;

AbsoluteOrientation::AbsoluteOrientation()
  : epsilon_(1e-14), rotationAlgorithm_(HORN_ALGORITHM)
{
}

AbsoluteOrientation::~AbsoluteOrientation()
{
}

int AbsoluteOrientation::
Compute_(const std::vector< BIAS::Vector3<double> > &X,
         const std::vector< BIAS::Vector3<double> > &Y,
         BIAS::RMatrix &dR, BIAS::Vector3<double> &dt,
         double &ds, bool enforceScale, const std::vector<double> &w)
{
  const unsigned int num = X.size();
  const bool useWeights = (w.size() > 0);
  if (num < MIN_CORRESPONDENCES) {
    return -1;
  }
  BIASASSERT(X.size() == num && Y.size() == num);
  BIASASSERT(!useWeights || w.size() == num);

  // Compute centroids CX, CY of point sets X, Y
  BIAS::Vector3<double> centX(0.0, 0.0, 0.0), centY(0.0, 0.0, 0.0);
  if (useWeights) {
    double sumWeights = 0.0;
    for (register unsigned int i = 0; i < num; i++) {
      BIASASSERT(w[i] >= 0.0);
      sumWeights += w[i];
      centX.AddIP(w[i]*X[i]);
      centY.AddIP(w[i]*Y[i]);
    }
    centX.DivideIP(sumWeights);
    centY.DivideIP(sumWeights);
  } else {
    for (register unsigned int i = 0; i < num; i++) {
      centX.AddIP(X[i]);
      centY.AddIP(Y[i]);
    }
    centX.DivideIP((double)num);
    centY.DivideIP((double)num);
  }

  // Normalize point sets with respect to their centroids
  std::vector< BIAS::Vector3<double> > normX;
  std::vector< BIAS::Vector3<double> > normY;
  normX.reserve(num);
  normY.reserve(num);
  for (register unsigned int i = 0; i < num; i++) {
    // If the normalised point is close to (0,0,0), we skip it, because we
    // can not compute the cross product with such points
    const Vector3<double> normPointX = X[i] - centX;
    const Vector3<double> normPointY = Y[i] - centY;

    if (normPointX.NormL2() > epsilon_ && normPointY.NormL2() > epsilon_) {
      normX.push_back(X[i] - centX);
      normY.push_back(Y[i] - centY);
    }
/*
#ifdef BIAS_DEBUG
    else {
      BIASWARN("Skipping correspondence pair - at least one point is too close "
               "to the centroid!");
    }
#endif
*/
  }

  // We may have skipped too many points near the centroids...
  if (normX.size() < MIN_CORRESPONDENCES) {
    return -1;
  }

  // Compute rotation
  int res = ComputeRotation(normX, normY, dR, w);

  // Compute scale (opt.) and translation
  if (res == 0) {
    if (!enforceScale) {
      double sumX = 0, sumY = 0;
      if (useWeights) {
        for (register unsigned int i = 0; i < num; i++) {
          sumX += w[i]*normX[i].ScalarProduct(normX[i]);
          sumY += w[i]*normY[i].ScalarProduct(normY[i]);
        }
      } else {
        for (register unsigned int i = 0; i < num; i++) {
          sumX += normX[i].ScalarProduct(normX[i]);
          sumY += normY[i].ScalarProduct(normY[i]);
        }
      }
      BIASASSERT(sumX > epsilon_);
      ds = sqrt(sumY/sumX);
    }
    dt = centY - ds*dR*centX;
  }

  return res;
}

int AbsoluteOrientation::
ComputeRotation(const std::vector< BIAS::Vector3<double> > &X,
                const std::vector< BIAS::Vector3<double> > &Y,
                BIAS::RMatrix &dR, const std::vector<double> &w)
{
  switch (rotationAlgorithm_)
  {
    case HORN_ALGORITHM:
      return ComputeRotationHorn_(X, Y, dR, w);
    case KABSCH_ALGORITHM:
      return ComputeRotationKabsch_(X, Y, dR, w);
  }
  BIASERR("Unknown rotation estimation method given!");
  return -1;
}

int AbsoluteOrientation::
ComputeRotationHorn_(const std::vector< BIAS::Vector3<double> > &X,
                     const std::vector< BIAS::Vector3<double> > &Y,
                     BIAS::RMatrix &dR, const std::vector<double> &w)
{
  const unsigned int num = X.size();
  const bool useWeights = (w.size() > 0);
  if (num < MIN_CORRESPONDENCES) {
    return -1;
  }
  BIASASSERT(X.size() == num && Y.size() == num);
  BIASASSERT(!useWeights || w.size() == num);

  int res = 0;
  BIAS::Matrix3x3<double> S(MatrixZero);
  BIAS::Matrix<double> N(4, 4, MatrixZero);

  // Compute covariance matrix S = X'Y from corresponding 3d points
  if (useWeights) {
    for (register unsigned int i = 0; i < num; i++) {
      BIASASSERT(X[i].NormL2() > epsilon_ && Y[i].NormL2() > epsilon_);
      S += w[i]*X[i].OuterProduct(Y[i]);
    }
  } else {
    for (register unsigned int i = 0; i < num; i++) {
      BIASASSERT(X[i].NormL2() > epsilon_ && Y[i].NormL2() > epsilon_);
      S += X[i].OuterProduct(Y[i]);
    }
  }

  // Solve Total Least Squares problem via SVD
  N[0][0] = S[0][0]+S[1][1]+S[2][2];
  N[0][1] = S[1][2]-S[2][1];
  N[0][2] = S[2][0]-S[0][2];
  N[0][3] = S[0][1]-S[1][0];
  N[1][0] = S[1][2]-S[2][1];
  N[1][1] = S[0][0]-S[1][1]-S[2][2];
  N[1][2] = S[0][1]+S[1][0];
  N[1][3] = S[2][0]+S[0][2];
  N[2][0] = S[2][0]-S[0][2];
  N[2][1] = S[0][1]+S[1][0];
  N[2][2] =-S[0][0]+S[1][1]-S[2][2];
  N[2][3] = S[1][2]+S[2][1];
  N[3][0] = S[0][1]-S[1][0];
  N[3][1] = S[2][0]+S[0][2];
  N[3][2] = S[1][2]+S[2][1];
  N[3][3] =-S[0][0]-S[1][1]+S[2][2];

  BIAS::SVD svd;
  res = svd.Compute(N);

  if (res == 0) {
    BIAS::Vector<double> v4;
    BIAS::Quaternion<double> dq;
    v4 = svd.GetV().GetCol(0);
    dq.SetQuaternion(v4[0], v4[1], v4[2], v4[3]);
    dq.Normalize();
    dR.SetFromQuaternion(dq);
  }

  return res;
}

int AbsoluteOrientation::
ComputeRotationKabsch_(const std::vector< BIAS::Vector3<double> > &X,
                       const std::vector< BIAS::Vector3<double> > &Y,
                       BIAS::RMatrix &dR, const std::vector<double> &w)
{
  const unsigned int num = X.size();
  const bool useWeights = (w.size() > 0);
  if (num < MIN_CORRESPONDENCES) {
    return -1;
  }
  BIASASSERT(X.size() == num && Y.size() == num);
  BIASASSERT(!useWeights || w.size() == num);

  int res = 0;
  BIAS::Matrix3x3<double> S(MatrixZero);

  // Compute covariance matrix S = X'Y from corresponding 3d points
  if (useWeights) {
    for (register unsigned int i = 0; i < num; i++) {
      BIASASSERT(X[i].NormL2() > epsilon_ && Y[i].NormL2() > epsilon_);
      S += w[i]*Y[i].OuterProduct(X[i]);
    }
  } else {
    for (register unsigned int i = 0; i < num; i++) {
      BIASASSERT(X[i].NormL2() > epsilon_ && Y[i].NormL2() > epsilon_);
      S += Y[i].OuterProduct(X[i]);
    }
  }

  // Compute optimal rotation matrix via SVD
  BIAS::SVD3x3 svd;
  res = svd.Compute(BIAS::Matrix<double>(S));

  if (res == 0) {
    BIAS::Matrix<double> D(3, 3, MatrixIdentity);
    // correct rotation matrix to insure a right-handed coordinate system
    if (S.GetDeterminant() < 0) D[2][2] = -1.0;
    // calculate optimal rotation matrix
    BIAS::Matrix<double> UD(3, 3, MatrixZero), UDVT(3, 3, MatrixZero);
    svd.GetU().Mult(D, UD);
    UD.Mult(svd.GetVT(), UDVT);
    dR = BIAS::RMatrix(UDVT);
  }

  return res; 
}

double AbsoluteOrientation::
GetResidualErrors(std::vector<double> &errors,
                  const std::vector< BIAS::Vector3<double> > &X,
                  const std::vector< BIAS::Vector3<double> > &Y,
                  const BIAS::RMatrix &dR, const BIAS::Vector3<double> &dt,
                  const double &ds, const std::vector<double> &w)
{
  const unsigned int num = X.size();
  const bool useWeights = (w.size() > 0);

  BIASASSERT(X.size() == num && Y.size() == num);
  BIASASSERT(!useWeights || w.size() == num);

  // Evaluate error residuals and compute sum of squared errors
  errors.resize(num);
  double error = 0.0;
  BIAS::Vector3<double> e;
  if (useWeights) {
    for (register unsigned int i = 0; i < num; i++) {
      e = Y[i] - ds*dR*X[i] - dt;
      errors[i] = w[i]*e.ScalarProduct(e);
      error += errors[i];
    }
  } else {
    for (register unsigned int i = 0; i < num; i++) {
      e = Y[i] - ds*dR*X[i] - dt;
      errors[i] = e.ScalarProduct(e);
      error += errors[i];
    }
  }
  return error;
}

double AbsoluteOrientation::
GetResidualErrors(std::vector<double> &errors,
                  const std::vector< BIAS::Vector3<double> > &X,
                  const std::vector< BIAS::Vector3<double> > &Y,
                  const std::vector< BIAS::Matrix3x3<double> > &CovX,
                  const std::vector< BIAS::Matrix3x3<double> > &CovY,
                  const BIAS::RMatrix &dR, const BIAS::Vector3<double> &dt,
                  const double &ds)
{
  const unsigned int num = X.size();

  BIASASSERT(X.size() == num && Y.size() == num);
  BIASASSERT(CovX.size() == num && CovY.size() == num);

  // Evaluate error residuals and compute sum of squared errors
  errors.resize(num);
  double error = 0.0;
  BIAS::Matrix3x3<double> Cov, invCov;
  BIAS::Vector3<double> e, invCovE;
  for (register unsigned int i = 0; i < num; i++) {
    Cov = ds*ds*dR*(CovX[i]*dR.Transpose()) + CovY[i];
    if (Cov.GetInverse(invCov) != 0) {
      BEXCEPTION("Unable to invert covariance matrix: " << Cov);
    }
    e = Y[i] - ds*dR*X[i] - dt;
    invCov.Mult(e, invCovE);
    errors[i] = e.ScalarProduct(invCovE);
    error += errors[i];
  }
  return error;
}

double AbsoluteOrientation::
GetResidualErrors(std::vector<double> &errors,
                  const std::vector< BIAS::Vector3<double> > &X,
                  const std::vector< BIAS::Vector3<double> > &Y,
                  const BIAS::RMatrix &dR, const std::vector<double> &w)
{
  return GetResidualErrors(errors, X, Y,
                           dR, BIAS::Vector3<double>(0,0,0), 1.0, w);
}

double AbsoluteOrientation::
GetResidualErrors(std::vector<double> &errors,
                  const std::vector< BIAS::Vector3<double> > &X,
                  const std::vector< BIAS::Vector3<double> > &Y,
                  const std::vector< BIAS::Matrix3x3<double> > &CovX,
                  const std::vector< BIAS::Matrix3x3<double> > &CovY,
                  const BIAS::RMatrix &dR)
{
  return GetResidualErrors(errors, X, Y, CovX, CovY,
                           dR, BIAS::Vector3<double>(0,0,0), 1.0);
}