Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
ExampleProjectionParametersPerspective2.cpp
1 /*
2  This file is part of the BIAS library (Basic ImageAlgorithmS).
3 
4  Copyright (C) 2003-2009 (see file CONTACT for details)
5  Multimediale Systeme der Informationsverarbeitung
6  Institut fuer Informatik
7  Christian-Albrechts-Universitaet Kiel
8 
9 
10  BIAS is free software; you can redistribute it and/or modify
11  it under the terms of the GNU Lesser General Public License as published by
12  the Free Software Foundation; either version 2.1 of the License, or
13  (at your option) any later version.
14 
15  BIAS is distributed in the hope that it will be useful,
16  but WITHOUT ANY WARRANTY; without even the implied warranty of
17  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  GNU Lesser General Public License for more details.
19 
20  You should have received a copy of the GNU Lesser General Public License
21  along with BIAS; if not, write to the Free Software
22  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
23  */
24 
25 
26 /**
27  @example ExampleProjectionParametersPerspective2.cpp
28  @relates ProjectionParametersPerspective
29  @brief small example demonstrating the ProjectionParametersPerspective
30  by applying project and unproject
31  @ingroup g_examples
32  @author grauel 9/2007
33 */
34 
35 #include <Geometry/ProjectionParametersPerspective.hh>
36 #include <Base/Image/Image.hh>
37 #include <Base/Image/ImageIO.hh>
38 #include <Base/Math/Random.hh>
39 #include <Base/Geometry/HomgPoint2D.hh>
40 #include <Utils/ThreeDOut.hh>
41 
42 using namespace BIAS;
43 using namespace std;
44 
45 
46 int main(int argc, char *argv[]) {
48 
49  // used .IOR - parameters
50  // 1
51  //-999
52  //-20.30377 cam const in mm (?)
53  //-0.06444 princpx normalized or mm (?)
54  //-0.15281 princpy
55  //-2.87698e-004 k1
56  //6.73824e-007 k2
57  // .85 r0
58  //-6.23736e-010
59  // 1.97915e-005 k3
60  //-2.11616e-005 k4
61  //-1.38247e-004 ascpect
62  // 8.89027e-006 skew
63  // 23.60000 sensorsize in mm
64  // 15.80000 sensorsize in mm
65  // 3872 pixel x
66  // 2592 pixel y
67 
68  // double CamConst = 20.30377; //in mm - perpendicular distance of projection center to image plane
69  // double SensorSizeX = 23.6; //in mm -
70  // double SensorSizeY = 15.8; //in mm
71  //
72  // unsigned int ImgWidth = 3872;
73  // unsigned int ImgHeight = 2592;
74  //
75  // double principalX = (double) ImgWidth*0.5 * (1-0.06444/SensorSizeX);
76  // double principalY = (double) ImgHeight*0.5 * (1-0.15281/SensorSizeY);
77  // double focalX = (double)ImgWidth*0.5*(CamConst/SensorSizeX );//in pixel
78  // double aspectrat = (1-1.38247e-004)*(double)ImgWidth/(double)ImgHeight;
79  //
80  // //ppp.SetDistortionType(DISTYPE_BROWN); //not neede anymore
81  // //also set by calling
82  // //SetUndistortionBrown()
83  // double kc1 = -2.87698e-004;
84  // double kc2 = 6.73824e-007 ;
85  // double kc3 = 1.97915e-005;
86  // double kc4 = -2.11616e-005;
87  //
88  // double r0 = .85; // second constant root of polynomial
89  //
90  //// ppp.SetUndistortionBrown(kc1,kc2,kc3,kc4,r0);
91  // ppp.SetFocalLengthAndAspect(focalX,aspectrat);
92  // ppp.SetPrincipal(principalX,principalY);
93  // ppp.SetIdealImageSize(ImgWidth,ImgHeight);
94 
95  BIAS::PMatrix P;
96  BIAS::Random Randomizer;
97 
98  unsigned int numTests = 1;
99  for (unsigned int tests=0; tests<numTests; tests++) {
100 
101  // Vector3<double> C(Randomizer.GetUniformDistributed(-2, 2), Randomizer.GetUniformDistributed(
102  // -2, 2), Randomizer.GetUniformDistributed(-2, 2));
103 
104  Vector3<double> C(1, 2, 0);
105 
106  // Vector3<double> RAxis(Randomizer.GetUniformDistributed(-1.0, 1.0),
107  // Randomizer.GetUniformDistributed(-1.0, 1.0), Randomizer.GetUniformDistributed(-1.0, 1.0));
108  // RAxis.Normalize();
109  // double rangle = Randomizer.GetUniformDistributed(-M_PI, M_PI);
110 
111  // RMatrix R(RAxis, rangle);
113  R.SetXYZ(30.0*M_PI/180.0, 3.0*M_PI/180.0, 0.0);
114  // R.SetIdentity();
115 
116  // Quaternion<double> rot;
117  // rot.SetValueAsAxisRad(RAxis, rangle);
118 
119  // PoseParametrization poseParam;
120  // poseParam.SetCQ(C, rot);
121  // ppp.SetPoseParametrization(poseParam);
122 
123  double focalX = Randomizer.GetUniformDistributed(100, 1000);
124  double aspectrat = Randomizer.GetNormalDistributed( 0.9, 1.1);
125 
126  double principalX = Randomizer.GetUniformDistributed(100, 200);
127  double principalY = Randomizer.GetUniformDistributed(100, 200);
128 
129  double ImgWidth = 300;
130  double ImgHeight = 300;
131 
133  K.SetFx(focalX);
134  K.SetFy(focalX*aspectrat);
135  K.SetHx(principalX);
136  K.SetHy(principalY);
137  K.SetSkew(Randomizer.GetUniformDistributed(-K[0][0], K[0][0]));
138  //K.SetSkew(0.0);
139 
140  P.Compose(K, R, C);
141 
142  if (numTests == 1) {
143  cout << "K " << K << endl;
144  cout << "R " << R << endl;
145  cout << "C " << C << endl;
146  cout << "P " << P << endl;
147  }
148 
150 
151  ppp.SetIdealImageSize(ImgWidth, ImgHeight);
152 
153  ppp.SetP(P);
154 
155  // generate some 3D points
156  int numPoints = 20;
157  vector<HomgPoint3D> points3D;
158  vector<double> zCorr;
159  HomgPoint3D temp3D;
160  for (int i = 0; i < numPoints; i++) {
161  temp3D = HomgPoint3D(Randomizer.GetUniformDistributed(-1, 1),
162  Randomizer.GetUniformDistributed(-1, 1), Randomizer.GetUniformDistributed(1, 2));
163  temp3D.Homogenize();
164  // cout << "num " << i << " : " << temp3D << endl;
165  zCorr.push_back(temp3D[2]);
166  points3D.push_back(temp3D);
167  }
168 
169  // project 3D points
170  vector<HomgPoint2D> points2D;
171  HomgPoint2D temp2D;
172  for (int i = 0; i < numPoints; i++) {
173  temp2D = ppp.Project(points3D[i], true);
174  temp2D.Homogenize();
175  // cout << "2D " << i << " : " << temp2D << endl;
176  points2D.push_back(temp2D);
177  }
178 
179  // unproject 2D points and compute sum of normed errors
180  vector<HomgPoint3D> unprojectRay, backprojectZDepth, backprojectWorldCoo;
181 
182  vector<double> depth;
183  double errorSum = 0.0;
184  double errorSum2 = 0.0;
185  HomgPoint3D ray;
186  Vector3<double> pointinCamCorrs, pos, dir;
187  HomgPoint2D withoutK;
188  double zTemp, depthTemp;
189  for (int j = 0; j < numPoints; j++) {
190 
191  // projection parameters perspective -> unprojectToRay
192  ppp.UnProjectToRay(points2D[j], pos, dir, true);
193  ray = dir;
194  if (ray[2] != 0.0) {
195  ray[0] *= zCorr[j] / ray[2];
196  ray[1] *= zCorr[j] / ray[2];
197  ray[2] *= zCorr[j] / ray[2];
198  ray.Homogenize();
199  ray[0] += C[0];
200  ray[1] += C[1];
201  ray[2] += C[2];
202  unprojectRay.push_back(ray);
203 
204  // cout << "ray num " << j << " : " << ray << endl;
205  errorSum += (ray - points3D[j]).NormL2();
206  } else {
207  cout << "ray[2] == 0 for point " << j << " " << ray << endl;
208  unprojectRay.push_back(HomgPoint3D(0, 0, 0, 1));
209  }
210 
211  // PMatrix backproject -> by zDepth
212  PMatrix withoutK;
213  KMatrix idK(MatrixIdentity);
214  Vector3<double> camPointTemp;
215  withoutK.Compose(idK, R, C);
216  camPointTemp = withoutK * points3D[j];
217  zTemp = camPointTemp[2];
218  P.BackprojectByZDepth(points2D[j][0], points2D[j][1], zTemp, temp3D);
219  temp3D.Homogenize();
220  backprojectZDepth.push_back(temp3D);
221  errorSum2 += (temp3D - points3D[j]).NormL2();
222 
223  // PMatrix backproject -> world coo
224  temp2D = points2D[j];
225  temp2D[0] -= principalX;
226  temp2D[1] = principalY - temp2D[1];
227  temp2D = K.Invert() * temp2D;
228  depthTemp = sqrt(sqrt(camPointTemp[0]*camPointTemp[0] - temp2D[1]*temp2D[1]) - temp2D[0]*temp2D[0]);
229  if(depthTemp!=depthTemp) depthTemp = 1.0;
230  P.BackprojectWorldCoo(points2D[j], depthTemp, temp3D);
231  if(temp3D[2] != 0){
232  temp3D.Homogenize();
233  backprojectWorldCoo.push_back(temp3D);
234  } else {
235  backprojectWorldCoo.push_back(HomgPoint3D(0,0,0,1));
236  cout << "problem with temp3D[2] " << temp3D << endl;
237  }
238  cout << "point : " << j << backprojectWorldCoo[j] << endl;
239  }
240 
241  cout << "error sum " << errorSum << endl;
242  cout << "error sum 2 " << errorSum2 << endl;
243 
244  if (numTests==1) {
245  ThreeDOutParameters param3DO;
246  param3DO.CameraStyle = PyramidMesh;
247  param3DO.PointStyle = Box;
248  param3DO.PointSize = 0.1;
249  param3DO.LineStyle = Solid;
250  param3DO.DrawUncertaintyEllipsoids = false;
251  ThreeDOut threeDOut(param3DO);
252 
253  for (int i = 0; i < numPoints; i++) {
254  points3D[i].Homogenize();
255  threeDOut.AddPoint(points3D[i], RGBAuc(255, 0, 0, 127));
256 
257  unprojectRay[i].Homogenize();
258  threeDOut.AddPoint(unprojectRay[i], RGBAuc(0, 255, 255, 127));
259 
260  backprojectZDepth[i].Homogenize();
261  threeDOut.AddPoint(backprojectZDepth[i], RGBAuc(255, 0, 255, 127));
262 
263  backprojectWorldCoo[i].Homogenize();
264  threeDOut.AddPoint(backprojectWorldCoo[i], RGBAuc(255, 255, 255, 127));
265 
266 
267  }
268 
269  Vector3<double> start(0.0, 0.0, 0.0);
270  Vector3<double> end(1.0, 0.0, 0.0);
271 
272  threeDOut.AddLine(start, end, RGBAuc(255, 0, 0, 127));
273  end.Set(0.0, 1.0, 0.0);
274  threeDOut.AddLine(start, end, RGBAuc(0, 255, 0, 127));
275  end.Set(0.0, 0.0, 1.0);
276  threeDOut.AddLine(start, end, RGBAuc(0, 0, 255, 127));
277  threeDOut.VRMLOut("test.wrl");
278  threeDOut.Dump();
279  } // end if(numTest ==1)
280  } // end for numTests
281 
282  return 0;
283 }
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
Definition: HomgPoint2D.hh:67
void SetXYZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order XYZ.
camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
Unified output of 3D entities via OpenGL or VRML.
Definition: ThreeDOut.hh:336
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
Definition: HomgPoint2D.hh:215
void Homogenize()
homogenize class data member elements to W==1 by divison by W
Definition: HomgPoint3D.hh:308
configuration struct for drawing styles of various 3d objects
Definition: ThreeDOut.hh:296
void SetSkew(const KMATRIX_TYPE &val)
Definition: KMatrix.cpp:81
virtual void SetP(const PMatrix &P)
set from P
double GetUniformDistributed(const double min, const double max)
on succesive calls return uniform distributed random variable between min and max ...
Definition: Random.hh:84
void SetIdealImageSize(unsigned int width, unsigned int height)
virtual void UnProjectToRay(const HomgPoint2D &pos, Vector3< double > &origin, Vector3< double > &direction, bool ignoreDistortion=false) const
Calculates the view ray, which belongs to the given position on the image plane, in global coordinate...
CameraDrawingStyle CameraStyle
Definition: ThreeDOut.hh:300
virtual HomgPoint2D Project(const HomgPoint3D &X, bool IgnoreDistortion=false) const
calculates the projection of a point in the world coordinate system to a pixel in the image plane of ...
3D rotation matrix
Definition: RMatrix.hh:49
void InvalidateDecomposition()
to re-Decompose_() after filling with data use this.
Definition: PMatrix.hh:499
LineDrawingStyle LineStyle
Definition: ThreeDOut.hh:301
BIAS::Vector4< unsigned char > RGBAuc
Definition: RGBA.hh:47
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
Definition: HomgPoint3D.hh:61
void SetFx(const KMATRIX_TYPE &val)
Definition: KMatrix.cpp:75
void SetHy(const KMATRIX_TYPE &val)
Definition: KMatrix.cpp:87
void SetFy(const KMATRIX_TYPE &val)
Definition: KMatrix.cpp:78
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
Definition: KMatrix.hh:48
void BackprojectWorldCoo(const HomgPoint2D &point, double depth, HomgPoint3D &res)
returns a 3D point res which is located depth away from center of projection in direction given by po...
Definition: PMatrix.cpp:468
double GetNormalDistributed(const double mean, const double sigma)
on succesive calls return normal distributed random variable with mean and standard deviation sigma ...
Definition: Random.hh:71
describes a projective 3D -&gt; 2D mapping in homogenous coordinates
Definition: PMatrix.hh:88
void BackprojectByZDepth(const double &x, const double &y, const double &z_depth, HomgPoint3D &res)
Inverts the PMatrix application to a 3D point whose z-component in cameracoordinates and its imagepoi...
Definition: PMatrix.cpp:454
PointDrawingStyle PointStyle
Definition: ThreeDOut.hh:299
void SetHx(const KMATRIX_TYPE &val)
Definition: KMatrix.cpp:84
KMatrix Invert() const
returns analyticaly inverted matrix
Definition: KMatrix.cpp:31
class for producing random numbers from different distributions
Definition: Random.hh:51
void Compose(const Matrix3x3< double > &K, const Matrix3x3< double > &R, const Vector3< double > &C)
composes this from K, R and C using P = [ K R&#39; | -K R&#39; C ] with R&#39; = transpose(R) ...
Definition: PMatrix.cpp:581
class BIASGeometryBase_EXPORT HomgPoint3D