ExampleFMatrix.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
*/

/**
   @example ExampleFMatrix.cpp
   @relates FMatrix
   @brief Example computing F matrix with 8-point, non-lin optimize, and gold standard
   @ingroup g_examples
   @author MIP
*/

#include "Geometry/FMatrix.hh"
#include "Geometry/RMatrix.hh"
#include "Geometry/EMatrix.hh"
#include "Base/Geometry/KMatrix.hh"
#include "../FMatrixEstimation.hh"
#include "../PMatrixEstimation.hh"
#include "../PMatrixLinear.hh"
#include "../Triangulation.hh"
#include "../../Base/Math/Random.hh"
#include "Utils/ThreeDOut.hh"

using namespace BIAS;
using namespace std;

int main() {
  BIAS::FMatrix F;

  BIAS::PMatrix P1, P2, P1Comp, P2Comp;
  BIAS::Random Randomizer;

  BIAS::RMatrix R1, R2;
  BIAS::KMatrix K(MatrixIdentity);
  BIAS::Vector3<double> C1, C2;

  // set rotation matrices
  R1.SetIdentity();
  R2.SetXYZ(0, 10.0*M_PI/180.0, 0);

  // camera center coordiantes
  C1[0] = 0.0;
  C1[1] = 0.0;
  C1[2] = 0.0;

  //  C2[0] = -1.0/sqrt(2.0);
  //  C2[1] = 1.0/sqrt(2.0);
  C2[0] = -1.0;
  C2[1] = 0.0;
  C2[2] = 0.0;

  // camera matrix for both cameras
  K.SetFx(400.0);
  K.SetFy(400.0);
  // in image coordinates
  K.SetHx(100.0);
  K.SetHy(100.0);
  K.SetSkew(0.0);

  K[2][2] = 1;

  // combine camera matrix, rotation matrix and camera center to  projection matrices
  P1.Compose(K, R1, C1);
  P2.Compose(K, R2, C2);

  cout << "R1 " << R1 << endl;
  cout << "R2 " << R2 << endl;
  cout << "C1 " << C1 << endl;
  cout << "C2 " << C2 << endl;
  cout << "K " << K << endl;
  cout << "P1 " << P1 << endl;
  cout << "P2 " << P2 << endl;

  // compute randomized correspondences
  // later compute more and put them on planes, check angle of reconstruction
  vector<HomgPoint3D> points, pointsComp;
  vector<HomgPoint2D> p1, p2;

  cout << "Randomized correspondences are "<<endl;
  int numPoints = 20;
  double noise = 0.5;
  for (int p=0; p<numPoints; p++) {
    points.push_back(HomgPoint3D(
                       Randomizer.GetUniformDistributed(-1.0, 1.0),
                       Randomizer.GetUniformDistributed(-1.0, 1.0),
                       Randomizer.GetUniformDistributed(3.0, 5.0)));
    p1.push_back(P1 * points[p]);
    if(noise > 0){
      p1[p][0] += Randomizer.GetNormalDistributed(0, noise);
      p1[p][1] += Randomizer.GetNormalDistributed(0, noise);
    }
    p1[p].Homogenize();
    cout << "{ "<<p1[p]<<" ";
    p2.push_back(P2 * points[p]);
    if(noise > 0){
      p2[p][0] += Randomizer.GetNormalDistributed(0, noise);
      p2[p][1] += Randomizer.GetNormalDistributed(0, noise);
    }
    p2[p].Homogenize();
    cout << p2[p]<<" }  "<<endl;
  }

  // compute points on line with right angle - are not used for eight point algorithm
  int numPointsOnLines = 20;
  vector<HomgPoint3D> line1, line2, line1Comp, line2Comp;
  vector<HomgPoint2D> l1P1, l1P2, l2P1, l2P2;
  double randLambda = 0.0;
  for (int i = 0; i < numPointsOnLines; i++) {
    randLambda = Randomizer.GetUniformDistributed(-0.5, 3.0);
    line1.push_back(HomgPoint3D(-1.0 + randLambda, 1.0 + randLambda, 1.0
                + randLambda));
    l1P1.push_back(P1 * line1[i]);
    l1P1[i].Homogenize();
    l1P2.push_back(P2 * line1[i]);
    l1P2[i].Homogenize();
    randLambda = Randomizer.GetUniformDistributed(-1.0, 1.0);
    line2.push_back(HomgPoint3D(1.0 + randLambda, -1.0 + randLambda, 1.0
                + randLambda));
    l2P1.push_back(P1 * line2[i]);
    l2P1[i].Homogenize();
    l2P2.push_back(P2 * line2[i]);
    l2P2[i].Homogenize();

  }

  // Compute FMatrix from correspondences
  bool NormalizeHartley = true;
  FMatrixEstimation FEstimator(NormalizeHartley);
  FEstimator.EightPoint(F, p1, p2);
  FMatrix F_opt = F;
  FEstimator.Optimize(F_opt, p1, p2);

  // test putting F together using both Pmatrices
  FMatrix testCompF(P1, P2);
  F.Normalize();
  cout << "F " << F << endl;
  cout << "F residualError " << F.GetResidualError(p1, p2) << endl;
  F_opt.Normalize();
  cout << "F " << F_opt << endl;
  cout << "F residualError " << F_opt.GetResidualError(p1, p2) << endl;
  testCompF.Normalize();
  cout << "testCompF " << testCompF << endl;
  cout << "testCompF " << testCompF.GetResidualError(p1, p2) << endl;

  // some resetting
  P1Comp.SetIdentity();
  P1Comp.InvalidateDecomposition();
  P2Comp.SetIdentity();
  P2Comp.InvalidateDecomposition();

  // disturb KMatrix
  KMatrix KFalsch(K);
  KFalsch[0][0] *= (1.0 + Randomizer.GetUniformDistributed(-0.75, 1.0));
  KFalsch[1][1] = KFalsch[0][0];

  cout << "disturbed kmatrix " << KFalsch << endl;

  // compute essential matrix from fundamental matrix and disturbed k matrices
  EMatrix E;
  // initFromF enforces the rotation part of the Ematrix to be a rotation
  E.InitFromF(F, KFalsch, KFalsch);

  // check epipoles
  HomgPoint2D Epipole1, Epipole2;
  E.GetEpipoles(Epipole1, Epipole2);
  cout << "Epipole1 " << Epipole1.Homogenize() << " Epipole2 "
       << Epipole2.Homogenize() << endl;

  // Estimate PMatrix using PMatrixEstimation using essential matrix because P1 is assumed to be identity
  PMatrixEstimation pEstimator;
  double BaselineMagnitude = (C1-C2).NormL2();

  // use foerstner method for rotation estimation
  pEstimator.ComputeFromFDirect(E, BaselineMagnitude, P1Comp, P2Comp);
  // use ?pollefey's? method for rotation estimation
  // pEstimator.ComputeFromFQuasiEuklid(E, BaselineMagnitude,P1Comp,P2Comp);

  // turn baseline if necessary - using ground truth
  if (P2Comp.GetC().ScalarProduct(C2) < 0.0)
    pEstimator.ComputeFromFDirect(E, -BaselineMagnitude, P1Comp, P2Comp);
  //pEstimator.ComputeFromFQuasiEuklid(E, -BaselineMagnitude,P1Comp,P2Comp);

  // add assumed KMatrix to Projection matrices
  P1Comp = KFalsch * P1Comp;
  P1Comp.InvalidateDecomposition();
  P2Comp = KFalsch * P2Comp;
  P2Comp.InvalidateDecomposition();

  cout << "P2COMP IS " << P2Comp << endl;
  cout<< "Resulting C vectors " << P1Comp.GetC() <<" " << P2Comp.GetC()
      <<endl;

  // Check if estimated rotation matrix for P2 really is a/the correct rotation matrix
  RMatrix REst;
  double almostZero = 1e-10;

  REst = (RMatrix) P2Comp.GetR();
  double det = REst.GetDeterminant();
  Matrix3x3<double> id = REst * REst.Transpose();

  cout << "resulting rotation matrices " << P1Comp.GetR() << " " << REst
       << endl;
  cout << "Det of P2 " << det << endl;
  cout << id << " identity test: " << id.IsIdentity(1) << endl;

  double phiX, phiY, phiZ;
  REst.GetRotationAnglesXYZ(phiX, phiY, phiZ);
  cout << "Rotation angles " << phiX*180.0/M_PI << " " << phiY*180.0 /M_PI
       << " " << phiZ*180.0/M_PI << endl;

  // check for orthonormal columns
  bool isOrthonormal = true;
  double scalarProduct1 = REst.GetColumn(0).ScalarProduct(REst.GetColumn(1));
  double scalarProduct2 = REst.GetColumn(0).ScalarProduct(REst.GetColumn(2));
  double scalarProduct3 = REst.GetColumn(1).ScalarProduct(REst.GetColumn(2));

  if (fabs(scalarProduct1) >= almostZero)
    isOrthonormal = false;
  if (fabs(scalarProduct2) >= almostZero)
    isOrthonormal = false;
  if (fabs(scalarProduct3) >= almostZero)
    isOrthonormal = false;

  if (!id.IsIdentity(1.0))
    isOrthonormal = false;

  cout << "sp 1 " << scalarProduct1 << " sp 2 " << scalarProduct2 << " sp 3 "
       << scalarProduct3 << " orthonormal columns " << isOrthonormal
       << endl;

  cout << "p1 est " << P1Comp << endl;
  cout << "p2 est " << P2Comp << endl;

  // triangulate using the new projection matrices -
  // are for example lines still lines?
  Triangulation triangulator;
  Vector3<double> tempPoint;
  for (int i = 0; i < numPoints; i++) {
    cout << "point " << i << " triangulation "
         <<
      triangulator.Triangulate(P1Comp, P2Comp, p1[i], p2[i], tempPoint)
         << " ";
    pointsComp.push_back(HomgPoint3D(tempPoint));
  }
  cout << endl;

  for (int i = 0; i < numPointsOnLines; i++) {
    triangulator.Triangulate(P1Comp, P2Comp, l1P1[i], l1P2[i],
                             tempPoint);
    line1Comp.push_back(HomgPoint3D(tempPoint));
    triangulator.Triangulate(P1Comp, P2Comp, l2P1[i], l2P2[i],
                             tempPoint);
    line2Comp.push_back(HomgPoint3D(tempPoint));
  }

  ThreeDOutParameters param;
  param.CameraStyle = PyramidMesh;
  param.PointStyle = Box;
  param.PointSize = 0.1;
  param.LineStyle = Solid;
  param.DrawUncertaintyEllipsoids = false;
  ThreeDOut threeDOut(param);

  for (int i = 0; i < numPoints; i++) {
    threeDOut.AddPoint(points[i], RGBAuc(0, 0, 255, 127));
  }

  for (int i = 0; i < numPoints; i++) {
    threeDOut.AddPoint(pointsComp[i], RGBAuc(255, 0, 0, 127));
  }
  for (int i = 0; i < numPointsOnLines; i++) {
    threeDOut.AddPoint(line1[i], RGBAuc(0, 255, 255, 127));
    threeDOut.AddPoint(line2[i], RGBAuc(255, 255, 255, 127));
  }

  for (int i = 0; i < numPointsOnLines; i++) {
    threeDOut.AddPoint(line1Comp[i], RGBAuc(255, 255, 0, 127));
    threeDOut.AddPoint(line2Comp[i], RGBAuc(255, 0, 255, 127));
  }

  threeDOut.AddPMatrix(P1, 512, 512, RGBAuc_WHITE_OPAQUE, 0.1);
  threeDOut.AddPMatrix(P2, 512, 512, RGBAuc(255, 0, 0, 127), 0.1);
  threeDOut.AddPMatrix(P1Comp, 512, 512, RGBAuc(0, 255, 255, 127), 0.1);
  threeDOut.AddPMatrix(P2Comp, 512, 512, RGBAuc(255, 0, 255, 127), 0.1);
  Vector3<double> start(0.0, 0.0, 0.0);
  Vector3<double> end(1.0, 0.0, 0.0);
  threeDOut.AddLine(start, end, RGBAuc(255, 0, 0, 127));
  end.Set(0.0, 1.0, 0.0);
  threeDOut.AddLine(start, end, RGBAuc(0, 255, 0, 127));
  end.Set(0.0, 0.0, 1.0);
  threeDOut.AddLine(start, end, RGBAuc(0, 0, 255, 127));
  threeDOut.VRMLOut("test.wrl");
  threeDOut.Dump();

  return 0;
}