InvestigatePRelations.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 InvestigatePRelations.cpp
   @relates PMatrixLinear
   @brief example investigating PR (?)
   @ingroup g_examples
   @author MIP
*/


#include <Base/Common/BIASpragma.hh>

#include <Base/Geometry/HomgPoint2D.hh>
#include <Geometry/PMatrix.hh>
#include <Geometry/RMatrix.hh>
#include <Base/Geometry/KMatrix.hh>
#include <Base/Math/Random.hh>
#include <Utils/ThreeDOut.hh>
#include <vector>
#include <Geometry/PMatrixLinear.hh>

#include <MathAlgo/SVD.hh>

using namespace BIAS;
using namespace std;


//int main(int argc, char *argv[]) 
int main() 
{

  unsigned int numPs = 50;
  
  vector<PMatrix> P_origs;
  vector<PMatrix> P_transformed;
  PMatrix P;
  
  KMatrix K;
  K.SetIdentity();
  K[0][0] = 400;
  K[1][1] = 400;
  K[0][2] = 200;
  K[1][2] = 200;
  
  Vector3<double> C;
  double phiX, phiY, phiZ;
  RMatrix R;

  Random randomizer;

  // build transformation matrix T
  Matrix4x4<double> T;
  C[0] = randomizer.GetUniformDistributed(-10.0,10.0);
  C[1] = randomizer.GetUniformDistributed(-10.0,10.0);
  C[2] = randomizer.GetUniformDistributed(-10.0,10.0);
  phiX = randomizer.GetUniformDistributed(-30.0,30.0) * M_PI/180.0;
  phiY = randomizer.GetUniformDistributed(-30.0,30.0) * M_PI/180.0;
  phiZ = randomizer.GetUniformDistributed(-30.0,30.0) * M_PI/180.0;
  R.SetXYZ(phiX, phiY, phiZ);
  for(int i=0; i < 3; i++){
    for(int j=0; j < 3; j++){
      T[i][j] = R[i][j];
    }
    T[i][3] = C[i]; 
  }
  T[3][0] = randomizer.GetUniformDistributed(-5.0, 5.0);
  T[3][1] = randomizer.GetUniformDistributed(-5.0, 5.0);
  T[3][2] = randomizer.GetUniformDistributed(-5.0, 5.0);
  T[3][3] = randomizer.GetUniformDistributed(-5.0, 5.0);
  
  cout << "T " << T << endl;
  
  // build Ps and transform them
  for(unsigned int i = 0; i < numPs; i++){
    C[0] = randomizer.GetUniformDistributed(-10.0,10.0);
    C[1] = randomizer.GetUniformDistributed(-10.0,10.0);
    C[2] = randomizer.GetUniformDistributed(-10.0,10.0);
    phiX = randomizer.GetUniformDistributed(-30.0,30.0) * M_PI/180.0;
    phiY = randomizer.GetUniformDistributed(-30.0,30.0) * M_PI/180.0;
    phiZ = randomizer.GetUniformDistributed(-30.0,30.0) * M_PI/180.0;
    R.SetXYZ(phiX, phiY, phiZ);
//    cout << "orig angles " << phiX << " " << phiY << " " <<  phiZ << endl;
    P.Compose(K, R, C);
    P_origs.push_back(P);
    P_transformed.push_back(PMatrix(P*T));
    R = RMatrix(P_transformed[i].GetR());
    
    // put noise on transformed matrices
    (P_transformed[i])[0][0] = (P_transformed[i])[0][0] + randomizer.GetUniformDistributed(-2.0,2.0); // fx
    (P_transformed[i])[0][1] = (P_transformed[i])[0][1] + randomizer.GetUniformDistributed(-0.2,0.2); // s
    (P_transformed[i])[0][2] = (P_transformed[i])[0][2] + randomizer.GetUniformDistributed(-2.0,2.0); // ux
    (P_transformed[i])[1][1] = (P_transformed[i])[1][1] + randomizer.GetUniformDistributed(-2.0,2.0); // fy
    (P_transformed[i])[1][2] = (P_transformed[i])[1][2] + randomizer.GetUniformDistributed(-2.0,2.0); // uy 
   }

  
  // least squares with one equation system per T column
  Matrix<double> A(numPs*3,4);
  A.SetIdentity();
  Vector<double> b(numPs*3);
  Matrix4x4<double> T_leastSquares;
  for(int col = 0; col < 4; col++){
    for(unsigned int camCount = 0; camCount < numPs; camCount++){
      for(int i=0; i < 3; i++){
        for(int j = 0; j < 4; j++){
          A[i+camCount*3][j] = (P_origs[camCount])[i][j];
        }
        b[i+camCount*3] = (P_transformed[camCount])[i][col];
      }
    }
//    cout <<"A " << A << endl;
//    cout <<"b " << b << endl;
    SVD svdA(A);
    Matrix<double> A_inv = svdA.Invert();
    Vector<double> T_col = A_inv * b;
    cout << T_col << endl;
    T_leastSquares[0][col] = T_col[0];
    T_leastSquares[1][col] = T_col[1];
    T_leastSquares[2][col] = T_col[2];
    T_leastSquares[3][col] = T_col[3];
  }
  cout << "Least squares T " << T_leastSquares << endl;
  
  SVD svdT(T_leastSquares);
  Matrix4x4<double> T_invLS = svdT.Invert();
  
  vector<PMatrix> P_leastSquares;
  PMatrix P_leastSquare;
  for(unsigned int i=0; i < numPs; i++){
    P_leastSquare = PMatrix(P_transformed[i] * T_invLS);
    P_leastSquare.InvalidateDecomposition();
    P_leastSquares.push_back(P_leastSquare);
  }
  
  ThreeDOutParameters param3DO;
  param3DO.CameraStyle = PyramidMesh;
  param3DO.PointStyle = Box;
  param3DO.PointSize = 0.1;
  param3DO.LineStyle = Solid;
  param3DO.DrawUncertaintyEllipsoids = false;
  ThreeDOut threeDOut(param3DO);

  for(unsigned int i = 0; i < numPs; i++){
    threeDOut.AddPMatrix(P_origs[i], 400, 400, RGBAuc(255, 0, 0, 127), 0.1);
//    P_transformed[i].InvalidateDecomposition();
//    threeDOut.AddPMatrix(P_transformed[i], 400, 400, RGBAuc(0, 255, 0, 127), 0.1);
    threeDOut.AddPMatrix(P_leastSquares[i], 400, 400, RGBAuc(0, 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;
}