Quadric3D.cpp

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#include "Quadric3D.hh"
#include <climits>

#define SVD_ZERO_THRESH DBL_EPSILON * 16.0

#define SINGULAR_THRESH_INVERSION 1e-20

using namespace BIAS;
using namespace std;

Quadric3D::Quadric3D(const HomgPoint3D& C, 
                     const CovMatrix3x3& cov, 
                     const double &dScale) {
  BIAS::Matrix3x3<double> InvCov, Tmp = cov * (dScale * dScale);
  
  int InversionResult = Tmp.GetInverse(InvCov);
  if (InversionResult!=0) {
    // degenerate covariance matrix !
    SVD CovSVD((Matrix<double>)Tmp);
    Vector<double> S;
    S = CovSVD.GetS();
    Matrix3x3<double> V, UT, SdT;
    SdT.SetZero();

    if (S[0]<SINGULAR_THRESH_INVERSION) 
      S[0]=SINGULAR_THRESH_INVERSION;
    if (S[1]<SINGULAR_THRESH_INVERSION) 
      S[1]=SINGULAR_THRESH_INVERSION;
    if (S[2]<SINGULAR_THRESH_INVERSION) 
      S[2]=SINGULAR_THRESH_INVERSION;

    SdT[0][0] = 1.0/S[0];
    SdT[1][1] = 1.0/S[1];
    SdT[2][2] = 1.0/S[2];

    V = CovSVD.GetV();
    UT = CovSVD.GetU().Transpose();
    InvCov = V * SdT * UT;
    //cout << "using svd to invert Tmp "<< Tmp<<"\n"<<InvCov<<endl;
  }

  BIAS::Matrix4x4<double> EllipsoidAtOrigin;
  EllipsoidAtOrigin.SetIdentity();
  for (unsigned int i=0; i<3; i++) {
    for (unsigned int j=0; j<3; j++) {
      EllipsoidAtOrigin[i][j] =  InvCov[i][j];
    }
  }
  EllipsoidAtOrigin[3][3] = -1;

  Matrix4x4<double> MoveToOrigin;
  MoveToOrigin.SetIdentity();
  
  for (int i=0; i<3; i++) {
    MoveToOrigin[i][3]  =  -C[i];
  }
  MoveToOrigin[3][3] = C[3];
  
 
  (*this) = MoveToOrigin.Transpose() * (EllipsoidAtOrigin * MoveToOrigin);
  // Quadric has radii dScale*sigma_i and is centered at point3d now

  Normalize();
}

unsigned int Quadric3D::GetSignature() {
  // do singular value decomposition of this
  SVD svd(*this);
  Vector4<double> S;
  Matrix4x4<double> VT;
  S = BIAS::Vector4<COVMATRIX3X3_TYPE>(svd.GetS());
  VT = svd.GetVT();
  
  // now we need the eigenvalues (with sign !)
  // compute this by projecting the (normalized) eigenvectors onto their images
  Vector4<double> eigenvector, eigenimage;
  signed char Signature = 0;
  for (unsigned int i=0; i<4; i++) {
    // get ith eigenvector and its image
    eigenvector = VT.GetRow(i);
    eigenimage = (*this)*eigenvector; 
    // compute scalar product, we are not "thinking" homogenized now
    // this is simply an analysis of a 4x4 matrix !
    S[i] = eigenimage[0] * eigenvector[0] + eigenimage[1] * eigenvector[1]+
      eigenimage[2] * eigenvector[2]+  eigenimage[3] * eigenvector[3];
    if (S[i]<-SVD_ZERO_THRESH) Signature--;
    else if (S[i]>SVD_ZERO_THRESH) Signature++;
  }
    
  // return number of eigenvalues with one sign minus number of eigenvalues
  // with opposite sign, choosing signs such that difference is not negative
  if (Signature<0) return (unsigned int)(-Signature);
  return (unsigned int)Signature;
}

Quadric3D Quadric3D::GetDualQuadric(bool UseSVD) const {
  if (UseSVD) {
    // we want to be numerically robust
    SVD qsvd((*this), SVD_ZERO_THRESH, false);
    return (qsvd.Invert()); 
  }

  // do it the fast way
  const QUADRIC3D_TYPE& a = (*this)[0][0];
  const QUADRIC3D_TYPE& b = (*this)[0][1];
  const QUADRIC3D_TYPE& c = (*this)[0][2];
  const QUADRIC3D_TYPE& d = (*this)[0][3];
  const QUADRIC3D_TYPE& e = (*this)[1][1];
  const QUADRIC3D_TYPE& f = (*this)[1][2];
  const QUADRIC3D_TYPE& g = (*this)[1][3];
  const QUADRIC3D_TYPE& h = (*this)[2][2];
  const QUADRIC3D_TYPE& i = (*this)[2][3];
  const QUADRIC3D_TYPE& k = (*this)[3][3];

  // analytical inverse, neglecting determinant
  Quadric3D Dual;
  Dual[0][0] = 2.0*f*g*i - e*i*i - f*f*k + e*h*k - g*g*h;
  Dual[1][0] = Dual[0][1] = d*g*h - d*f*i - c*g*i + b*i*i + c*f*k - b*h*k;
  Dual[2][0] = Dual[0][2] = c*g*g + d*e*i - b*g*i - c*e*k + b*f*k  -d*f*g;
  Dual[3][0] = Dual[0][3] = b*g*h + d*f*f - d*e*h + c*e*i - b*f*i -c*f*g;
  Dual[1][1] = 2.0*c*d*i - a*i*i - c*c*k + a*h*k  -d*d*h;
  Dual[2][1] = Dual[1][2] = a*g*i - d*(c*g + b*i - d*f) + b*c*k - a*f*k;
  Dual[3][1] = Dual[1][3] = b*d*h - a*g*h + a*f*i - c*(d*f + b*i - c*g);
  Dual[2][2] = 2.0*b*d*g - a*g*g - b*b*k + a*e*k - d*d*e;
  Dual[3][2] = Dual[2][3] = c*d*e - b*d*f - b*c*g + a*f*g + b*b*i - a*e*i;
  Dual[3][3] = 2.0*b*c*f - a*f*f - b*b*h + a*e*h -c*c*e;
  return Dual;
}