PMatrixTest2.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 PMatrixTest2.cpp
   @relates PMatrix
   @brief example investigating P-Matrix
   @ingroup g_examples
   @author MIP
*/

#include "../../Base/Math/Random.hh"
#include "../PMatrix.hh"
#include "../RMatrix.hh"
//#include "../KMatrix.hh"

using namespace BIAS;
using namespace std;

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

    BIAS::PMatrix P;
    BIAS::Random Randomizer;

    unsigned int numTests = 30;
    for (unsigned int i=0; i<numTests; i++) {

        Vector3<double> C(Randomizer.GetUniformDistributed(-200, 200),
                Randomizer.GetUniformDistributed(-200, 200),
                Randomizer.GetUniformDistributed(-200, 200));

        Vector3<double> RAxis(Randomizer.GetUniformDistributed(-1.0, 1.0),
                Randomizer.GetUniformDistributed(-1.0, 1.0),
                Randomizer.GetUniformDistributed(-1.0, 1.0));
        RAxis.Normalize();
        double rangle = Randomizer.GetUniformDistributed(-M_PI, M_PI);
        RMatrix R(RAxis, rangle);

        KMatrix K;
        K.SetIdentity();
        K[0][0] = Randomizer.GetUniformDistributed(100, 1000);
        K[1][1] = Randomizer.GetNormalDistributed(K[0][0], 0.1*K[0][0]);

        K.SetHx(Randomizer.GetUniformDistributed(100, 200));
        K.SetHy(Randomizer.GetUniformDistributed(100, 200));

        K.SetSkew(Randomizer.GetUniformDistributed(-K[0][0], K[0][0]));
        cout << "skew " << K.GetSkew() << endl;
//      K.SetSkew(0.0);
        
        // check if opt axes "meet" in front of cams
        if (C[0] * R[0][2] >0.0)
            C[0] *= -1.0;
        if (C[1] * R[1][2] >0.0)
            C[1] *= -1.0;
        C.Normalize();

        P.Compose(K, R, C);

        for(int x=0; x < 3; x++){
          for(int y = 0; y < 4; y++){
            P[x][y] += Randomizer.GetUniformDistributed(P[x][y]*0.1, P[x][y]*0.1);
          }
        }
        
        P.InvalidateDecomposition();

        KMatrix KEst;
        RMatrix REst;
        Vector3<double> CEst;

        KEst = P.GetK();
        REst = P.GetR();
        CEst = P.GetC();

        PMatrix PEst;
        PEst.Compose(KEst, REst, CEst);
        cout << "Test no " << i << " : " << ((PMatrix)(P - PEst)).IsZero(1e-12) << endl;

        PEst.InvalidateDecomposition();
        KEst = PEst.GetK();
        REst = PEst.GetR();
        CEst = PEst.GetC();
    
        cout << "K " << (KMatrix)(K - KEst) << endl;
        cout << "R " << (RMatrix)(R - REst) << endl;
        cout << "C " << C - CEst << endl;
        
    }

    return 0;
}