HomgPoint2D.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 "HomgPoint2D.hh"
#include <Base/Math/Matrix3x3.hh>

using namespace BIAS;
using namespace std;

ostream& operator<<(ostream& os, const enum HomgPoint2D::EPosition pos)
{
  switch(pos){
  case HomgPoint2D::TopLeft: os << "topleft"; break;
  case HomgPoint2D::Top: os << "top"; break;
  case HomgPoint2D::TopRight: os << "topright"; break;
  case HomgPoint2D::Left: os << "left"; break;
  case HomgPoint2D::Center: os << "center"; break;
  case HomgPoint2D::Right: os << "right"; break;
  case HomgPoint2D::BottomLeft: os << "bottomleft"; break;
  case HomgPoint2D::Bottom: os << "bottom"; break;
  case HomgPoint2D::BottomRight: os << "bottomright"; break;
  case HomgPoint2D::Infinity: os << "infinity"; break;
  case HomgPoint2D::InvalidPosition: os << "invalid"; break;
  }
  return os;
}

HomgPoint2D& HomgPoint2D::
Homogenize(Matrix3x3<HOMGPOINT2D_TYPE>& cov)
{
  register const HOMGPOINT2D_TYPE w = data_[2];
  Matrix3x3<HOMGPOINT2D_TYPE> J(MatrixZero);
  if (fabs(w)>HOMGPOINT2D_TYPE_EPS){
    /** the jacobian of the Homogenize() function is either
        | 1/w  0    -x/w^2 |
        | 0    1/w  -y/w^2 |
        | 0    0     0     | */ 
    J[0][0] = J[1][1] = 1.0/w;
    HOMGPOINT2D_TYPE w2 = w*w;
    J[0][2] = -data_[1]/w2;
    J[1][2] = -data_[2]/w2;
  } else {
    /** or | 1 0 0 |
           | 0 1 0 |
           | 0 0 0 | */
    J[0][0] = J[1][1] = 1.0;
  }
  cov = J * cov * J.Transpose();
  
  return Homogenize();
}