Basic Image AlgorithmS Library 2.8.0

ExampleFMatrix2.cpp

00001 /*
00002  This file is part of the BIAS library (Basic ImageAlgorithmS).
00003 
00004  Copyright (C) 2003-2009    (see file CONTACT for details)
00005  Multimediale Systeme der Informationsverarbeitung
00006  Institut fuer Informatik
00007  Christian-Albrechts-Universitaet Kiel
00008 
00009 
00010  BIAS is free software; you can redistribute it and/or modify
00011  it under the terms of the GNU Lesser General Public License as published by
00012  the Free Software Foundation; either version 2.1 of the License, or
00013  (at your option) any later version.
00014 
00015  BIAS is distributed in the hope that it will be useful,
00016  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  GNU Lesser General Public License for more details.
00019 
00020  You should have received a copy of the GNU Lesser General Public License
00021  along with BIAS; if not, write to the Free Software
00022  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00023  */
00024 
00025 /**
00026    @example ExampleFMatrix2
00027    @relates FMatrix
00028    @brief Example computing F matrix with 8-point, non-linear optimize, and gold standard
00029    @ingroup g_examples
00030    @author MIP
00031 */
00032 
00033 #include "../FMatrix.hh"
00034 #include "../FMatrixEstimation.hh"
00035 #include "../PMatrixEstimation.hh"
00036 #include "../PMatrixLinear.hh"
00037 #include "../Triangulation.hh"
00038 #include "../../Base/Math/Random.hh"
00039 
00040 using namespace BIAS;
00041 using namespace std;
00042 
00043 // ---------------------------------------------------------------------------
00044 // testing eight point and Gold Standard algorithm for F matrix estimation
00045 
00046 int main() {
00047   BIAS::FMatrix FEightPoint, FOptimize, FGoldSt, TheoF;
00048 
00049   BIAS::PMatrix P1, P2;
00050   BIAS::Random Randomizer;
00051 
00052   BIAS::RMatrix R1, R2;
00053   BIAS::KMatrix K(MatrixIdentity);
00054   BIAS::Vector3<double> C1, C2;
00055 
00056   // set rotation matrices
00057   R1.SetIdentity();
00058   //R2.SetXYZ(20.0*M_PI/180.0, 10.0*M_PI/180.0, -60*M_PI/180.0);
00059   R2.SetXYZ(0.0, 5.0*M_PI/180.0, 0.0);
00060   // camera center coordiantes
00061   C1[0] = 0.0;
00062   C1[1] = 0.0;
00063   C1[2] = 0.0;
00064 
00065   C2[0] = -1.0/sqrt(2.0);
00066   C2[1] = 1.0/sqrt(2.0);
00067   //  C2[0] = -1.0;
00068   //  C2[1] =  3.0;
00069   C2[2] = 0.0;
00070 
00071   //  // camera matrix for both cameras
00072   K.SetFx(400.0);
00073   K.SetFy(400.0);
00074   // in image coordinates
00075   K.SetHx(100.0);
00076   K.SetHy(100.0);
00077   K.SetSkew(0.0);
00078 
00079   K[2][2] = 1;
00080 
00081   // combine camera matrix, rotation matrix and camera center to  projection matrices
00082   P1.Compose(K, R1, C1);
00083   P2.Compose(K, R2, C2);
00084 
00085   P1.InvalidateDecomposition();
00086   P2.InvalidateDecomposition();
00087 
00088   // compute true F matrix
00089   TheoF.ComputeFromPMatrices(P1, P2);
00090 
00091   vector<HomgPoint3D> points;
00092   vector<HomgPoint2D> p1, p2;
00093 
00094   // noise is added to point correspondences
00095   vector<HomgPoint2D> p1Noisy, p2Noisy;
00096   double noiseB = 10;
00097   double testDist1, testDist2, testDist3, testDist4;
00098 
00099   unsigned int numPoints = 80;
00100   cout << "Randomized correspondences are "<<endl;
00101   for (unsigned int p=0; p<numPoints; p++) {
00102     cout << "number " << p << endl;
00103     points.push_back(HomgPoint3D(Randomizer.GetUniformDistributed(-1.0, 1.0),
00104         Randomizer.GetUniformDistributed(-1.0, 1.0), Randomizer.GetUniformDistributed(-1.0, 1.0)));
00105     p1.push_back(P1 * points[p]);
00106     p1[p].Homogenize();
00107     cout << "{ "<<p1[p]<<" ";
00108     p2.push_back(P2 * points[p]);
00109     p2[p].Homogenize();
00110     cout << p2[p]<<" }  "<<endl;
00111 
00112 //    if (p < 8) {
00113       testDist1 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
00114       testDist2 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
00115       testDist3 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
00116       testDist4 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
00117       p1Noisy.push_back(HomgPoint2D((p1[p])[0] + testDist1, (p1[p])[1] + testDist2));
00118       p2Noisy.push_back(HomgPoint2D((p2[p])[0] + testDist3, (p2[p])[1] + testDist4));
00119 //    } else {
00120 //      p1Noisy.push_back(p1[p]);
00121 //      p2Noisy.push_back(p2[p]);
00122 //    }
00123     cout << "{ " << p1Noisy[p] << " " << p2Noisy[p] << " }" << endl;
00124   }
00125 
00126   bool NormalizeHartley = true;
00127   FMatrixEstimation FEstimator(NormalizeHartley);
00128 
00129   // vars for epipole computation
00130   HomgPoint2D e8Point1, e8Point2, theoe1, theoe2, eGoldSt1, eGoldSt2, eOpt1, eOpt2;
00131   double residualError8Point, residualErrorGoldStandard, residualErrorTest;
00132   double residualError8Point_n, residualErrorGoldStandard_n, residualErrorTest_n;
00133   double residualErrorOpt_n, residualErrorOpt;
00134 
00135   residualErrorTest_n = TheoF.GetResidualError(p1Noisy, p2Noisy);
00136   residualErrorTest = TheoF.GetResidualError(p1, p2);
00137 
00138   // use eight point algorithm and the optimize function applying the
00139   // levenberg marquardt algorithm to optimize the parameters of the Fmatrix
00140   // using the sum of squared distances to the epipolar line as an error function
00141   FEightPoint.SetIdentity();
00142   FEstimator.Linear(FEightPoint, p1Noisy, p2Noisy);
00143   residualError8Point_n = FEightPoint.GetResidualError(p1Noisy, p2Noisy);
00144   residualError8Point = FEightPoint.GetResidualError(p1, p2);
00145   FEightPoint.GetEpipolesHomogenized(e8Point1, e8Point2);
00146   e8Point1.Normalize();
00147   e8Point2.Normalize();
00148 
00149   // use optimize on 8Point result
00150   FOptimize = FEightPoint;
00151   FEstimator.Optimize(FOptimize, p1Noisy, p2Noisy);
00152   residualErrorOpt_n = FOptimize.GetResidualError(p1Noisy, p2Noisy);
00153   residualErrorOpt = FOptimize.GetResidualError(p1, p2);
00154   FOptimize.GetEpipolesHomogenized(eOpt1, eOpt2);
00155   if(!eOpt1.IsZero()) eOpt1.Normalize();
00156   if(!eOpt2.IsZero())eOpt2.Normalize();
00157 
00158   // use Gold Standard algorithm
00159   FGoldSt.SetIdentity();
00160   FEstimator.GoldStandard(FGoldSt, p1Noisy, p2Noisy);
00161   residualErrorGoldStandard_n = FGoldSt.GetResidualError(p1Noisy, p2Noisy);
00162   residualErrorGoldStandard = FGoldSt.GetResidualError(p1, p2);
00163   FGoldSt.GetEpipolesHomogenized(eGoldSt1, eGoldSt2);
00164   if(!eGoldSt1.IsZero()) eGoldSt1.Normalize();
00165   if(!eGoldSt2.IsZero()) eGoldSt2.Normalize();
00166 
00167   // cout results
00168   cout << "True F matrix " << 1/TheoF[2][2] * TheoF << endl;
00169   cout << "Residual Error " << residualErrorTest  << " noisy " << residualErrorTest_n << endl;
00170   TheoF.GetEpipolesHomogenized(theoe1, theoe2);
00171   theoe1.Normalize();
00172   theoe2.Normalize();
00173   cout << "True epipoles are "<<theoe1<<" and "<<theoe2<<endl;
00174   cout << endl;
00175 
00176   cout << "Eight point algorithm " << endl;
00177   cout << "Fmatrix " << 1/FEightPoint[2][2] * FEightPoint << endl;
00178   cout << "Residual Error " << residualError8Point << " noisy " << residualError8Point_n  << endl;
00179   cout << "est. epipoles are "<<e8Point1<<" and "<<e8Point2<<endl;
00180 
00181   cout << endl;
00182 
00183   cout << "Optimize algorithm " << endl;
00184   cout << "Fmatrix " << 1/FOptimize[2][2] * FOptimize << endl;
00185   cout << "Residual Error " << residualErrorOpt << " noisy " << residualErrorOpt_n  << endl;
00186   cout << "est. epipoles are "<<eOpt1<<" and "<<eOpt2<<endl;
00187 
00188   cout << endl;
00189 
00190 
00191   cout << "Gold Standard algorithm" << endl;
00192   cout << "Fmatrix " << 1/FGoldSt[2][2] * FGoldSt << endl;
00193   cout << "Residual Error " << residualErrorGoldStandard << " noisy " << residualErrorGoldStandard_n << endl;
00194   cout << "est. epipoles are "<<eGoldSt1<<" and "<<eGoldSt2<<endl;
00195 
00196   return 0;
00197 }
 All Classes Functions Variables Typedefs Enumerations Enumerator Friends