Basic Image AlgorithmS Library 2.8.0

ExampleProjectionParametersPerspective2.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 /**
00027    @example ExampleProjectionParametersPerspective2.cpp
00028    @relates ProjectionParametersPerspective
00029    @brief small example demonstrating the ProjectionParametersPerspective
00030    by applying project and unproject
00031    @ingroup g_examples
00032    @author grauel 9/2007
00033 */
00034 
00035 #include <Geometry/ProjectionParametersPerspective.hh>
00036 #include <Base/Image/Image.hh>
00037 #include <Base/Image/ImageIO.hh>
00038 #include <Base/Math/Random.hh>
00039 #include <Base/Geometry/HomgPoint2D.hh>
00040 #include <Utils/ThreeDOut.hh>
00041 
00042 using namespace BIAS;
00043 using namespace std;
00044 
00045 
00046 int main(int argc, char *argv[]) {
00047   ProjectionParametersPerspective ppp;
00048 
00049   // used .IOR - parameters
00050   // 1                     
00051   //-999        
00052   //-20.30377    cam const in mm (?)
00053   //-0.06444     princpx    normalized or mm (?)
00054   //-0.15281     princpy
00055   //-2.87698e-004  k1
00056   //6.73824e-007   k2
00057   // .85           r0
00058   //-6.23736e-010  
00059   // 1.97915e-005  k3
00060   //-2.11616e-005  k4
00061   //-1.38247e-004  ascpect
00062   // 8.89027e-006  skew
00063   // 23.60000      sensorsize in mm
00064   // 15.80000      sensorsize in mm
00065   // 3872          pixel x
00066   // 2592          pixel y 
00067 
00068   //  double CamConst = 20.30377; //in mm - perpendicular distance of projection center to image plane
00069   //  double SensorSizeX = 23.6;  //in mm - 
00070   //  double SensorSizeY = 15.8;  //in mm
00071   //
00072   //  unsigned int ImgWidth  = 3872;
00073   //  unsigned int ImgHeight = 2592;
00074   //
00075   //  double principalX = (double) ImgWidth*0.5  *  (1-0.06444/SensorSizeX);
00076   //  double principalY = (double) ImgHeight*0.5 *  (1-0.15281/SensorSizeY);
00077   //  double focalX = (double)ImgWidth*0.5*(CamConst/SensorSizeX );//in pixel
00078   //  double aspectrat = (1-1.38247e-004)*(double)ImgWidth/(double)ImgHeight;
00079   //  
00080   //  //ppp.SetDistortionType(DISTYPE_BROWN);  //not neede anymore
00081   //                                           //also set by calling
00082   //                                           //SetUndistortionBrown()
00083   //  double kc1 = -2.87698e-004;
00084   //  double kc2 = 6.73824e-007 ;
00085   //  double kc3 = 1.97915e-005;
00086   //  double kc4 = -2.11616e-005;
00087   //
00088   //  double r0  = .85;  // second constant root of polynomial
00089   //
00090   ////  ppp.SetUndistortionBrown(kc1,kc2,kc3,kc4,r0);
00091   //  ppp.SetFocalLengthAndAspect(focalX,aspectrat);
00092   //  ppp.SetPrincipal(principalX,principalY);    
00093   //  ppp.SetIdealImageSize(ImgWidth,ImgHeight);
00094 
00095   BIAS::PMatrix P;
00096   BIAS::Random Randomizer;
00097 
00098   unsigned int numTests = 1;
00099   for (unsigned int tests=0; tests<numTests; tests++) {
00100 
00101     //    Vector3<double> C(Randomizer.GetUniformDistributed(-2, 2), Randomizer.GetUniformDistributed(
00102     //        -2, 2), Randomizer.GetUniformDistributed(-2, 2));
00103 
00104     Vector3<double> C(1, 2, 0);
00105 
00106     //    Vector3<double> RAxis(Randomizer.GetUniformDistributed(-1.0, 1.0),
00107     //        Randomizer.GetUniformDistributed(-1.0, 1.0), Randomizer.GetUniformDistributed(-1.0, 1.0));
00108     //    RAxis.Normalize();
00109     //    double rangle = Randomizer.GetUniformDistributed(-M_PI, M_PI);
00110 
00111     //    RMatrix R(RAxis, rangle);
00112     RMatrix R(MatrixIdentity);
00113     R.SetXYZ(30.0*M_PI/180.0, 3.0*M_PI/180.0, 0.0);
00114     //    R.SetIdentity();
00115 
00116     //    Quaternion<double> rot;
00117     //    rot.SetValueAsAxisRad(RAxis, rangle);
00118 
00119     //    PoseParametrization poseParam;
00120     //    poseParam.SetCQ(C, rot);
00121     //    ppp.SetPoseParametrization(poseParam);
00122 
00123     double focalX = Randomizer.GetUniformDistributed(100, 1000);
00124     double aspectrat = Randomizer.GetNormalDistributed( 0.9, 1.1);
00125 
00126     double principalX = Randomizer.GetUniformDistributed(100, 200);
00127     double principalY = Randomizer.GetUniformDistributed(100, 200);
00128 
00129     double ImgWidth = 300;
00130     double ImgHeight = 300;
00131 
00132     KMatrix K(MatrixIdentity);
00133     K.SetFx(focalX);
00134     K.SetFy(focalX*aspectrat);
00135     K.SetHx(principalX);
00136     K.SetHy(principalY);
00137     K.SetSkew(Randomizer.GetUniformDistributed(-K[0][0], K[0][0]));
00138     //K.SetSkew(0.0);
00139 
00140     P.Compose(K, R, C);
00141 
00142     if (numTests == 1) {
00143       cout << "K " << K << endl;
00144       cout << "R " << R << endl;
00145       cout << "C " << C << endl;
00146       cout << "P " << P << endl;
00147     }
00148     
00149     P.InvalidateDecomposition();
00150 
00151     ppp.SetIdealImageSize(ImgWidth, ImgHeight);
00152 
00153     ppp.SetP(P);
00154 
00155     // generate some 3D points
00156     int numPoints = 20;
00157     vector<HomgPoint3D> points3D;
00158     vector<double> zCorr;
00159     HomgPoint3D temp3D;
00160     for (int i = 0; i < numPoints; i++) {
00161       temp3D = HomgPoint3D(Randomizer.GetUniformDistributed(-1, 1),
00162           Randomizer.GetUniformDistributed(-1, 1), Randomizer.GetUniformDistributed(1, 2));
00163       temp3D.Homogenize();
00164       //      cout << "num " << i << " : " << temp3D << endl;
00165       zCorr.push_back(temp3D[2]);
00166       points3D.push_back(temp3D);
00167     }
00168 
00169     // project 3D points
00170     vector<HomgPoint2D> points2D;
00171     HomgPoint2D temp2D;
00172     for (int i = 0; i < numPoints; i++) {
00173       temp2D = ppp.Project(points3D[i], true);
00174       temp2D.Homogenize();
00175       //      cout << "2D " << i << " : " << temp2D << endl;
00176       points2D.push_back(temp2D);
00177     }
00178 
00179     // unproject 2D points and compute sum of normed errors
00180     vector<HomgPoint3D> unprojectRay, backprojectZDepth, backprojectWorldCoo;
00181 
00182     vector<double> depth;
00183     double errorSum = 0.0;
00184     double errorSum2 = 0.0;
00185     HomgPoint3D ray;
00186     Vector3<double> pointinCamCorrs, pos, dir;
00187     HomgPoint2D withoutK;
00188     double zTemp, depthTemp;
00189     for (int j = 0; j < numPoints; j++) {
00190 
00191       // projection parameters perspective -> unprojectToRay 
00192       ppp.UnProjectToRay(points2D[j], pos, dir, true);
00193       ray = dir;
00194       if (ray[2] != 0.0) {
00195         ray[0] *= zCorr[j] / ray[2];
00196         ray[1] *= zCorr[j] / ray[2];
00197         ray[2] *= zCorr[j] / ray[2];
00198         ray.Homogenize();
00199         ray[0] += C[0];
00200         ray[1] += C[1];
00201         ray[2] += C[2];
00202         unprojectRay.push_back(ray);
00203 
00204         //      cout << "ray num " << j << " : " << ray << endl;
00205         errorSum += (ray - points3D[j]).NormL2();
00206       } else {
00207         cout << "ray[2] == 0 for point " << j << " " << ray << endl;
00208         unprojectRay.push_back(HomgPoint3D(0, 0, 0, 1));
00209       }
00210 
00211       // PMatrix backproject -> by zDepth
00212       PMatrix withoutK;
00213       KMatrix idK(MatrixIdentity);
00214       Vector3<double> camPointTemp;
00215       withoutK.Compose(idK, R, C);
00216       camPointTemp = withoutK * points3D[j];
00217       zTemp = camPointTemp[2];
00218       P.BackprojectByZDepth(points2D[j][0], points2D[j][1], zTemp, temp3D);
00219       temp3D.Homogenize();
00220       backprojectZDepth.push_back(temp3D);
00221       errorSum2 += (temp3D - points3D[j]).NormL2();
00222       
00223       // PMatrix backproject -> world coo
00224       temp2D = points2D[j];
00225       temp2D[0] -= principalX;
00226       temp2D[1] = principalY - temp2D[1];
00227       temp2D = K.Invert() * temp2D;
00228       depthTemp = sqrt(sqrt(camPointTemp[0]*camPointTemp[0] - temp2D[1]*temp2D[1]) - temp2D[0]*temp2D[0]);
00229       if(depthTemp!=depthTemp) depthTemp = 1.0;
00230       P.BackprojectWorldCoo(points2D[j], depthTemp, temp3D);
00231       if(temp3D[2] != 0){
00232         temp3D.Homogenize();
00233         backprojectWorldCoo.push_back(temp3D);
00234       } else {
00235         backprojectWorldCoo.push_back(HomgPoint3D(0,0,0,1));
00236         cout << "problem with temp3D[2] " << temp3D << endl;
00237       }
00238       cout << "point : " << j << backprojectWorldCoo[j] << endl;
00239     }
00240 
00241     cout << "error sum   " << errorSum << endl;
00242     cout << "error sum 2 " << errorSum2 << endl;
00243 
00244     if (numTests==1) {
00245       ThreeDOutParameters param3DO;
00246       param3DO.CameraStyle = PyramidMesh;
00247       param3DO.PointStyle = Box;
00248       param3DO.PointSize = 0.1;
00249       param3DO.LineStyle = Solid;
00250       param3DO.DrawUncertaintyEllipsoids = false;
00251       ThreeDOut threeDOut(param3DO);
00252 
00253       for (int i = 0; i < numPoints; i++) {
00254         points3D[i].Homogenize();
00255         threeDOut.AddPoint(points3D[i], RGBAuc(255, 0, 0, 127));
00256 
00257         unprojectRay[i].Homogenize();
00258         threeDOut.AddPoint(unprojectRay[i], RGBAuc(0, 255, 255, 127));
00259 
00260         backprojectZDepth[i].Homogenize();
00261         threeDOut.AddPoint(backprojectZDepth[i], RGBAuc(255, 0, 255, 127));
00262         
00263         backprojectWorldCoo[i].Homogenize();
00264         threeDOut.AddPoint(backprojectWorldCoo[i], RGBAuc(255, 255, 255, 127));
00265               
00266         
00267       }
00268 
00269       Vector3<double> start(0.0, 0.0, 0.0);
00270       Vector3<double> end(1.0, 0.0, 0.0);
00271 
00272       threeDOut.AddLine(start, end, RGBAuc(255, 0, 0, 127));
00273       end.Set(0.0, 1.0, 0.0);
00274       threeDOut.AddLine(start, end, RGBAuc(0, 255, 0, 127));
00275       end.Set(0.0, 0.0, 1.0);
00276       threeDOut.AddLine(start, end, RGBAuc(0, 0, 255, 127));
00277       threeDOut.VRMLOut("test.wrl");
00278       threeDOut.Dump();
00279     } // end if(numTest ==1)
00280   } // end for numTests
00281 
00282   return 0;
00283 }
 All Classes Functions Variables Typedefs Enumerations Enumerator Friends