ExampleCamPoseCalib.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 ExampleCamPoseCalib.cpp
   @relates CamPoseCalib
   @brief Example for estimating camera pose with CamPoseCalib
   @ingroup g_examples
   @author MIP
*/

#include "Geometry/CamPoseCalib.hh"
#include "Base/Math/Random.hh"

using namespace BIAS;
using namespace std;

int nrPoints=100;
int percentOutlier=0;
double boxSize=10, boxDepth=1, boxDistance=30; // boxSize in xy-direction
double sigmaNoise=0.5, outlier=5; // in pixels !

//int main(int argc, char *argv[]) 
int main(int, char **) 
{
  BIAS::Random ra;
  // create random 3D points
  std::vector<Vector3<double> > mps;
  Vector3<double> mp(3);
  for (int i=0;i<nrPoints;i++) {
    mp[0]=ra.GetUniformDistributed(-boxSize/2.0,boxSize/2.0);
    mp[1]=ra.GetUniformDistributed(-boxSize/2.0,boxSize/2.0);
    mp[2]=ra.GetUniformDistributed(boxDistance-boxDepth/2.0,
                                   boxDistance+boxDepth/2.0);    
    mps.push_back(mp);
  }
  //  mps[0]= Vector3<double>(1,0,18);
  //   mps[1]= Vector3<double>(-1,1,20);
  //   mps[2]= Vector3<double>(1,1,22);
  //   mps[3]= Vector3<double>(0,0,20);
  
  double fx=912/2.0,fy=912/2.0,cx=383/2.0,cy=287/2.0;
  cx=343/2.0,cy=247/2.0;
  Vector3<double> C;
  
  CamPoseCalib estimator;
  // estimator.SetDebugLevel( estimator.GetDebugLevel() | 
  //                            D_BIAS_CAMPOSE_ESTIMATE_STEPS);
  //   estimator.SetDebugLevel( estimator.GetDebugLevel() | 
  //                           D_BIAS_CAMPOSE_ESTIMATE_EQSYS);
  //  estimator.SetDebugLevel( estimator.GetDebugLevel() | 
  //                     D_BIAS_CAMPOSE_APPLY_SOL);
  estimator.SetDebugLevel( estimator.GetDebugLevel() | 
                           D_BIAS_CAMPOSE_ESTIMATE_ITER);
  //estimator.AddDebugLevel( D_BIAS_CAMPOSE_LM );
  // estimator.SetDebugLevel( estimator.GetDebugLevel() | 
  //                       D_BIAS_CAMPOSE_EQSYS);
  //  estimator.AddDebugLevel(D_BIAS_CAMPOSE_SECD);
  /////////////////////////////////////
  cout<<endl<<endl<<
    "---- estimating external parameters --------"<<endl;

  /// set the initial projection matrix
  KMatrix K;
  K.SetIdentity();
  K[0][0]= fx;               K[0][2]= cx;
  K[1][0]=   0; K[1][1]= fy; K[1][2]= cy;  // virtual image size of 320x240
  RMatrix R,resR;
  R.SetXYZ(0,0,0);
  C.Set(0,0,0);
  PMatrix P1;
  P1.Compose(K,R,C);
  estimator.SetInitialCamera(P1);
  //  estimator.SetInitialCamera(K);

  // now change it a little and project the model points
  K[0][0]= fx ;          K[0][2]= cx;
  K[1][1]= fy;            K[1][2]= cy;
  R.SetXYZ(0, -45.0/180.0*M_PI, 0/180.0*M_PI);
  C.Set(100,0,0);
  PMatrix P;
  P.Compose(K,R,C);
  
  // only estimatiion of the external params and the focal length
  //  estimator.SetEstimateFocal(2);

  std::vector<Vector2<double> > tps;
  Vector2<double> y;
  //project the model points
  for (unsigned int i=0;i<mps.size();i++) {
    HomgPoint2D(P*HomgPoint3D(mps[i])).GetEuclidean(y);
    tps.push_back(y);
  }

  // add some "noise"
  if (sigmaNoise>1E-15)
    for (unsigned int i=0;i<tps.size();i++) {
      tps[i][0]+=ra.GetNormalDistributed(0,sigmaNoise);
      tps[i][1]+=ra.GetNormalDistributed(0,sigmaNoise);
    }
 
  // and a few "outlier"
  if  (percentOutlier>0.000001) 
    for (unsigned int i=0;i<tps.size()/(percentOutlier*100);) {
      double offset=ra.GetNormalDistributed(0,outlier);
      if (fabs(offset)>(3*sigmaNoise)) {
        tps[i][0]+=offset;
        i++;
      }
      offset=ra.GetNormalDistributed(0,outlier);
      if (fabs(offset)>(3*sigmaNoise)) {
        tps[i][1]+=offset;
        i++;
      }
    }

  //  /// test covars
  //   std::vector<BIAS::Matrix2x2<double> > covars;
  //   covars.push_back(Matrix2x2<double>( 1,0, 0,1 ));
  //   covars.push_back(Matrix2x2<double>( 1,0, 0,1 ));
  //   covars.push_back(Matrix2x2<double>( 1,0, 0,1 ));
  //   covars.push_back(Matrix2x2<double>( 1001,999, 999,1001 )); //diagonal covar
  //   covars[3]= covars[3].Invert();

  //   tps[3][0]+=9;  tps[3][1]+=9; // should have no effect, cause of diag covar

  for (unsigned int i=0;i<mps.size();i++) {
    estimator.AddCorr(mps[i],tps[i]);
  }

 
  //   estimator.SetVariances(covars);
  estimator.Estimate(50);

  ///////// output the result
  resR=estimator.GetRotation();
  cout<<"estimated R:"<<resR<<endl;
  BIAS::Vector3<double> axis;
  double angle;
  resR.GetRotationAxisAngle(axis,angle);
  cout<<"Rotation axis:"<<axis<<" angle:"<<angle/M_PI*180.0<<endl;
  cout<<"estimated trans:"<<estimator.GetTranslation()<<endl;
  PMatrix Pest;
  estimator.GetP(Pest);
  Vector2<double> yEst;
  HomgPoint2D(Pest*HomgPoint3D(mps[4])).GetEuclidean(yEst);
  cout<<"avg error:"<<estimator.GetAvgError()<<endl<<
    " y5:"<<tps[4]<<" estimatedP*mp5:"<<yEst<<endl;
  cout<<"Real K:"<<K<<endl
      <<"Estimated K:"<<Pest.GetK()<<endl;
  Matrix<double> coVar;
  estimator.GetCoVarMatrix(coVar);
  cout<<"CoVariance of Estimation:"<<coVar<<endl;
  


   cout<<endl<<endl<<
      "---- Reestimating with principal point and Huber weighting function ----"
        <<endl;    
    // now all 10 parameters are estimated, starting point is the 
    // last estimated P
      estimator.SetEstimatePrincipal(1);
    estimator.SetWeighting(1.0,1);
    estimator.Estimate(50);

    resR=estimator.GetRotation();
    cout<<"estimated R:"<<resR<<endl;
    resR.GetRotationAxisAngle(axis,angle);
    cout<<"Rotation axis:"<<axis<<" angle:"<<angle/M_PI*180.0<<endl;
    cout<<"estimated trans:"<<estimator.GetTranslation()<<endl;
    estimator.GetP(Pest);
    HomgPoint2D(Pest*HomgPoint3D(mps[4])).GetEuclidean(yEst);
    cout<<"avg error:"<<estimator.GetAvgError()<<endl<<
      " y5:"<<tps[4]<<" estimatedP*mp5:"<<yEst<<endl;
    cout<<"Real K:"<<K<<endl
        <<"Estimated K:"<<Pest.GetK()<<endl;
    estimator.GetCoVarMatrix(coVar);
    cout<<"CoVariance of Estimation:"<<coVar<<endl;


  
   //  // for profiling
//     for (unsigned int r=0;r<200;r++) {
//       estimator.Reset();
    
//       for (unsigned int i=0;i<mps.size();i++) {
//         estimator.AddCorr(mps[i],tps[i]);
//       }
//       estimator.SetWeighting(1.0,-1);
//       estimator.Estimate(50);
//     }
}