ExampleProjectionParametersPerspective2.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 ExampleProjectionParametersPerspective2.cpp
   @relates ProjectionParametersPerspective
   @brief small example demonstrating the ProjectionParametersPerspective
   by applying project and unproject
   @ingroup g_examples
   @author grauel 9/2007
*/

#include <Geometry/ProjectionParametersPerspective.hh>
#include <Base/Image/Image.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Math/Random.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Utils/ThreeDOut.hh>

using namespace BIAS;
using namespace std;


int main(int argc, char *argv[]) {
  ProjectionParametersPerspective ppp;

  // used .IOR - parameters
  // 1                     
  //-999        
  //-20.30377    cam const in mm (?)
  //-0.06444     princpx    normalized or mm (?)
  //-0.15281     princpy
  //-2.87698e-004  k1
  //6.73824e-007   k2
  // .85           r0
  //-6.23736e-010  
  // 1.97915e-005  k3
  //-2.11616e-005  k4
  //-1.38247e-004  ascpect
  // 8.89027e-006  skew
  // 23.60000      sensorsize in mm
  // 15.80000      sensorsize in mm
  // 3872          pixel x
  // 2592          pixel y 

  //  double CamConst = 20.30377; //in mm - perpendicular distance of projection center to image plane
  //  double SensorSizeX = 23.6;  //in mm - 
  //  double SensorSizeY = 15.8;  //in mm
  //
  //  unsigned int ImgWidth  = 3872;
  //  unsigned int ImgHeight = 2592;
  //
  //  double principalX = (double) ImgWidth*0.5  *  (1-0.06444/SensorSizeX);
  //  double principalY = (double) ImgHeight*0.5 *  (1-0.15281/SensorSizeY);
  //  double focalX = (double)ImgWidth*0.5*(CamConst/SensorSizeX );//in pixel
  //  double aspectrat = (1-1.38247e-004)*(double)ImgWidth/(double)ImgHeight;
  //  
  //  //ppp.SetDistortionType(DISTYPE_BROWN);  //not neede anymore
  //                                           //also set by calling
  //                                           //SetUndistortionBrown()
  //  double kc1 = -2.87698e-004;
  //  double kc2 = 6.73824e-007 ;
  //  double kc3 = 1.97915e-005;
  //  double kc4 = -2.11616e-005;
  //
  //  double r0  = .85;  // second constant root of polynomial
  //
  ////  ppp.SetUndistortionBrown(kc1,kc2,kc3,kc4,r0);
  //  ppp.SetFocalLengthAndAspect(focalX,aspectrat);
  //  ppp.SetPrincipal(principalX,principalY);    
  //  ppp.SetIdealImageSize(ImgWidth,ImgHeight);

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

  unsigned int numTests = 1;
  for (unsigned int tests=0; tests<numTests; tests++) {

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

    Vector3<double> C(1, 2, 0);

    //    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);
    RMatrix R(MatrixIdentity);
    R.SetXYZ(30.0*M_PI/180.0, 3.0*M_PI/180.0, 0.0);
    //    R.SetIdentity();

    //    Quaternion<double> rot;
    //    rot.SetValueAsAxisRad(RAxis, rangle);

    //    PoseParametrization poseParam;
    //    poseParam.SetCQ(C, rot);
    //    ppp.SetPoseParametrization(poseParam);

    double focalX = Randomizer.GetUniformDistributed(100, 1000);
    double aspectrat = Randomizer.GetNormalDistributed( 0.9, 1.1);

    double principalX = Randomizer.GetUniformDistributed(100, 200);
    double principalY = Randomizer.GetUniformDistributed(100, 200);

    double ImgWidth = 300;
    double ImgHeight = 300;

    KMatrix K(MatrixIdentity);
    K.SetFx(focalX);
    K.SetFy(focalX*aspectrat);
    K.SetHx(principalX);
    K.SetHy(principalY);
    K.SetSkew(Randomizer.GetUniformDistributed(-K[0][0], K[0][0]));
    //K.SetSkew(0.0);

    P.Compose(K, R, C);

    if (numTests == 1) {
      cout << "K " << K << endl;
      cout << "R " << R << endl;
      cout << "C " << C << endl;
      cout << "P " << P << endl;
    }
    
    P.InvalidateDecomposition();

    ppp.SetIdealImageSize(ImgWidth, ImgHeight);

    ppp.SetP(P);

    // generate some 3D points
    int numPoints = 20;
    vector<HomgPoint3D> points3D;
    vector<double> zCorr;
    HomgPoint3D temp3D;
    for (int i = 0; i < numPoints; i++) {
      temp3D = HomgPoint3D(Randomizer.GetUniformDistributed(-1, 1),
          Randomizer.GetUniformDistributed(-1, 1), Randomizer.GetUniformDistributed(1, 2));
      temp3D.Homogenize();
      //      cout << "num " << i << " : " << temp3D << endl;
      zCorr.push_back(temp3D[2]);
      points3D.push_back(temp3D);
    }

    // project 3D points
    vector<HomgPoint2D> points2D;
    HomgPoint2D temp2D;
    for (int i = 0; i < numPoints; i++) {
      temp2D = ppp.Project(points3D[i], true);
      temp2D.Homogenize();
      //      cout << "2D " << i << " : " << temp2D << endl;
      points2D.push_back(temp2D);
    }

    // unproject 2D points and compute sum of normed errors
    vector<HomgPoint3D> unprojectRay, backprojectZDepth, backprojectWorldCoo;

    vector<double> depth;
    double errorSum = 0.0;
    double errorSum2 = 0.0;
    HomgPoint3D ray;
    Vector3<double> pointinCamCorrs, pos, dir;
    HomgPoint2D withoutK;
    double zTemp, depthTemp;
    for (int j = 0; j < numPoints; j++) {

      // projection parameters perspective -> unprojectToRay 
      ppp.UnProjectToRay(points2D[j], pos, dir, true);
      ray = dir;
      if (ray[2] != 0.0) {
        ray[0] *= zCorr[j] / ray[2];
        ray[1] *= zCorr[j] / ray[2];
        ray[2] *= zCorr[j] / ray[2];
        ray.Homogenize();
        ray[0] += C[0];
        ray[1] += C[1];
        ray[2] += C[2];
        unprojectRay.push_back(ray);

        //      cout << "ray num " << j << " : " << ray << endl;
        errorSum += (ray - points3D[j]).NormL2();
      } else {
        cout << "ray[2] == 0 for point " << j << " " << ray << endl;
        unprojectRay.push_back(HomgPoint3D(0, 0, 0, 1));
      }

      // PMatrix backproject -> by zDepth
      PMatrix withoutK;
      KMatrix idK(MatrixIdentity);
      Vector3<double> camPointTemp;
      withoutK.Compose(idK, R, C);
      camPointTemp = withoutK * points3D[j];
      zTemp = camPointTemp[2];
      P.BackprojectByZDepth(points2D[j][0], points2D[j][1], zTemp, temp3D);
      temp3D.Homogenize();
      backprojectZDepth.push_back(temp3D);
      errorSum2 += (temp3D - points3D[j]).NormL2();
      
      // PMatrix backproject -> world coo
      temp2D = points2D[j];
      temp2D[0] -= principalX;
      temp2D[1] = principalY - temp2D[1];
      temp2D = K.Invert() * temp2D;
      depthTemp = sqrt(sqrt(camPointTemp[0]*camPointTemp[0] - temp2D[1]*temp2D[1]) - temp2D[0]*temp2D[0]);
      if(depthTemp!=depthTemp) depthTemp = 1.0;
      P.BackprojectWorldCoo(points2D[j], depthTemp, temp3D);
      if(temp3D[2] != 0){
        temp3D.Homogenize();
        backprojectWorldCoo.push_back(temp3D);
      } else {
        backprojectWorldCoo.push_back(HomgPoint3D(0,0,0,1));
        cout << "problem with temp3D[2] " << temp3D << endl;
      }
      cout << "point : " << j << backprojectWorldCoo[j] << endl;
    }

    cout << "error sum   " << errorSum << endl;
    cout << "error sum 2 " << errorSum2 << endl;

    if (numTests==1) {
      ThreeDOutParameters param3DO;
      param3DO.CameraStyle = PyramidMesh;
      param3DO.PointStyle = Box;
      param3DO.PointSize = 0.1;
      param3DO.LineStyle = Solid;
      param3DO.DrawUncertaintyEllipsoids = false;
      ThreeDOut threeDOut(param3DO);

      for (int i = 0; i < numPoints; i++) {
        points3D[i].Homogenize();
        threeDOut.AddPoint(points3D[i], RGBAuc(255, 0, 0, 127));

        unprojectRay[i].Homogenize();
        threeDOut.AddPoint(unprojectRay[i], RGBAuc(0, 255, 255, 127));

        backprojectZDepth[i].Homogenize();
        threeDOut.AddPoint(backprojectZDepth[i], RGBAuc(255, 0, 255, 127));
        
        backprojectWorldCoo[i].Homogenize();
        threeDOut.AddPoint(backprojectWorldCoo[i], RGBAuc(255, 255, 255, 127));
              
        
      }

      Vector3<double> start(0.0, 0.0, 0.0);
      Vector3<double> end(1.0, 0.0, 0.0);

      threeDOut.AddLine(start, end, RGBAuc(255, 0, 0, 127));
      end.Set(0.0, 1.0, 0.0);
      threeDOut.AddLine(start, end, RGBAuc(0, 255, 0, 127));
      end.Set(0.0, 0.0, 1.0);
      threeDOut.AddLine(start, end, RGBAuc(0, 0, 255, 127));
      threeDOut.VRMLOut("test.wrl");
      threeDOut.Dump();
    } // end if(numTest ==1)
  } // end for numTests

  return 0;
}