HomgPoint3DCov.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 "HomgPoint3DCov.hh"
#include <Base/Geometry/HomgPoint3D.hh>
#include <Base/Math/Matrix3x3.hh>
#include <Base/Debug/Exception.hh>

using namespace BIAS;
using namespace std;

HomgPoint3DCov::
HomgPoint3DCov()
  : Matrix4x4<double>()
{}

HomgPoint3DCov::
HomgPoint3DCov(const MatrixInitType& i)
  : Matrix4x4<double>(i) {}

HomgPoint3DCov::
HomgPoint3DCov(const Matrix3x3<double>& m)
  : Matrix4x4<double>() 
{
  SetEuclidean(m);
}

HomgPoint3DCov::
HomgPoint3DCov(const Matrix4x4<double>& m)
  : Matrix4x4<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; */
    //BEXCEPTION("not a covariance matrix "<<*this);
    // BIASERR("not a covariance matrix, forcing symmetry!! "<<*this);
    MakeSymmetric_();
  }
}

HomgPoint3DCov::
HomgPoint3DCov(const Matrix<double>& m)
  : Matrix4x4<double>(m) 
{
  if (!CheckSymmetry_()){
    //BIASERR("not a covariance matrix, forcing symmetry!! "<<*this);
    MakeSymmetric_();
  }
}

HomgPoint3DCov::
HomgPoint3DCov(const TNT::Matrix<double>& m)
  : Matrix4x4<double>(m) 
{
  if (!CheckSymmetry_()) {
    //BIASERR("Not a covariance matrix, forcing symmetry!! " << *this);
    //BIASABORT;
    MakeSymmetric_();
  }
}

HomgPoint3DCov::
~HomgPoint3DCov()
{}

void HomgPoint3DCov::
Homogenize(HomgPoint3D& p)
{
  if (!p.IsAtInfinity()){
    // the jacobian of the Homogenize() function
    // 1/w  0    0   -x/w^2 
    // 0    1/w  0   -y/w^2
    // 0    0    1/w -z/w^2
    // 0    0    0   0
    Matrix4x4<double> J(MatrixZero);
    J[0][0] = J[1][1] = J[2][2] = 1.0/p[3];
    J[0][3] = -p[0]/p[3]/p[3];
    J[1][3] = -p[1]/p[3]/p[3];
    J[2][3] = -p[2]/p[3]/p[3];
    // the covariance transforms as J * cov * J^T
    *this = J * *this * J.Transpose();
    p.Homogenize();
  } else {
    cout << __FILE__<<":"<<__LINE__<<" p "<<p<<" at infinity, doing nothing\n";
  }
}


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


void HomgPoint3DCov::
SetEuclidean(const Matrix3x3<double>& m)
{
  SetZero();
  (*this)[0][0] = m[0][0];
  (*this)[0][1] = m[0][1];
  (*this)[0][2] = m[0][2];
  (*this)[1][0] = m[1][0];
  (*this)[1][1] = m[1][1];
  (*this)[1][2] = m[1][2];
  (*this)[2][0] = m[2][0];
  (*this)[2][1] = m[2][1];
  (*this)[2][2] = m[2][2];
  if (!CheckSymmetry_()){
    // BIASERR("not a covariance matrix, forcing symmetry!! "<<*this);
    MakeSymmetric_();
  }
}