HomgPoint2DCov.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 "HomgPoint2DCov.hh"
#include "HomgPoint2D.hh"
//#include <Base/Math/Matrix2x2.hh>
#include <Base/Debug/Exception.hh>

using namespace BIAS;
using namespace std;

HomgPoint2DCov::
HomgPoint2DCov() : Matrix3x3<double> ()
{}

HomgPoint2DCov::
HomgPoint2DCov(const MatrixInitType& i) : Matrix3x3<double>(i) {}

HomgPoint2DCov::
HomgPoint2DCov(const Matrix3x3<double>& m) : Matrix3x3<double>(m) 
{
  if (!CheckSymmetry()){
    /*cout << setprecision(20) << (*this)[0][1] <<" == "<< (*this)[1][0] << boolalpha << Equal((*this)[0][1], (*this)[1][0])<<"\t"<<((*this)[0][1]==(*this)[1][0])<<endl;
    cout << setprecision(20) << (*this)[0][2] <<" == "<< (*this)[2][0] << boolalpha << Equal((*this)[0][2], (*this)[2][0])<<endl; 
    cout << setprecision(20) << (*this)[1][2] <<" == "<< (*this)[2][1] << boolalpha << Equal((*this)[1][2], (*this)[2][1])<<endl; */
    //BIASERR("Not a covariance matrix, forcing symmetry! " << *this);
    MakeSymmetric();
    //BEXCEPTION("Not a covariance matrix, symmetry check failed: " << *this);
  }
}

HomgPoint2DCov::
HomgPoint2DCov(const Matrix2x2<double>& m)
  : Matrix3x3<double>() 
{
  SetEuclidean(m);
}

HomgPoint2DCov::
HomgPoint2DCov(const Matrix<double>& m)
  : Matrix3x3<double>(m) 
{
  if (!CheckSymmetry()){
    BEXCEPTION("not a covariance matrix "<<*this);
  }
}

HomgPoint2DCov::
HomgPoint2DCov(const TNT::Matrix<double>& m)
  : Matrix3x3<double>(m) 
{
  if (!CheckSymmetry()){
    BEXCEPTION("not a covariance matrix "<<*this);
  }
}

HomgPoint2DCov::
~HomgPoint2DCov()
{}


void HomgPoint2DCov::
Homogenize(HomgPoint2D& p)
{
  if (!p.IsAtInfinity()){
    // The jacobian of homogenization is given by:
    // 1/w  0    -x/w^2 
    // 0    1/w  -y/w^2
    // 0    0    0
    Matrix3x3<double> J(MatrixZero);
    J[0][0] = J[1][1] = 1.0/p[2];
    J[0][2] = -p[0]/p[2]/p[2];
    J[1][2] = -p[1]/p[2]/p[2];
    // The covariance transforms as J * cov * J^T
    *this = HomgPoint2DCov(J * *this * J.Transpose());
    p.Homogenize();
  } else {
    cout << __FILE__<<":"<<__LINE__<<" p " << p << " at infinity, doing nothing." << endl;
  }
}


void HomgPoint2DCov::
Normalize(HomgPoint2D& p)
{
  const double norm = p.NormL2();
  // The jacobian of normalization is given by:
  // J = 1/|p|(I - pp^T/p^Tp)
  // Foerstner 2004, "Uncertainty and Projective Geometry"
  Matrix3x3<double> J(MatrixIdentity);
  Matrix3x3<double> tmp = p.OuterProduct(p);
  tmp /= p.ScalarProduct(p);
  J -= tmp;
  J /= norm;
  // The covariance transforms as J * cov * J^T
  *this = HomgPoint2DCov(J * *this * J.Transpose());
  p /= norm;
}


Matrix2x2<double> HomgPoint2DCov::
GetEuclidean() const 
{
  if (!IsHomogenized()){
    BEXCEPTION("Euclidean cov matrices can only be returned when homogenized: " << *this);
  }
  Matrix2x2<double> res;
  res[0][0] = (*this)[0][0];
  res[0][1] = (*this)[0][1];
  res[1][0] = (*this)[1][0];
  res[1][1] = (*this)[1][1];
  return res;
}


void HomgPoint2DCov::
SetEuclidean(const Matrix2x2<double>& m)
{
  SetZero();
  (*this)[0][0] = m[0][0] ;
  (*this)[0][1] = m[0][1] ;
  (*this)[1][0] = m[1][0] ;
  (*this)[1][1] = m[1][1] ;
  if (!CheckSymmetry()){
    BEXCEPTION("Not a covariance matrix, symmetry check failed: "<< *this);
  }
}


bool HomgPoint2DCov::
CheckSymmetry() const
{ 
  const double eps = 1e-10;
  return ( Equal((*this)[0][1], (*this)[1][0], eps) && 
           Equal((*this)[0][2], (*this)[2][0], eps) && 
           Equal((*this)[1][2], (*this)[2][1], eps) ); 
}

void HomgPoint2DCov::MakeSymmetric() { 
  (*this)[1][0] = (*this)[0][1] = ((*this)[0][1] + (*this)[1][0])/2.0;
  (*this)[2][0] = (*this)[0][2] = ((*this)[0][2] + (*this)[2][0])/2.0;
  (*this)[2][1] = (*this)[1][2] = ((*this)[1][2] + (*this)[2][1])/2.0;
}

void HomgPoint2DCov::SetAndMakeSymmetric(const Matrix3x3<double> &m)
{
  (*this)[0][0] = m[0][0];
  (*this)[1][1] = m[1][1];
  (*this)[2][2] = m[2][2];
  (*this)[1][0] = (*this)[0][1] = (m[0][1] + m[1][0])/2.0;
  (*this)[2][0] = (*this)[0][2] = (m[0][2] + m[2][0])/2.0;
  (*this)[2][1] = (*this)[1][2] = (m[1][2] + m[2][1])/2.0;
}