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

#include <Base/Geometry/KMatrix.hh> 
#include <Base/Geometry/RMatrixBase.hh> 
#include <Base/Geometry/HomgPoint2D.hh> 
#include <Base/Geometry/HomgPoint3D.hh> 
#include <Base/Math/Matrix4x4.hh> 
#include <Base/Debug/Debug.hh> 

using namespace BIAS;
using namespace std;


int Normalization::
Hartley(const vector<HomgPoint3D>& Points, vector<HomgPoint3D>& NormPoints,
        Matrix4x4<double> & normMatrix) const 
{
  normMatrix.SetIdentity();
    const unsigned int uiPointCount(Points.size());
    // we need at least two points for a stddev
    if (uiPointCount<2) return -1;
        
  int result = 0;
  
  NormPoints.resize(uiPointCount);

  // compute mean 
  double meanX(0), meanY(0), meanZ(0);
  for (unsigned int i=0; i<uiPointCount; i++) {
    meanX += Points[i][0];
    meanY += Points[i][1];
    meanZ += Points[i][2];
#ifdef BIAS_DEBUG
    if (Points[i][3]!=1.0) {
      BIASERR("Supplied non-homogenized point ("<<i<<") ! "<<Points[i]);
      BIASABORT;
    }
#endif
  }
  meanX /= (double)uiPointCount;
  meanY /= (double)uiPointCount;
  meanZ /= (double)uiPointCount;

  // compute mean distance from center
  double meandist(0);
  for (unsigned int i=0; i<uiPointCount; i++) {
    const BIAS::HomgPoint3D& CurPoint = Points[i];
    meandist += sqrt((CurPoint[0]-meanX) * (CurPoint[0]-meanX) +
                     (CurPoint[1]-meanY) * (CurPoint[1]-meanY) +
                     (CurPoint[2]-meanZ) * (CurPoint[2]-meanZ));
  }
  meandist /= (double)uiPointCount;

  if (meandist>0.0) {
    // compute Normalization such that mean is zero and mean distance is sqrt2
    normMatrix[0][0] = M_SQRT2/meandist;
    normMatrix[0][3] = -meanX/meandist;
    normMatrix[1][1] = M_SQRT2/meandist;
    normMatrix[1][3] = -meanY/meandist;

    normMatrix[2][2] = M_SQRT2/meandist;
    normMatrix[2][3] = -meanZ/meandist;
  } else {
    // stddev is zero, all the same point !!!
    result = -2;
  }
    HomgPoint3D p;
  // now normalize all the points
  for (unsigned int i=0; i<uiPointCount; i++) {
        normMatrix.Mult(Points[i], p);
        NormPoints[i]=p;
    }
    return result;                                   
}


int Normalization::
Hartley(const vector<HomgPoint2D>& Points, vector<HomgPoint2D>& NormPoints,
        KMatrix& Normalization) const 
{
  Normalization.SetIdentity();
    const unsigned int uiPointCount(Points.size());
    // we need at least two points for a stddev
    if (uiPointCount<2) return -1;
        
  int result = 0;
  
  NormPoints.reserve(uiPointCount);
#ifdef BIAS_DEBUG
  if (NormPoints.size()>0) {
    BIASERR("non-empty vector supplied !");
    BIASABORT;
  }
#endif
  // compute mean 
  double meanX(0), meanY(0);
  for (unsigned int i=0; i<uiPointCount; i++) {
    meanX += Points[i][0];
    meanY += Points[i][1];
#ifdef BIAS_DEBUG
    if (Points[i][2]!=1.0) {
      BIASERR("Supplied non-homogenized point ("<<i<<") ! "<<Points[i]);
      BIASABORT;
    }
#endif
  }
  meanX /= (double)uiPointCount;
  meanY /= (double)uiPointCount;
  
  // compute mean distance from center
  double meandist(0);
  for (unsigned int i=0; i<uiPointCount; i++) {
    const BIAS::HomgPoint2D& CurPoint = Points[i];
    meandist += sqrt((CurPoint[0]-meanX) * (CurPoint[0]-meanX)  +
                     (CurPoint[1]-meanY) * (CurPoint[1]-meanY));
  }
  meandist /= (double)uiPointCount;

  if (meandist>0.0) {
    // compute Normalization such that mean is zero and mean distance is sqrt2
    Normalization[0][0] = M_SQRT2/meandist;
    Normalization[0][2] = -meanX/meandist;
    Normalization[1][1] = M_SQRT2/meandist;
    Normalization[1][2] = -meanY/meandist;
  } else {
    // stddev is zero, all the same point !!!
    result = -2;
  }
    HomgPoint2D p;
  // now normalize all the points
  for (unsigned int i=0; i<uiPointCount; i++) {
        Normalization.Mult(Points[i], p);
        NormPoints.push_back(p.Homogenize());
    }
    return result;                                   
}

int Normalization::
Woelk(const vector<HomgPoint2D>& Points,
      vector<HomgPoint2D>& NormPoints,
      Matrix3x3<double>& Normalization) const 
{
    const unsigned int uiPointCount(Points.size());
    // we need at least two points for a stddev
    if (uiPointCount<2) return -1;
        
  // compute mean 
  Vector3<double> mean(0.0);
  for (unsigned int i=0; i<uiPointCount; i++) {
    BIASASSERT(Equal(Points[i].NormL2(), 1.0));
    mean += Points[i];                                  
  }
  mean /= (double)uiPointCount;
  
  // compute rotation such that mean vector is (0, 0, 1)
  BIASASSERT(!Equal(mean.NormL2(), 0.0));
  Vector3<double> nmean = mean / mean.NormL2();
  const Vector3<double> z(0.0, 0.0, 1.0);
  Vector3<double> axis = nmean.CrossProduct(z);
  double angle = axis.NormL2();
  // rounding error guards
  if (angle>=1.0) angle = 1.0;
  if (angle<=-1.0) angle = -1.0;
  if (Equal(angle, 0.0)){
    axis.Set(0.0, 0.0, 1.0);
  } else {
    axis /= angle;
  }
  angle = asin(angle);
  RMatrixBase R;
  R.Set(axis, angle);

  //cout << "R * mean = "<<R*nmean<<endl;

  // compute mean angle to (0,0,1)
  Vector3<double> diff;
  double ang;
  double meanang = 0.0;
  double max_ang = 1.0;
  for (unsigned int i=0; i<uiPointCount; i++) {
    diff = R * Points[i];
    ang = diff.ScalarProduct(z);
    meanang += ang;
    max_ang = min(ang, max_ang);
  }
  meanang /= (double)uiPointCount;
  if (max_ang>=1.0) max_ang = 1.0;
  if (max_ang<=-1.0) max_ang = -1.0;
  max_ang = acos(max_ang);
  //INFNANCKECK(max_ang);
  //cout << "max angle: "<<max_ang/M_PI*180.0<<endl;
  if (meanang>=1.0) meanang = 1.0;
  if (meanang<=-1.0) meanang = -1.0;
  meanang = acos(meanang);
  //INFNANCKECK(meanang);
  //cout << "mean angle: "<<meanang/M_PI*180.0<<endl;

  // the maximum allowed angle
  const double ang_thresh = 1.4; // 80 deg
  // the angular threshold represents a tradeoff between the 
  // accuracy of the rotation matrix and the accuracy of the
  // epipole, when using the normalization for essential matrix
  // estimation. (heuristically investigated)
  // bigger angular threshold result in better estimation of the epipole
  const double des_ang = 1.0; // 60 deg
  //const double des_ang = .785375; // 45 deg
  //const double des_ang = 1.3962; // 80 deg
  Matrix3x3<double> K(MatrixIdentity);
  // compute normalization such that the mean angle is des_ang
  // and the maximum angle does not exceed ang_thresh
  if (max_ang<ang_thresh && meanang<des_ang){
    double des_scale = tan(des_ang) / tan(meanang);
    //cout << "desired scale: "<<des_scale<<endl;
    double max_scale = tan(ang_thresh) / tan(max_ang);
    //cout << "max scale: "<<max_scale<<endl;
    double scale = min(des_scale, max_scale);
    //cout << "scale: "<<scale<<endl;
    for (int i=0; i<2; i++){
      K[i][i] = scale;
    }
    K[2][2] = 1.0;
  }
  Normalization = K * R;

  // now normalize all the points
  NormPoints.resize(uiPointCount);
  max_ang = 1.0;
  for (unsigned int i=0; i<uiPointCount; i++) {
    Normalization.Mult(Points[i], NormPoints[i]);
    //BIASASSERT(!Equal(NormPoints[i].NormL2(), 0.0));
    //NormPoints[i] /= NormPoints[i].NormL2();
    //max_ang = min(max_ang, NormPoints[i].ScalarProduct(z));
    }
  //cout << "max angle after scaling: "<<acos(max_ang)*180.0/M_PI<<endl;
    return 0;                                    
}
#undef M_SQRT3