ProjectionError.cpp

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003, 2004    (see file CONTACTS 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 "ProjectionError.hh"
using namespace BIAS;
using namespace std;

BIAS::ProjectionError::ProjectionError()
    : cam(2)
{
}

void BIAS::ProjectionError::Init(int res, double baseline)
{
  this->baseline = baseline;
  
  cam[Left].SetSimplePerspective(0.87, res, res);
  cam[Right].SetSimplePerspective(0.87, res, res);

  cam[Left].SetC(Vector3<double>(0.0, 0.0, 0.0));
  cam[Right].SetC(Vector3<double>(baseline, 0.0, 0.0));
  
  RMatrix r;

  r.SetIdentity();
  r.SetXYZ(0.0, 0.0, 0.0);
  cam[Left].SetR(r);

  r.SetIdentity();
  r.SetXYZ(0.0, 0.0, 0.0);
  cam[Right].SetR(r);
}
void BIAS::ProjectionError::Init(int res, double depth, double angle)
{
  baseline = tan(angle) * depth;
  Init(res, baseline);
}

double BIAS::ProjectionError::ComputeDiff(double z, double offset)
{
  HomgPoint3D p(0.0, 0.0, z);
  HomgPoint2D prj;
  vector<Vector3<double> > ray(2);

  prj = cam[Right].Project(p);
  if (prj.IsAtInfinity())
    return -1.0;

  Vector3<double> pos;
  prj[0] += offset;
  cam[Right].UnProjectToRay(prj, pos, ray[Right]);
  ray[Left] = Vector3<double>(p[0], p[1], p[2]) - cam[Left].GetC();
  
  /*
    Schnittpunktberechnung:
      P ist der Schnittpunkt
      b ist der Baselinevektor
      P = t * ray[Left]
      P = b + (s * ray[Right])

      -->
      t * ray[Left] = b + (s * ray[Right])
  <=> b = t * ray[Left] - s * ray[Right]

    Aus ray[Left] und ray[Right] lässt sich eine 3x2 Matrix A erstellen.
    Es gilt:
      b = A * x
    Wobei x ein 2D Vektor mit x-Koordinate = t und y-Koordinate = s ist.

      (A^-1 * A) * x = A^-1 * b
      x = A^-1 * b

    Wenn man den Vektor x hat, kann man s und t verwenden.
  */
  Matrix<double> c(3, 2);
  for (int i = 0; i < 3; i++)
  {
    c[i][Left] = ray[Left][i];
    c[i][Right] = -ray[Right][i];
  }
  
  SVD svd(c);
  c = svd.Invert();
  Vector3<double> b(baseline, 0.0, 0.0);
  Vector2<double> result = c * b;

  double t = result[0];
  Vector3<double> prjp = ray[Left] * t;
  // t wird im Richtungsvektor eingesetzt.
  Vector3<double> vp = Vector3<double>(p[0], p[1], p[2]) + cam[Left].GetC();
  double diff = prjp.Length();// - vp.Length();
  
  return abs(diff);
}

const ProjectionParametersPerspective*
    BIAS::ProjectionError::GetCam(ELeftRight which)
{
  if (cam.size() > (unsigned int)which && which >= 0)
    return &cam[which];
  else
    return NULL;
}

double BIAS::ProjectionError::GetBaseline() const
{
  return baseline;
}