ExampleTriangulateOptimal.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 ExampleTriangulateOptimal.cpp
   @relates Triangulation
   @brief example demonstrating triangulation
   @ingroup g_examples
   @author woelk 08/2004 
*/

#include <Geometry/Triangulation.hh>
#include <Base/Math/Random.hh>
#include <Base/Debug/TimeMeasure.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Geometry/KMatrix.hh>
#include <Geometry/PMatrix.hh>

using namespace BIAS;
using namespace std;


int main(int argc, char *argv[])
{
  Triangulation tri;
  //  tri.AddDebugLevel(D_TR_OPT);
  Random ran;
  KMatrix K;
  RMatrixBase R0, R1;
  PMatrix P0, P1;
  Vector3<double> C0, C1, r0, r1;
  Vector3<double> eucl3d;
  HomgPoint3D p3d, tp3d;
  HomgPoint2D p0, p1, gtp0, gtp1, np0, np1;
  double sigma=2.5;
  int num=100, nump=10; // test num times
  bool read=false, finished=false;
  double osum=0.0, ksum=0.0, ssum=0.0, osum2=0.0, ksum2=0.0, ssum2=0.0;
  int onum=0, knum=0, snum=0;
    
  TimeMeasure ot, kt, st;

  if (argc==2) {
    read=true;
  }
  if (read){
    tri.AddDebugLevel("D_TR_OPT");
    num=1;
  }

  K.SetIdentity();
  K[0][0]=K[1][1]=840.0;
  K[0][2]=320.0;
  K[1][2]=240.0;

  for (int p=0; p<nump; p++){
    r0.Set(ran.GetNormalDistributed(0.0, 0.1), 
           ran.GetNormalDistributed(0.0, 0.1),
           ran.GetNormalDistributed(0.0, 0.1));
    r1.Set(ran.GetNormalDistributed(0.0, 0.5), 
           ran.GetNormalDistributed(0.0, 0.5),
           ran.GetNormalDistributed(0.0, 0.5));
  
    R0.SetXYZ(r0);
    R1.SetXYZ(r1);

    C0.Set(ran.GetNormalDistributed(0.0, 0.1), 
           ran.GetNormalDistributed(0.0, 0.1),
           ran.GetNormalDistributed(0.0, 0.1));
    C1.Set(ran.GetNormalDistributed(1.0, 0.1), 
           ran.GetNormalDistributed(0.0, 0.1),
           ran.GetNormalDistributed(0.0, 0.1));


    if (read){
      ifstream is(argv[1]);
      is >> K >> R0 >> R1 >> C0 >> C1 >> np0 >> np1 >> p3d >> gtp0 >> gtp1;
      is.close();
      cout << K << R0 << R1 << C0 << C1 << np0 << np1 << p3d<<gtp0<<gtp1<<endl;
    }

    P0.Compose(K, R0, C0);
    P1.Compose(K, R1, C1);
    P0.InvalidateDecomposition();
    P1.InvalidateDecomposition();

    for (int i=0; i<num; i++){
      p3d.Set(ran.GetUniformDistributed(-5.0, 5.0), 
              ran.GetUniformDistributed(-5.0, 5.0),
              ran.GetUniformDistributed(4.0, 10.0));
      gtp0=P0*p3d;
      gtp1=P1*p3d;

      gtp0.Homogenize();
      gtp1.Homogenize();

      if (num<5){
        cout << "\ntrue 3d point                                 : "<<p3d<<endl;
        cout << "true correspondences                  : "<<gtp0<<" <--> "<<gtp1<<endl;
      }

      if (read){
        p0=np0;
        p1=np1;
      } else {
        p0=gtp0;
        p1=gtp1;
        p0[0]+=ran.GetNormalDistributed(0.0, sigma);
        p0[1]+=ran.GetNormalDistributed(0.0, sigma);
        p1[0]+=ran.GetNormalDistributed(0.0, sigma);
        p1[1]+=ran.GetNormalDistributed(0.0, sigma);
        np0=p0;
        np1=p1;
      }

      if (num<5){
        cout << "noisy correspondences (sigma = "<<setw(5)<<sigma<<") : "<<p0<<" <--> "
             <<p1<<endl;
      }
      ot.Start();
      if (tri.Optimal(P0, P1, p0, p1, tp3d)==0){
        ot.Stop();
        if (num<5){
          if (tri.DebugLevelIsSet("D_TR_OPT")){
            cout << "true 3d point                                 : "<<p3d<<endl;
            cout << "true correspondences                  : "<<gtp0<<" <--> "<<gtp1<<endl;
            cout << "noisy correspondences (sigma = "<<setw(5)<<sigma<<") : "<<np0<<" <--> "
                 <<np1<<endl;
          }
          cout << "corrected correspondences             : "<<p0<<" <--> "<<p1<<endl;
          p0=P0*tp3d; p0.Homogenize();
          p1=P1*tp3d; p1.Homogenize();
          cout << "projected triangulated point          : "<<p0<<" <--> "<<p1<<endl;
          cout << "optimal triangulated 3D point                 : "<<tp3d<<endl;
        
        
        }
        cout << "optimal euclidean dist. in 3D : "<<(tp3d-p3d).NormL2()
             <<"\teucl. dist in images: "<<(np0-p0).NormL1()<<"  "
             <<(np1-p1).NormL1()<<endl;
        osum+=(tp3d-p3d).NormL2();
        osum2+=(tp3d-p3d).NormL2()*(tp3d-p3d).NormL2();
        onum++;
        if ((tp3d-p3d).NormL2()>1.0){
          cout << K << R0 << R1 << C0 << C1 << np0 << np1<<p3d<<gtp0<<gtp1<<endl;
          ofstream of("deb.txt");
          of << K << R0 << R1 << C0 << C1 << np0 << np1 << p3d<<gtp0<<gtp1<<endl;
          of.close();
          //         finished=true;
          //         /*
          //           ifstream is("deb.txt");
          //           is >> K >> R0 >> R1 >> C0 >> C1 >> np0 >> np1 >> p3d >> gtp0 >> gtp1;
          //           is.close();
          //           cout << K << R0 << R1 << C0 << C1 << np0<<np1<<p3d<<gtp0<<gtp1<<endl;
          //         */
        }
      } else {
        ot.Stop();
        cout << "optimal triangulation failed\n";
      }

      p0=np0;
      p1=np1;
      kt.Start();
      if (tri.Triangulate(P0, P1, p0, p1, eucl3d)>=0){
        tp3d.Set(eucl3d);
        kt.Stop();
        if (num<5){
          cout << "kanatani triangulated 3D point                : "<<tp3d<<endl;
        }
        cout << "kanatani euclidean dist. in 3D: "<<(tp3d-p3d).NormL2()<<endl;
        ksum+=(tp3d-p3d).NormL2();
        ksum2+=(tp3d-p3d).NormL2()*(tp3d-p3d).NormL2();
        knum++;
      } else {
        tp3d.Set(eucl3d);
        kt.Stop();
        cout << "kanatani triangulation failed\n";
      }
   
      p0=np0;
      p1=np1;
      double dist, ang;
      st.Start();
      if (tri.Triangulate2D(P0, P1, p0, p1, tp3d, dist, ang)==0){
        st.Stop();
        if (num<5){
          cout << "2D triangulated 3D point                      : "<<tp3d<<endl;
        }
        cout << "2D euclidean dist. in 3D      : "<<(tp3d-p3d).NormL2()<<"\tdist: "
             <<dist<<"\tang: "<<ang<<endl;
        ssum+=(tp3d-p3d).NormL2();
        ssum2+=(tp3d-p3d).NormL2()*(tp3d-p3d).NormL2();
        snum++;
      } else {
        st.Stop();
        cout << "2D triangulation failed\n";
      }
      if (finished) return -1;
    
    }
  }
  if (onum>1 && knum>1 && snum>1){
    cout << "optimal calculated "<<onum<<" times with mean error "
         <<osum/(double)onum<<" +- "
         << (osum2-(osum*osum)/(double)onum)/(double)(onum-1.0) 
         << "\t"<< ot.GetRealTime()/(double)onum<<" us "<<endl;
    cout << "kanatani calculated "<<knum<<" times with mean error "
         <<ksum/(double)knum<<" +- "
         << (ksum2-(ksum*ksum)/(double)knum)/(double)(knum-1.0) 
         << "\t"<< kt.GetRealTime()/(double)knum<<" us "<<endl;
    cout << "2D  calculated "<<snum<<" times with mean error "
         <<ssum/(double)snum<<" +- "
         << (ssum2-(ssum*ssum)/(double)snum)/(double)(snum-1.0) 
         <<"\t"<< st.GetRealTime()/(double)snum<<" us "<<endl;
  }
  

  return 0;
}