CovMatrix3x3.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 "CovMatrix3x3.hh"

using namespace BIAS;
using namespace std;


CovMatrix3x3::CovMatrix3x3()
  : Matrix3x3<COVMATRIX3X3_TYPE>() {
  _IsDecomposed = false;
}

CovMatrix3x3::~CovMatrix3x3() {}

CovMatrix3x3::CovMatrix3x3(const CovMatrix3x3& mat)
{
  (*this) = mat;
}

int CovMatrix3x3::GetPCA(Vector3<COVMATRIX3X3_TYPE>& S, 
                         Matrix3x3<COVMATRIX3X3_TYPE>& VT)
{
  if (!_IsDecomposed){
    if (_Decompose()!=0)
      return -1;
  } else {
#ifdef BIAS_DEBUG
    if (memcmp( (void*)ConsistencyBackup_.GetData(),
                (void*)GetData(), 9*sizeof(COVMATRIX3X3_TYPE))) {
      BIASERR("Corrupted Data in CovMatrix. Maybe you forgot to invalidate."
              <<"Consistency backup="<<ConsistencyBackup_<<" Current Matrix="
              <<(*this));
      BIASABORT;
    }
#endif
  }
  S  = _S;
  VT = _VT;
 
  return 0;
}

int CovMatrix3x3::GetS(Vector3<COVMATRIX3X3_TYPE>& S)
{
  if (!_IsDecomposed){
    if (_Decompose()!=0)
      return -1;
  } else {
#ifdef BIAS_DEBUG
    if (memcmp( (void*)ConsistencyBackup_.GetData(),
                (void*)GetData(), 9*sizeof(COVMATRIX3X3_TYPE))) {
      BIASERR("Corrupted Data in CovMatrix. Maybe you forgot to invalidate.");
      BIASABORT;
    }
#endif
  }
  
  S = _S;
  
  return 0;
}

int CovMatrix3x3::_Decompose()
{
  if (_svd.Compute((Matrix<COVMATRIX3X3_TYPE>)(*this))!=0){
    return -1;
  }
  _S = Vector3<COVMATRIX3X3_TYPE>(_svd.GetS());
  _VT = Matrix3x3<COVMATRIX3X3_TYPE>(_svd.GetVT());
  _U = Matrix3x3<COVMATRIX3X3_TYPE>(_svd.GetU()); 
#ifdef BIAS_DEBUG
  ConsistencyBackup_        = (Matrix3x3<COVMATRIX3X3_TYPE>) (*this);
#endif
  _IsDecomposed = true;
 
  return 0;
}


CovMatrix3x3& CovMatrix3x3::operator=(const CovMatrix3x3& mat)
{
  Matrix3x3<COVMATRIX3X3_TYPE>::operator=(mat);
  _S  = mat._S;
  _U  = mat._U;
  _VT = mat._VT;
  
#ifdef BIAS_DEBUG
  ConsistencyBackup_        = mat.ConsistencyBackup_;
#endif
  _IsDecomposed = mat._IsDecomposed;

  //_svd=mat._svd;
  return (*this);
}


/** Computes the bounding box x=[xmin,xmax] , y=[ymin,ymax] in image  as 
    determined from the projected covariance matrix covmat. */
int CovMatrix3x3::GetSearchArea(PMatrixBase& P, HomgPoint3D& point3D, 
                                Vector2<double>& x, Vector2<double>& y, 
                                double scale)
{
  int result=0;
  
  Vector3<double> S(0,0,0), TT(0,0,0);
  Matrix3x3<double> VT;
  HomgPoint3D M;
  HomgPoint2D m, point2d;
  if (GetPCA(S,VT)!=0){
    result = -1;
  }
    
  x.Set(DBL_MAX,-DBL_MAX); y.Set(DBL_MAX,-DBL_MAX); 
  for (int i=0;i<3; i++)
    for (double sig=-1; sig <= 1.1; sig+=2){
      // compute point on uncertainty ellipsoid
      //      HomgPoint3D T(VT.GetRow(i)); T.Homogenize();
      TT.Set(VT.GetRow(i)); 
      TT *= (sig*scale*S[i]);
      point3D.Homogenize();
      TT[0] +=point3D[0];
      TT[1] +=point3D[1];
      TT[2] +=point3D[2];
      M.Set(TT[0], TT[1], TT[2]);
      // project into image
      m=P*M;      
      m.Homogenize();
      // update bounding box params
      if (m[0]<x[0]) x[0]=m[0];
      if (m[0]>x[1]) x[1]=m[0];
      if (m[1]<y[0]) y[0]=m[1];
      if (m[1]>y[1]) y[1]=m[1];
    }

  //// check if the projected point3D is in the box
  //// otherwise one or more 3DPoints of the ellipsoid are behind the camera
  point2d=P*point3D; 
  point2d.Homogenize();     
  if (x[1]<point2d[0]) {
    x[0]=x[1]; x[1]=DBL_MAX;
  } 
  if (x[0]>point2d[0]) {
    x[1]=x[0]; x[0]=-DBL_MAX;
  }

  if (y[1]<point2d[1]) {
    y[0]=y[1]; y[1]=DBL_MAX;
  } 
  if (y[0]>point2d[1]) {
    y[1]=y[0]; y[0]=-DBL_MAX;
  }

  return result;
}

/** Computes the bounding box x=[xmin,xmax] , y=[ymin,ymax] in image  as 
    determined from the projected covariance matrix covmat. 
    A min Search Area can be specified with minSearchArea(which is the
    radius of the uncertainty sphere).
    
*/
int CovMatrix3x3::GetSearchArea(PMatrixBase& P, HomgPoint3D& point3D, 
                                Vector2<double>& x, Vector2<double>& y, 
                                double scale, double minSearchArea)
{
  int result=0;
  
  Vector3<double> S, TT;
  Matrix3x3<double> VT;
  HomgPoint3D M;
  HomgPoint2D m, point2d;
  if (GetPCA(S,VT)!=0){
    result = -1;
  }
  point3D.Homogenize();
  x.Set(DBL_MAX,-DBL_MAX); y.Set(DBL_MAX,-DBL_MAX); 
  for (int i=0;i<3; i++)
    for (double sig=-1; sig <= 1.1; sig+=2){
      // compute point on uncertainty ellipsoid
      //      HomgPoint3D T(VT.GetRow(i)); T.Homogenize();
      TT.SetZero(); 
      TT[i]=1.0;
      // at first calculate minimum area with scaled unit sphere
      TT *= sig*minSearchArea;
      TT[0] +=point3D[0];
      TT[1] +=point3D[1];
      TT[2] +=point3D[2];
      M.Set(TT[0], TT[1], TT[2]);
      //// project into image
      m=P*M;      
      m.Homogenize();
      // update bounding box params
      if (m[0]<x[0]) x[0]=m[0];
      if (m[0]>x[1]) x[1]=m[0];
      if (m[1]<y[0]) y[0]=m[1];
      if (m[1]>y[1]) y[1]=m[1];

      //// second: project real cov Matrix
      TT.Set(VT.GetRow(i)); 
      TT *= (sig*scale*S[i]);     
      TT[0] +=point3D[0];
      TT[1] +=point3D[1];
      TT[2] +=point3D[2];
      M.Set(TT[0], TT[1], TT[2]);
      // project into image
      m=P*M;      
      m.Homogenize();
      // update bounding box params
      if (m[0]<x[0]) x[0]=m[0];
      if (m[0]>x[1]) x[1]=m[0];
      if (m[1]<y[0]) y[0]=m[1];
      if (m[1]>y[1]) y[1]=m[1];
    }

  //// check if the projected point3D is in the box
  //// otherwise one or more 3DPoints of the ellipsoid are behind the camera
  point2d=P*point3D; 
  point2d.Homogenize();     
  if (x[1]<point2d[0]) {
    x[0]=x[1]; x[1]=DBL_MAX;
  } 
  if (x[0]>point2d[0]) {
    x[1]=x[0]; x[0]=-DBL_MAX;
  }

  if (y[1]<point2d[1]) {
    y[0]=y[1]; y[1]=DBL_MAX;
  } 
  if (y[0]>point2d[1]) {
    y[1]=y[0]; y[0]=-DBL_MAX;
  }

  return result;
}