ExampleTriangulate.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 ExampleTriangulate.cpp
   @relates Triangulation, CovMatrix3x3
   @brief example demonstrating triangulation
   @ingroup g_examples
   @author ischiller 10/2009 
*/


#include <Geometry/Triangulation.hh>
#include <Geometry/RMatrix.hh>
#include <Geometry/PMatrix.hh>
#include <Base/Geometry/KMatrix.hh>
#include "Geometry/CovMatrix3x3.hh"
#include <fstream>
#include <iostream>

using namespace BIAS;
using namespace std;

//int main(int argc, char *argv[]) 
int main() 
{
  PMatrix pleft,pleftScale,pleftScaleTriang,pright,prightScale,prightScaleTriang;
  ifstream infileL("left.matrix");
  ifstream infileR("right.matrix");

  Vector3<double> eucl3d;
  HomgPoint2D pl, pr;
  HomgPoint3D p3, p3s, p32, p32s;
  CovMatrix3x3 cov;
  Triangulation triangulator;

  //  triangulator.SetDebugLevel(D_TR_POS|D_TR_TR);

  RMatrix rleft, rright, Rscale;
  KMatrix K;
  Vector3<double> Cleft,CleftScaleShift, Cright, CrightScaleShift;
  rleft.SetXYZ(0.0, 0.0, 0.0);  
  rright.SetXYZ(0.0, 0.0, 0.0); Rscale.SetXYZ(0.4, 0.0, 0.0);
  K.SetIdentity();
  K[0][0]=K[1][1]=480;
  K[0][2]=640/2.0;
  K[1][2]=480/2.0;
  Cleft.Set(0.0, 0.0, 0.0);
  Cright.Set(2.5, 0.0, -5.0);
  pleft.Compose(K, rleft, Cleft);
  pright.Compose(K, rright, Cright);
  
  double scale = 10.0;
  Vector3<double> shift(1.0, 1.0, 1.0);
  CleftScaleShift=scale*Cleft; 
  CleftScaleShift +=shift; 
  CrightScaleShift=scale*Cright;
  CrightScaleShift+=shift;
  pleftScale.Compose(K, Rscale, CleftScaleShift);
  pleftScaleTriang.Compose(K,Rscale.Transpose()*Rscale,Rscale.Transpose()*(CleftScaleShift-shift)/scale);
  prightScale.Compose(K, Rscale, CrightScaleShift);
  prightScaleTriang.Compose(K,Rscale.Transpose()*Rscale,Rscale.Transpose()*(CrightScaleShift-shift)/scale);

  pleft.InvalidateDecomposition();
  pleftScale.InvalidateDecomposition();
  pright.InvalidateDecomposition();
  if (infileL) infileL>>pleft;
  if (infileR) infileR>>pright;
  cout<<"Read:"<<endl<<pleft<< endl <<pleftScale<<endl<<pright<<endl;
  srand(0);
  for (int i = 0; i <=5; i++){ 
    // 3D point
    BIAS::HomgPoint3D point3d;
    for (int k=0; k<3; k++)
      // point coordinate between 0 and 1000
      point3d[k]=10.0*rand()/(RAND_MAX+1.0)-5;
    point3d[2]+=6;
    point3d[3]=1;
    // project the point3d to get its image
    BIAS::HomgPoint2D point2d_im1(pleft*point3d),
      point2d_im1s(pleftScale*point3d);
    BIAS::HomgPoint2D point2d_im2(pright*point3d),
      point2d_im2s(prightScale*point3d);
    point2d_im1.Homogenize();point2d_im1s.Homogenize();
    point2d_im2.Homogenize();point2d_im2s.Homogenize();
    // add up to 1 pixel  noise 
    //point2d_im1 = (rand()/(RAND_MAX+1.0)*2.0-1.0) + point2d_im1;
    //point2d_im2 = (rand()/(RAND_MAX+1.0)*2.0-1.0) + point2d_im2;
    //point2d_im1[2]=1.0;  point2d_im2[2]=1.0;
    //cerr << "point left: "<< point2d_im1 << "\tpoint right: "<< point2d_im2
    //     << endl;
    if (triangulator.Triangulate(pleft, pright,  point2d_im1,  point2d_im2, 
                                 eucl3d)!=0){
      cerr << "error triangulating using SVD"<<endl;
    }
    p3.Set(eucl3d);
    cout << "p3 =" << p3;
    triangulator.Triangulate(pleftScaleTriang, prightScaleTriang,  
                             point2d_im1s, point2d_im2s, eucl3d);
    p3s.Set(eucl3d);
    Vector3<double> vp3s(p3s[0]/p3s[3], (p3s[1]/p3s[3]), p3s[2]/p3s[3]);
    vp3s = Rscale*vp3s*scale+shift;
    cout << "p3 with scaled, rot baseline= " <<   p3s <<  ", vp3s = " 
         << vp3s << endl;
    cout << "error is [" << (vp3s[0]-p3[0])/p3[0] << ", " << (vp3s[1]-p3[1])/p3[1] 
         << ", " <<(vp3s[2]-p3[2])/p3[2] << "] " << endl;
    /* 
       if (triangulator.Triangulate2D(pleft, pright,  point2d_im1,  point2d_im2, 
       p32, dist, angle)!=0){ 
       cerr << "error triangulating using GetNormRayWorldCoo"<<endl;
       }
       cout << "p32 =" << p32;
                                   
       triangulator.Triangulate2D(pleftScale, prightScale,  point2d_im1,  point2d_im2, 
       p32s, dist, angle);
       cout << "p32 with scaled rot baseline= " <<   p32s << endl;
       p32[1]=p32s[1]-10;
       p32s =  Vector3<double>(Rscale.Transpose()*p32s);
       cout << "scale is [" << p32s[0]/p32[0] << ", " << p32s[1]/p32[1] << ", " 
       << p32s[2]/p32[2] << "] " << endl; 
       cerr <<"SVD      point3d: "<<point3d<<" \tp3:"<<p3<<"\tdifference: "
       <<(point3d-p3).norm_L2()<<endl;;
       cerr <<"NormRay  point3d: "<<point3d<<" \tp3:"<<p32<<"\tdifference: "
       <<(point3d-p32).norm_L2()<<endl<<endl;
       if (triangulator.GetCovarianceAnalytic(pleft, pright,  
       point2d_im1,  point2d_im2,
       p3, cov, 1)!=0) {
       cerr << "error getCovariance"<<endl;
       }
       cerr<<"CovMatrix:<<"<<cov<<endl;
    */
  }

  pl.Set(196, 100);
  pr.Set(155, 101);
 
  cout<<"Now triangulation of:"<<pl<<" "<<pr<<endl;

  cout<<"result of Triangluation: "<<
    triangulator.Triangulate(pleft, pright, pl, pr, eucl3d)<<endl;
  p3.Set(eucl3d);
  cout<<"p3: "<<p3<<endl;
  //  cout<<"Covariance Matrix: "<<endl<<cov<<endl;
  //  cout<<"distance of rays: "<<dist<<endl;
  
  Vector3<double> a1,a2, b1,b2;
  a1[0]= 0; a2[0]= 0; 
  a1[1]= 0; a2[1]= 0;
  a1[2]= 0; a2[2]= 1;

  b1[0]= 0; b2[0]= 1; 
  b1[1]= 1; b2[1]= 1;
  b1[2]= 0; b2[2]= 0;

  cout<<"Interscection of: a and b:"<<
    triangulator.Intersect(a1,a2,b1,b2)<<endl;

  cout<<"Interscection of: b and a:"<<
    triangulator.Intersect(b1,b2,a1,a2)<<endl;
  return 0;
}