Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
Triangulation.cpp
1 /* This file is part of the BIAS library (Basic ImageAlgorithmS).
2 
3  Copyright (C) 2003-2009 (see file CONTACT for details)
4  Multimediale Systeme der Informationsverarbeitung
5  Institut fuer Informatik
6  Christian-Albrechts-Universitaet Kiel
7 
8  BIAS is free software; you can redistribute it and/or modify
9  it under the terms of the GNU Lesser General Public License as published by
10  the Free Software Foundation; either version 2.1 of the License, or
11  (at your option) any later version.
12 
13  BIAS is distributed in the hope that it will be useful,
14  but WITHOUT ANY WARRANTY; without even the implied warranty of
15  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  GNU Lesser General Public License for more details.
17 
18  You should have received a copy of the GNU Lesser General Public License
19  along with BIAS; if not, write to the Free Software
20  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */
21 #include "Triangulation.hh"
22 #include <Base/Geometry/RMatrixBase.hh>
23 #include <Geometry/ProjectionParametersBase.hh>
24 #include <Geometry/Pose.hh>
25 #include <Base/Math/Random.hh>
26 #include <Base/Debug/Error.hh>
27 #include <MathAlgo/PolynomialSolve.hh>
28 #include <Base/Geometry/HomgPoint2D.hh>
29 #include <Base/Geometry/HomgPoint3D.hh>
30 #include <Base/Geometry/HomgPoint2DCov.hh>
31 #include <Base/Geometry/HomgPoint3DCov.hh>
32 #include <Base/Math/Matrix3x3.hh>
33 #include <Base/Math/Matrix4x4.hh>
34 #include "PMatrix.hh"
35 #include "FMatrix.hh"
36 
37 #include <cmath>
38 
39 using namespace BIAS;
40 using namespace std;
41 
42 
43 Triangulation::
44 Triangulation()
45 : Debug()
46 {
47  NewDebugLevel("D_TR_POS");
48  NewDebugLevel("D_TR_TR");
49  NewDebugLevel("D_TR_ARG");
50  NewDebugLevel("D_TR_INF");
51  NewDebugLevel("D_TR_OPT");
52  NewDebugLevel("D_TR_OPT_TRANSF");
53  NewDebugLevel("D_TR_OPT_F");
54  NewDebugLevel("D_TRIANG_COVWEIGHT");
55  NewDebugLevel("D_TR_INTERSECT");
56  NewDebugLevel("D_TR_RAY");
57 
58  _UpdateCovWeights();
59 }
60 
61 
62 Triangulation::
63 ~Triangulation()
64 {}
65 
66 
67 int Triangulation::
68 Optimal(PMatrix &P1, PMatrix &P2, HomgPoint2D& p1,
69  HomgPoint2D& p2, HomgPoint3D& point3d)
70 {
71  FMatrix F(P1, P2);
72 
73  int res=CorrectCorrespondences(F, p1, p2);
74  if (res==0){
75  Vector3<double> r=F*p2;
76  BCDOUT(D_TR_OPT, "p1: "<<p1<<"tp2: "<<p2<<endl);
77  BCDOUT(D_TR_OPT, "p1' * F * p2 = "<<r.ScalarProduct(p1)<<endl);
78  res=TriangulateLinear(P1, P2, p1, p2, point3d);
79  } else {
80  BIASERR("CorrectCorrespondences failed "<<res);
81  }
82  return res;
83 }
84 
85 
86 int Triangulation::
87 TriangulateLinear(PMatrix &P1, PMatrix &P2, HomgPoint2D& p1,
88  HomgPoint2D& p2, HomgPoint3D& point3d)
89 {
90  // set up DLT according to Zisserman: "Multiple View Geometry", p.297
91  Matrix4x4<double> A;
92 
93  A[0][0] = P1[0][0] * p1[2] - P1[2][0] * p1[0];
94  A[0][1] = P1[0][1] * p1[2] - P1[2][1] * p1[0];
95  A[0][2] = P1[0][2] * p1[2] - P1[2][2] * p1[0];
96  A[0][3] = P1[0][3] * p1[2] - P1[2][3] * p1[0];
97 
98  A[1][0] = P1[1][0] * p1[2] - P1[2][0] * p1[1];
99  A[1][1] = P1[1][1] * p1[2] - P1[2][1] * p1[1];
100  A[1][2] = P1[1][2] * p1[2] - P1[2][2] * p1[1];
101  A[1][3] = P1[1][3] * p1[2] - P1[2][3] * p1[1];
102 
103  A[2][0] = P2[0][0] * p2[2] - P2[2][0] * p2[0];
104  A[2][1] = P2[0][1] * p2[2] - P2[2][1] * p2[0];
105  A[2][2] = P2[0][2] * p2[2] - P2[2][2] * p2[0];
106  A[2][3] = P2[0][3] * p2[2] - P2[2][3] * p2[0];
107 
108  A[3][0] = P2[1][0] * p2[2] - P2[2][0] * p2[1];
109  A[3][1] = P2[1][1] * p2[2] - P2[2][1] * p2[1];
110  A[3][2] = P2[1][2] * p2[2] - P2[2][2] * p2[1];
111  A[3][3] = P2[1][3] * p2[2] - P2[2][3] * p2[1];
112 
113  A.NormalizeRows();
114 
115  SVD svd(A);
116  BIAS::Matrix<double> VT = svd.GetVT();
117  point3d = (BIAS::Vector4<double>) VT.GetRow(3);
118  point3d.Homogenize();
119  return 0;
120 }
121 
122 
123 int Triangulation::
124 TriangulateLinear(vector<PMatrix> pmatrices, vector<HomgPoint2D> homgPoints2D,
125  HomgPoint3D& point3d)
126 {
127  if( pmatrices.size() != homgPoints2D.size() || pmatrices.size() < 2)
128  {
129  BIASERR("Not enough HomgPoints or PMatrices.");
130  return -1;
131  }
132 
133  // compute number of rows for matrix
134  int size = (int)homgPoints2D.size();
135  unsigned int rows = 4 * size * (size - 1) / 2;
136 
137  Matrix<double> A(rows, 4);
138  int row = 0;
139 
140  for (int i = 0; i < size; i++)
141  {
142  for (int j = 0; j < i; j++)
143  {
144  PMatrix P1 = pmatrices[i];
145  PMatrix P2 = pmatrices[j];
146  HomgPoint2D p1 = homgPoints2D[i];
147  HomgPoint2D p2 = homgPoints2D[j];
148 
149  A[row][0] = P1[0][0] * p1[2] - P1[2][0] * p1[0];
150  A[row][1] = P1[0][1] * p1[2] - P1[2][1] * p1[0];
151  A[row][2] = P1[0][2] * p1[2] - P1[2][2] * p1[0];
152  A[row][3] = P1[0][3] * p1[2] - P1[2][3] * p1[0];
153 
154  row++;
155 
156  A[row][0] = P1[1][0] * p1[2] - P1[2][0] * p1[1];
157  A[row][1] = P1[1][1] * p1[2] - P1[2][1] * p1[1];
158  A[row][2] = P1[1][2] * p1[2] - P1[2][2] * p1[1];
159  A[row][3] = P1[1][3] * p1[2] - P1[2][3] * p1[1];
160 
161  row++;
162 
163  A[row][0] = P2[0][0] * p2[2] - P2[2][0] * p2[0];
164  A[row][1] = P2[0][1] * p2[2] - P2[2][1] * p2[0];
165  A[row][2] = P2[0][2] * p2[2] - P2[2][2] * p2[0];
166  A[row][3] = P2[0][3] * p2[2] - P2[2][3] * p2[0];
167 
168  row++;
169 
170  A[row][0] = P2[1][0] * p2[2] - P2[2][0] * p2[1];
171  A[row][1] = P2[1][1] * p2[2] - P2[2][1] * p2[1];
172  A[row][2] = P2[1][2] * p2[2] - P2[2][2] * p2[1];
173  A[row][3] = P2[1][3] * p2[2] - P2[2][3] * p2[1];
174 
175  row++;
176  }
177  }
178 
179  A.NormalizeRows();
180 
181  SVD svd(A);
182  BIAS::Matrix<double> VT = svd.GetVT();
183  point3d = (BIAS::Vector4<double>) VT.GetRow(3);
184  point3d.Homogenize();
185 
186  return 0;
187 }
188 
189 
190 int Triangulation::
191 CorrectCorrespondences(const FMatrix& F, HomgPoint2D& p1,
192  HomgPoint2D& p2, bool refine)
193 {
194 #ifdef BIAS_DEBUG
195  BCDOUT(D_TR_OPT_F, "p1: "<<p1<<"\tp2: "<<p2<<endl);
196  BCDOUT(D_TR_OPT_F, "before: p2^T * F * p1 = "<<(F*p1).ScalarProduct(p2)
197  <<endl);
198 #endif
199 
200  p1.Homogenize();
201  p2.Homogenize();
202  Matrix3x3<double> T, Tp, Ti, Tpi;
203  T.SetIdentity();
204  Tp.SetIdentity();
205  Ti.SetIdentity();
206  Tpi.SetIdentity();
207  T[0][2]=-p1[0];
208  T[1][2]=-p1[1];
209  Tp[0][2]=-p2[0];
210  Tp[1][2]=-p2[1];
211  Ti[0][2]=p1[0];
212  Ti[1][2]=p1[1];
213  Tpi[0][2]=p2[0];
214  Tpi[1][2]=p2[1];
215 
216  BCDOUT(D_TR_OPT_TRANSF, "T: "<<T<<"\nTp: "<<Tp<<"\nTi: "<<Ti
217  <<"\nTpi: "<<Tpi<<endl);
218 
219  FMatrix mF;
220  mF=Tpi.Transpose()*F*Ti;
221  BCDOUT(D_TR_OPT_TRANSF, "orig F: "<<F<<"\nTpi.Transpose() * F * Ti = "
222  <<mF<<endl);
223 
224  HomgPoint2D e, ep;
225  F.GetEpipoles(e, ep);
226  ep.Homogenize(); e.Homogenize();
227  BCDOUT(D_TR_OPT_TRANSF, "orig epipoles: "<<e<<"\t"<<ep<<endl);
228 
229  mF.GetEpipoles(e, ep);
230  BCDOUT(D_TR_OPT_TRANSF, "epipoles: "<<e<<"\t"<<ep<<endl);
231 
232 #ifdef BIAS_DEBUG
233  Vector3<double> r;
234  mF.TransposedMult(ep, r);
235  BCDOUT(D_TR_OPT_TRANSF, "F*e = "<<mF*e<<"\tep^T*F = "<<r<<endl);
236 
237  // BIASASSERT((mF*e).NormL2()<=1e-10);
238  // BIASASSERT(r.NormL2()<=1e-10);
239 #endif
240 
241  double se, sep;
242  se=sqrt(e[0]*e[0]+e[1]*e[1]);
243  sep=sqrt(ep[0]*ep[0]+ep[1]*ep[1]);
244 
245  e/=se;
246  ep/=sep;
247 
248  BCDOUT(D_TR_OPT_TRANSF, "normalized epipoles: "<<e<<"\t"<<ep<<endl);
249 
250  RMatrixBase R, Rp;
251  R.SetIdentity();
252  Rp.SetIdentity();
253 
254  R[0][0]=R[1][1]=e[0];
255  R[0][1]=e[1];
256  R[1][0]=-e[1];
257 
258  Rp[0][0]=Rp[1][1]=ep[0];
259  Rp[0][1]=ep[1];
260  Rp[1][0]=-ep[1];
261 
262  BCDOUT(D_TR_OPT_TRANSF, "R*e = "<<R*e<<"\tRp*ep = "<<Rp * ep<<endl);
263 
264  mF = Rp * mF * R.Transpose();
265 
266  BCDOUT(D_TR_OPT_TRANSF, "Rp * Tpi.Transpose() * F * Ti * R.Transpose() = "
267  <<mF<<endl);
268 
269 #ifdef BIAS_DEBUG
270  Vector3<double> r4(0.0, 0.0, 1.0);
271  BCDOUT(D_TR_OPT_F, "before: (0, 0, 1) * mF * (0, 0, 1)^T = "
272  <<(mF*r4).ScalarProduct(r4)<<endl);
273  BCDOUT(D_TR_OPT_TRANSF, "Ti * R.Transpose() * (0,0,1): "
274  <<Ti * R.Transpose() * r4
275  <<"\t(Ti * R.Transpose() * (0,0,1))-p1 ="
276  <<(Ti * R.Transpose() * r4)-p1<<endl);
277  BCDOUT(D_TR_OPT_TRANSF, "Tpi * Rp.Transpose() * (0,0,1): "
278  <<Tpi * Rp.Transpose() * r4<<"\t(Tpi * Rp.Transpose() * r4)-p2 = "
279  <<(Tpi * Rp.Transpose() * r4)-p2<<endl);
280 #endif
281 
282  double f, fp, a, b, c, d;
283  f=e[2];
284  fp=ep[2];
285  a=mF[1][1];
286  b=mF[1][2];
287  c=mF[2][1];
288  d=mF[2][2];
289  BCDOUT(D_TR_OPT, "ffpd: "<<f*fp*d<<"\t-fpc: "<<-fp*c<<"\t-fpd: "<<-fp*d
290  <<"\n-fb: "<<-f*b<<"\ta: "<<a<<"\tb: "<<b
291  <<"\n-fd: "<<-f*d<<"\tc: "<<c<<"\td: "<<d<<endl<<endl);
292 
293  BCDOUT(D_TR_OPT, "a: "<<a<<"\tb: "<<b<<"\tc: "<<c<<"\td: "<<d<<"\tf: "<<f
294  <<"\tfp: "<<fp<<endl);
295  /** see mupad
296  assume(a, Type::Real):
297  assume(b, Type::Real):
298  assume(c, Type::Real):
299  assume(d, Type::Real):
300  assume(f, Type::Real):
301  assume(mfp, Type::Real):
302  assume(t, Type::Real):
303 
304  g:=t*((a*t+b)^2+mfp^2*(c*t+d)^2)^2-(a*d-b*c)*(1+f^2*t^2)^2*(a*t+b)*(c*t+d):
305 
306  g:=simplify(expand(g)):
307 
308  PRETTYPRINT := FALSE:
309 
310  res:=poly(collect(g, [t]), [t]);
311 
312  fprint(Unquoted, 0, "coeff[0]=", coeff(res, t, 0), "; // t^0"):
313  fprint(Unquoted, 0, "coeff[1]=", coeff(res, t, 1), "; // t^1"):
314  fprint(Unquoted, 0, "coeff[2]=", coeff(res, t, 2), "; // t^2"):
315  fprint(Unquoted, 0, "coeff[3]=", coeff(res, t, 3), "; // t^3"):
316  fprint(Unquoted, 0, "coeff[4]=", coeff(res, t, 4), "; // t^4"):
317  fprint(Unquoted, 0, "coeff[5]=", coeff(res, t, 5), "; // t^5"):
318  fprint(Unquoted, 0, "coeff[6]=", coeff(res, t, 6), "; // t^6"):
319  */
320  vector<POLYNOMIALSOLVE_TYPE> coeff(7);
321  vector<complex<POLYNOMIALSOLVE_TYPE> > csol;
322  double a2=a*a, b2=b*b, c2=c*c, d2=d*d, f2=f*f, fp2=fp*fp;
323  double a3=a2*a, b3=b2*b, c3=c2*c, d3=d2*d;
324  double a4=a2*a2, b4=b2*b2, c4=c2*c2, d4=d2*d2, f4=f2*f2, fp4=fp2*fp2;
325  double ab=a*b, cd=c*d;
326  coeff[0]=b2*cd - ab*d2; // t^0
327  coeff[1]=b4 - a2*d2 + b2*c2 + d4*fp4 + 2*b2*d2*fp2; // t^1
328  coeff[2]=4*a*b3 + ab*c2 - a2*cd - 2*ab*d2*f2 + 2*b2*cd*f2 + 4*c*d3*fp4 +
329  4*ab*d2*fp2 + 4*b2*cd*fp2; // t^2
330  coeff[3]=8*ab*cd*fp2 + 6*a2*b2 - 2*a2*d2*f2 + 2*b2*c2*f2 + 2*a2*d2*fp2 +
331  2*b2*c2*fp2 + 6*c2*d2*fp4; // t^3
332  coeff[4]=4*a3*b + 2*ab*c2*f2 - 2*a2*cd*f2 - ab*d2*f4 + b2*cd*f4 +
333  4*c3*d*fp4 + 4*ab*c2*fp2 + 4*a2*cd*fp2; // t^4
334  coeff[5]=a4 + c4*fp4 - a2*d2*f4 + b2*c2*f4 + 2*a2*c2*fp2; // t^5
335  coeff[6]=ab*c2*f4 - a2*cd*f4; // t^6
336 
337  PolynomialSolve ps;
338  if(DebugLevelIsSet("D_TR_OPT"))
339  ps.AddDebugLevel(D_PS_GSL);
340  // crop upper zero coeffizients
341  ps.CheckCoefficients(coeff);
342  // while ((*coeff.rbegin())==0.0) { coeff.pop_back(); }
343  int psres=ps.Numeric(coeff, csol);
344  BCDOUT(D_TR_OPT, "csol.size(): "<<csol.size()<<"\tpsres: "<<psres<<endl);
345  if (psres<0){
346  BIASERR("error solving polynomial "<<psres);
347  BIASABORT;
348  //return psres;
349  }
350  if (refine)
351  ps.NonLinearRefine(coeff, csol);
352 
353  // vector<POLYNOMIALSOLVE_TYPE> sol;
354  // ps.Solve(coeff, sol);
355  // BCDOUT(D_TR_OPT, "sol.size(): "<<sol.size()<<endl);
356 
357  vector<double> s(csol.size()+1);
358 
359  for (unsigned i=0; i<csol.size(); i++){
360  double sol2=real(csol[i])*real(csol[i]);
361  double t1=1.0+f2*sol2;
362  double t2=c*real(csol[i])+d;
363  double t3=a*real(csol[i])+b;
364  double t4=c*real(csol[i])+d;
365 
366  s[i]=sol2/(t1*t1)+(t2*t2)/(t3*t3+fp2*t4*t4);
367  BCDOUT(D_TR_OPT, "dist["<<i<<"] = "<<s[i]<<" = "<<sol2/(t1*t1)<<" + "
368  <<(t2*t2)/(t3*t3+fp2*t4*t4)<<endl);
369  }
370  s[csol.size()]=1.0/f2+c2/(a2+fp2*c2);
371  BCDOUT(D_TR_OPT, "dist["<<csol.size()<<"] = "<<s[csol.size()]
372  <<" = "<<1.0/f2<<" + "<<c2/(a2+fp2*c2)<<endl);
373 
374  double tmin=real(csol[0]);
375  #ifdef BIAS_DEBUG
376  double costmin=s[0];
377  int minres=0;
378 
379 
380  for (unsigned i=1; i<s.size(); i++){
381  if (s[i]<costmin) { costmin=s[i]; minres=i; tmin=real(csol[i]); }
382  }
383  #endif
384  BCDOUT(D_TR_OPT, "costmin = "<<costmin<<"\tat "<<minres<<"\ttmin: "
385  <<tmin<<endl);
386  BCDOUT(D_TR_OPT, "poli("<<tmin<<") = "<<coeff[0] + tmin*coeff[1] +
387  tmin*tmin*coeff[2]+tmin*tmin*tmin*coeff[3]+
388  tmin*tmin*tmin*tmin*coeff[4]+tmin*tmin*tmin*tmin*tmin*coeff[5]+
389  tmin*tmin*tmin*tmin*tmin*tmin*coeff[6]<<endl);
390  BCDOUT(D_TR_OPT, "ps.EvaluatePolynomial("<<tmin<<", coeff) = "
391  <<ps.EvaluatePolynomial(tmin, coeff)<<endl);
392 
393 
394  Vector3<double> l, lp;
395  l.Set(tmin*f, 1.0, -tmin);
396  lp.Set(-fp*(c*tmin+d), a*tmin+b, c*tmin+d);
397  // BCDOUT(D_TR_OPT, "l: "<<l<<"\tlp: "<<lp<<endl);
398 
399  HomgPoint2D xh, xhp;
400  xh.Set(-l[0]*l[2], -l[1]*l[2], l[0]*l[0]+l[1]*l[1]);
401  xhp.Set(-lp[0]*lp[2], -lp[1]*lp[2], lp[0]*lp[0]+lp[1]*lp[1]);
402  //BCDOUT(D_TR_OPT, "xh: "<<xh<<"\txhp: "<<xhp<<endl);
403 
404 #ifdef BIAS_DEBUG
405  //BCDOUT(D_TR_OPT, "xh: "<<xh<<"\txhp: "<<xhp<<endl);
406  BCDOUT(D_TR_OPT_F, "optimized: xhp * mF * xh = "
407  <<(mF*xh).ScalarProduct(xhp)<<endl);
408 #endif
409 
410  p1 = Ti * R.Transpose() * xh;
411  p2 = Tpi * Rp.Transpose() * xhp;
412  p1.Homogenize();
413  p2.Homogenize();
414  BCDOUT(D_TR_OPT, "optimized points "<<p1<<" <--> "<<p2<<endl);
415 
416 #ifdef BIAS_DEBUG
417  BCDOUT(D_TR_OPT_F, "optimized: p2^T * F * p1 = "<<(F*p1).ScalarProduct(p2)
418  <<endl);
419 #endif
420 
421  return 0;
422 }
423 
424 
425 int Triangulation::
426 Triangulate(const Vector3<double>& C1, const Quaternion<double>& Q1,
427  const Vector3<double>& C2, const Quaternion<double>& Q2,
428  const HomgPoint2D& p1, const HomgPoint2D& p2,
429  HomgPoint3D& point3d)
430 {
431  BIASWARNONCE("Triangulate(const Vector3<double>& C1, const Quaternion<double>& Q1, const Vector3<double>& C2, const Quaternion<double>& Q2, const HomgPoint2D& p1, const HomgPoint2D& p2, HomgPoint3D& point3d) is deprecated, \n please use Triangulate(const Vector3<double>& C1, const Quaternion<double>& Q1, const Vector3<double>& C2, const Quaternion<double>& Q2, const HomgPoint2D& p1, const HomgPoint2D& p2, Vector3<double>& point3d). ");
432  //BIASABORT;
433  Vector3<double> euc;
434  int res = Triangulate(C1, Q1, C2, Q2, p1, p2, euc);
435  point3d.Set(euc);
436  return res;
437 }
438 
439 
440 int Triangulation::
441 Triangulate(const Vector3<double>& C1, const Quaternion<double>& Q1,
442  const Vector3<double>& C2, const Quaternion<double>& Q2,
443  const HomgPoint2D& p1, const HomgPoint2D& p2,
444  Vector3<double>& point3d)
445 {
446  Vector3<double> c1(C1), c2(C2), normray1, normray2;
447 
448  normray1 = Q1.MultVec(p1);
449  normray2 = Q2.MultVec(p2);
450  normray1 /= normray1.NormL2();
451  normray2 /= normray2.NormL2();
452 
453  return Triangulation::Intersect(c1, normray1, c2, normray2, point3d);
454 }
455 
456 
457 int Triangulation::
458 Triangulate(const Pose& P1, const Pose& P2, const HomgPoint2D& p1,
459  const HomgPoint2D& p2, HomgPoint3D& point3d)
460 {
461  BIASWARNONCE("Triangulate(const Pose &P1, const Pose &P2, const HomgPoint2D& p1, const HomgPoint2D& p2, HomgPoint3D& point3d) is deprecated, \n please use Triangulate(const Pose &P1, const Pose &P2, const HomgPoint2D& p1, const HomgPoint2D& p2, Vector3<double>& point3d). ");
462  //BIASABORT;
463  Vector3<double> euc;
464  int res = Triangulate(P1, P2, p1, p2, euc);
465  point3d.Set(euc);
466  return res;
467 }
468 
469 
470 int Triangulation::
471 Triangulate(const Pose& P1, const Pose& P2, const HomgPoint2D& p1,
472  const HomgPoint2D& p2, Vector3<double>& point3d)
473 {
474  int res = 0;
475  Vector3<double> normray1, center1, normray2, center2;
476  BCDOUT(D_TR_RAY, "point = 1: "<<p1<<"\t"<<p2<<endl);
477 
478  BCDOUT(D_TR_RAY, "P1: "<<P1<<"\tP2: "<<P2<<endl);
479  Matrix3x3<double> P1I, P2I;
480  P1I = P1.GetR();
481  P2I = P2.GetR();
482 
483  BCDOUT(D_TR_RAY, "P1I: "<<P1I<<"\tP2I: "<<P2I<<endl);
484  normray1 = P1I * p1;
485  normray1 /= normray1.NormL2();
486  normray2 = P2I * p2;
487  normray2 /= normray2.NormL2();
488 
489  HomgPoint3D c1, c2;
490  center1 = P1.GetC();
491  center2 = P2.GetC();
492 
493  Vector3<double> vecX;
494  BCDOUT(D_TR_RAY, "center/normray = 1: "<<center1<<"/"<<normray1
495  <<", 2: "<<center2<<"/"<<normray2<<endl);
496  res = Triangulation::Intersect(center1, normray1, center2, normray2, vecX);
497  point3d[0] = vecX[0];
498  point3d[1] = vecX[1];
499  point3d[2] = vecX[2];
500 
501  return res;
502 }
503 
504 
505 int Triangulation::
506 TriangulateProjective(const ProjectionParametersBase *P1,
507  const ProjectionParametersBase *P2,
508  const HomgPoint2D& p1, const HomgPoint2D& p2,
509  HomgPoint3D& point3d)
510 {
511  Vector3<double> euc, pos;
512  int res = Triangulate(P1, P2, p1, p2, euc);
513  if (res == TRIANG_RES_PARALLEL){ // lines are parallel, point is at infinity
514  P1->UnProjectToRay(p1, pos, euc);
515  point3d.Set(euc[0], euc[1], euc[2], 0.);
516  } else {
517  point3d.Set(euc);
518  }
519 
520  return res;
521 }
522 
523 
524 int Triangulation::
525 Triangulate(const ProjectionParametersBase *P1,
526  const ProjectionParametersBase *P2,
527  const HomgPoint2D& p1, const HomgPoint2D& p2,
528  HomgPoint3D& point3d)
529 {
530  BIASWARNONCE("Triangulate(const ProjectionParametersBase *P1, const ProjectionParametersBase *P2, const HomgPoint2D& p1, const HomgPoint2D& p2, HomgPoint3D& point3d) is deprecated, \n please use Triangulate(const ProjectionParametersBase *P1, const ProjectionParametersBase *P2, const HomgPoint2D& p1, const HomgPoint2D& p2, Vector3<double>& point3d).");
531  //BIASABORT;
532  Vector3<double> euc;
533  int res = Triangulate(P1, P2, p1, p2, euc);
534  point3d.Set(euc);
535  return res;
536 }
537 
538 
539 int Triangulation::
540 Triangulate(const ProjectionParametersBase *P1,
541  const ProjectionParametersBase *P2,
542  const HomgPoint2D& p1, const HomgPoint2D& p2,
543  Vector3<double>& point3d)
544 {
545  int res = 0;
546  Vector3<double> normray1, center1, normray2, center2, pos;
547 
548  center1 = P1->GetC();
549  //normray1 = P1->UnProjectToRay(p1);
550  P1->UnProjectToRay(p1, pos, normray1);
551  if(normray1.NormL2() < 1e-12)
552  return -1;
553  normray1.Normalize();
554  center2 = P2->GetC();
555  //normray2 = P2->UnProjectToRay(p2);
556  P2->UnProjectToRay(p2, pos, normray2);
557  if(normray2.NormL2() < 1e-12)
558  return -1;
559  normray2.Normalize();
560 
561  BCDOUT(D_TR_RAY, "center/normray = 1: "<<center1<<"/"<<normray1
562  <<", 2: "<<center2<<"/"<<normray2<<endl);
563  res = Triangulation::Intersect(center1, normray1, center2, normray2, point3d);
564 
565  return res;
566 }
567 
568 
569 int Triangulation::
570 Triangulate(PMatrix &P1, PMatrix &P2,
571  const HomgPoint2D& p1, const HomgPoint2D& p2,
572  Vector3<double>& point3d)
573 {
574  int res = 0;
575  Vector3<double> normray1, center1, normray2, center2;
576  BCDOUT(D_TR_RAY, "point = p1: " << p1 << "\tp2: " << p2 << endl);
577 
578  BCDOUT(D_TR_RAY, "P1: " << P1 << "\tP2: " << P2 << endl);
579  Matrix3x3<double> P1I, P2I;
580  P1.GetHinf(P1I);
581  P2.GetHinf(P2I);
582  BCDOUT(D_TR_RAY, "P1I: " << P1I << "\tP2I: " << P2I << endl);
583  normray1 = P1I * p1;
584  normray1 /= normray1.NormL2();
585  normray2 = P2I * p2;
586  normray2 /= normray2.NormL2();
587 
588 // P1.GetC(center1);
589 // P2.GetC(center2);
590  HomgPoint3D c1, c2;
591  P1.GetNullVector(c1);
592  P2.GetNullVector(c2);
593  c1.Homogenize();
594  c2.Homogenize();
595  center1.Set(c1[0], c1[1], c1[2]);
596  center2.Set(c2[0], c2[1], c2[2]);
597 
598  BCDOUT(D_TR_RAY, "center/normray = 1: " << center1 << "/" << normray1
599  <<", 2: " << center2 << "/" << normray2 << endl);
600  res = Triangulation::Intersect(center1, normray1, center2, normray2, point3d);
601 
602  return res;
603 }
604 
605 
606 int Triangulation::
607 Triangulate(PMatrix &P1, PMatrix &P2,
608  const HomgPoint2D& p1, const HomgPoint2D& p2,
609  HomgPoint3D& point3d)
610 {
611  BIASWARNONCE("Triangulate(PMatrix& P1, PMatrix& P2, const HomgPoint2D& p1, const HomgPoint2D& p2, HomgPoint3D& point3d) is deprecated, \n please use Triangulate(const PMatrix& P1, PMatrix& P2, const HomgPoint2D& p1, const HomgPoint2D& p2, Vector3<double>& point3d). ");
612  //BIASABORT;
613  Vector3<double> euc;
614  int res = Triangulate(P1, P2, p1, p2, euc);
615  point3d.Set(euc);
616  point3d[3] = 1.0; // ensure that point is homogenized!
617  return res;
618 }
619 
620 
621 int Triangulation::
622 Triangulate2D(PMatrix& P1, PMatrix& P2, HomgPoint2D& p1,
623  HomgPoint2D& p2, HomgPoint3D& point3d,
624  double& dist, double& angle)
625 {
626  int res = 0;
627  BCDOUT(D_TR_ARG, "P1: "<<P1<<"\nP2: "<<P2<<"\np1: "<<p1<<"\tp2: "<<p2<<endl);
628  Vector3<double> ray1, ray2;
629  double gamma, tau;
630  double s1t,s2t;
631  HomgPoint3D p3d1, p3d2;
632  Vector3<double> T;
633  //cerr << "C1: "<< P1.GetC()<< "\tC2: "<< P2.GetC() << endl;
634  //cerr << "p1: "<< p1 << "\tp2: "<< p2 << endl;
635  Vector3<double> C1, C2;
636  if ((P1.GetC(C1) != 0) || (P2.GetC(C2) != 0)){
637  BIASERR("error getting camera center "<<P1<<" "<<C1<<"\n"<<P2<<" "<<C2);
638  return -3;
639  }
640  T = C1 - C2; //camera base line
641  ray1=P1.GetNormRayWorldCoo(p1);
642  ray2=P2.GetNormRayWorldCoo(p2);
643  BCDOUT(D_TR_TR,"center1: "<<C1<<"\t ray1: "<<ray1<<endl);
644  BCDOUT(D_TR_TR,"center2: "<<C2<<"\t ray2: "<<ray2<<endl);
645 
646  // compute different scalar products
647  ray1.ScalarProduct(ray2, angle);
648  ray1.ScalarProduct(T, s1t);
649  ray2.ScalarProduct(T, s2t);
650 
651 // cerr << "( " << setprecision(14) << angle * s2t - s1t <<" ) / ( "
652 // << setprecision(14) << 1 - angle * angle << " )"<<endl;
653  tau = (angle * s2t - s1t)/(1 - angle * angle);
654  gamma = tau * angle + s2t;
655  BCDOUT(D_TR_TR,"T: "<<T<<"\tang: "<<angle<<"\ts1t: "<<s1t<<"\ts2t: "<<s2t
656  <<"\ttau: "<<tau<<"\tgamma: "<<gamma<<endl);
657 
658 // cerr <<"ray1: "<< ray1 << "\tray2: "<<ray2<<"\tT: "<<T<<endl;
659 // cerr <<"angle: "<<angle<<"\ts1t: "<<s1t<<"\ts2t: "<<s2t
660 // <<"\ttau: "<<tau<<"\tgamma: "<<gamma<<"\t"<<(ray1-ray2).NormL2()<<endl;
661 
662  if ( tau < 0 || gamma < 0){
663  // no valid point because intersection point is behind
664  // one camera or behind both cameras
665  BCDOUT(D_TR_POS,"3d point lies behind one or both cameras"<<endl);
666  point3d.Set(0.0, 0.0, 0.0);
667  res = TRIANG_RES_BEHIND_CAMERA;
668  BCDOUT(D_TR_INF, "B"); BIASFLUSHDOUT;
669  } else if ((ray1-ray2).NormL2()<PARALLEL_THRESHOLD) {
670  // no valid point because rays do not intersect
671  BCDOUT(D_TR_POS,"no triangulation possible, rays are parallel"<<endl);
672  point3d.Set(0.0, 0.0, 0.0);
673  res = TRIANG_RES_PARALLEL;
674  BCDOUT(D_TR_INF, "i"); BIASFLUSHDOUT;
675  } else {// maybe a correct point
676  p3d1=C1 + ray1 * tau;
677  p3d2=C2 + ray2 * gamma;
678  dist=(p3d1 - p3d2).NormL2();
679  BCDOUT(D_TR_TR,"p3d1: "<<p3d1<<"\tp3d2: "<<p3d2<<endl);
680  //cerr << "dist: " << dist << endl;
681  point3d=(p3d1+p3d2)/2.0;
682  //cerr <<"p3d: "<<(p3d1+p3d2)/2.0<<endl;
683 // point3d[0] = (P1.GetC()[0] + P2.GetC()[0] + tau * ray1[0] +
684 // gamma * ray2[0]) / 2.0;
685 // point3d[1] = (P1.GetC()[1] + P2.GetC()[1] + tau * ray1[1] +
686 // gamma * ray2[1]) / 2.0;
687 // point3d[2] = (P1.GetC()[2] + P2.GetC()[2] + tau * ray1[2] +
688 // gamma * ray2[2]) / 2.0;
689 // point3d[3] = 1.0;
690  BCDOUT(D_TR_INF, "."); BIASFLUSHDOUT;
691  }
692  return res;
693 }
694 
695 
696 int Triangulation::
697 Triangulate2D(PMatrix& P1, PMatrix& P2, HomgPoint2D& p1,
698  HomgPoint2D& p2, HomgPoint3D& point3D,
699  Matrix3x3<double>& CovMatrix, double& dist,
700  double& angle, const double& triangdeltapixel)
701 {
702  int res=0;
703  if ((res=Triangulate2D(P1, P2, p1, p2, point3D, dist, angle))==0)
704  res=GetCovariance2D(P1, P2, p1, p2, point3D, CovMatrix, triangdeltapixel);
705  return res;
706 }
707 
708 
709 
710 void Triangulation::
711 _UpdateCovWeights(const double &TriangDeltaPixels, const double &sigma)
712 {
713  //AddDebugLevel(D_TRIANG_COVWEIGHT);
714  _dTriangDeltaPixels = TriangDeltaPixels;
715  _dTriangSigmaPixels = sigma;
716 
717  BCDOUT(D_TRIANG_COVWEIGHT, "triangulating in window with "
718  <<TriangDeltaPixels<<" using sigma "<<sigma<<endl);
719  // initialize Gauss-weights
720  for (int y=-1; y <= 1; y++){
721  for (int x=-1; x <= 1; x++) {
722  // construct weight for deviation (0,1,2)
723  cov_weights_[x+1][y+1] =
724  exp(-((((double)y*TriangDeltaPixels)*((double)y*TriangDeltaPixels)
725  +((double)x*TriangDeltaPixels)*((double)x*TriangDeltaPixels))/
726  (2.0*sigma*sigma)));
727 
728  BCDOUT(D_TRIANG_COVWEIGHT, (2.0*sigma*sigma)<<"\t"
729  <<(((double)y*TriangDeltaPixels)*
730  ((double)y*TriangDeltaPixels)+
731  ((double)x*TriangDeltaPixels)*
732  ((double)x*TriangDeltaPixels))<<"\t"
733  <<exp(-((((double)y*TriangDeltaPixels)*
734  ((double)y*TriangDeltaPixels)
735  +((double)x*TriangDeltaPixels)*
736  ((double)x*TriangDeltaPixels))/
737  (2.0*sigma*sigma)))<<endl);
738  /*1.0/(TriangDeltaPixels*sqrt(2.0*M_PI)*sigma)*exp(-(abs(y)+ abs(x))/
739  (2.0*sigma*sigma));*/
740  BCDOUT(D_TRIANG_COVWEIGHT, "weight["<<x+1<<"]["<<y+1<<"] = "
741  <<cov_weights_[x+1][y+1]<<endl);
742  }
743  }
744 }
745 
746 
747 int Triangulation::
748 GetCovariance2D(PMatrix& P1, PMatrix& P2, HomgPoint2D& p1,
749  HomgPoint2D& p2, HomgPoint3D &point3D,
750  Matrix3x3<double>& covariance,
751  const double &triangdeltapixel/*=1.0*/)
752 {
753 #ifdef BIAS_TRIANG_DEBUG
754  covpoints_.clear();
755  covweights_.clear();
756 #endif
757  int n=0, res;
758  //cerr << "point3D: "<<point3D<<endl;
759  p1.Homogenize();
760  p2.Homogenize();
761 
762  if (_dTriangDeltaPixels != triangdeltapixel){
763  _UpdateCovWeights(triangdeltapixel);
764  }
765 
766  covariance.SetZero();
767 
768  // loop over all combinations x around p1[0] and p2[0]
769  // create point cloud of image variance 1 pixel and estimate
770  // 3D covarianve from it
771  HomgPoint3D X; //used in inner loop
772  double sumweight = 0;
773  double dist, angle, wl, weight;
774 
775  HomgPoint2D p3, p4;
776 
777  point3D.Homogenize();
778  // loop around left pixel
779  for (int yl=-1; yl <= 1; yl++)
780  for (int xl=-1; xl <= 1; xl++) {
781  p3.Set(p1[0] + (double)xl*triangdeltapixel,
782  p1[1] + (double)yl*triangdeltapixel);
783 
784  wl = cov_weights_[xl+1][yl+1];
785 
786  // loop around right pixel
787  for (int yr=-1; yr <= 1; yr++)
788  for (int xr=-1; xr <= 1; xr++) {
789  // construct total weight
790  weight =wl*cov_weights_[xr+1][yr+1];
791 
792  p4.Set(p2[0] + (double)xr*triangdeltapixel,
793  p2[1] + (double)yr*triangdeltapixel);
794 
795  res = Triangulate2D(P1, P2, p3, p4, X, dist, angle);
796 
797  if (res == 0) {
798  X.Homogenize();
799  n++;
800 #ifdef BIAS_TRIANG_DEBUG
801  covpoints_.push_back(X);
802  covweights_.push_back(weight);
803 #endif
804  //cerr <<X<<" \t X / point3D "<< X[0] / point3D[0]<< " " <<
805  // X[1] / point3D[1]<< " " << X[2] / point3D[2]<< " " <<dist<<endl;
806  // now Outer product
807  for (unsigned int i=0;i<3;i++) {
808  for (unsigned int j=0;j<3;j++) {
809  covariance[i][j] +=
810  ((X - point3D)[i]) * ((X - point3D)[j]) * weight ;
811  }
812  }
813  sumweight += weight;
814  } else {
815  BCDOUT(D_TR_INF, "B"); BIASFLUSHDOUT;
816  }
817  }
818  }
819  // cerr << endl;
820  // normalize Cov
821  if (sumweight!=0){
822  covariance = covariance*(1.0/sumweight);
823  // cerr << num_weights<<" weights, sum="<<sumweight<<endl;
824  // for (int i=0; i<3; i++)
825  // cerr << i<<": " << covariance[i][i] << " ";
826  // cerr <<endl;
827  //cerr << "point3D: "<<point3D<<" calculated cov from " << n << " points " << endl;
828  return 0;
829  } else {
830  return -4;
831  }
832 }
833 
834 
835 #ifdef BIAS_TRIANG_DEBUG
836 void Triangulation::
837 GetCovPoints(vector<HomgPoint3D>& pvec, vector<double>& weight)
838 {
839  vector<HomgPoint3D>::iterator pit;
840  vector<double>::iterator dit;
841  for (dit=covweights_.begin(), pit=covpoints_.begin(); pit!=covpoints_.end();
842  pit++, dit++){
843  pvec.push_back((*pit));
844  weight.push_back((*dit));
845  };
846 }
847 #endif
848 
849 
850 int Triangulation::
851 GetCovarianceAnalytic(PMatrix& P1, PMatrix& P2, HomgPoint2D& p1,
852  HomgPoint2D& p2, HomgPoint3D &point3D,
853  Matrix3x3<double>& covariance,
854  const double &triangdeltapixel,
855  const double &sigma)
856 {
857  // for performance reasons, we dont want to homogenize each time
858  // therefore rely on homogenized points and check in debug mode
859 #ifdef BIAS_DEBUG
860  if (!p1.IsHomogenized()) {
861  BIASERR("Supplied unhomogenized point p1 for covariance computation !"<<
862  " Fix this, since this is only checked in DEBUG mode. Aborting.");
863  BIASABORT;
864  }
865  if (!p2.IsHomogenized()) {
866  BIASERR("Supplied unhomogenized point p2 for covariance computation !"<<
867  " Fix this, since this is only checked in DEBUG mode. Aborting.");
868  BIASABORT;
869  }
870  if (!point3D.IsHomogenized()) {
871  BIASERR("Supplied unhomogenized 3d point for covariance computation !"<<
872  " Fix this, since this is only checked in DEBUG mode. Aborting.");
873  BIASABORT;
874  }
875 #endif
876 
877 #ifdef BIAS_TRIANG_DEBUG
878  covpoints_.clear();
879  covweights_.clear();
880 #endif
881 
882  if ((_dTriangDeltaPixels != triangdeltapixel)
883  || (_dTriangSigmaPixels != sigma)) {
884  _UpdateCovWeights(triangdeltapixel,sigma);
885  }
886 
887  covariance.SetZero();
888 
889  // loop over all combinations x around p1[0] and p2[0]
890  // create point cloud of image variance 1 pixel and estimate
891  // 3D covarianve from it
892  Vector3<double> X; //used in inner loop
893  double sumweight = 0;
894 
895  // cout << "cov_weights_:"<<cov_weights_ << endl;
896  HomgPoint2D p3, p4;
897 
898  // loop around left pixel
899  for (int yl=-1; yl <= 1; yl++)
900  for (int xl=-1; xl <= 1; xl++) {
901  p3.Set(p1[0] + (double)xl*triangdeltapixel,
902  p1[1] + (double)yl*triangdeltapixel);
903 
904  double wl = cov_weights_[xl+1][yl+1];
905 
906  // loop around right pixel
907  for (int yr=-1; yr <= 1; yr++)
908  for (int xr=-1; xr <= 1; xr++) {
909 
910 
911  p4.Set(p2[0] + (double)xr*triangdeltapixel,
912  p2[1] + (double)yr*triangdeltapixel);
913 
914  //static int count=0;
915  //cout << setw(3)<<count<<" : "<<p2<<" "<<p3<<endl;
916  //count++;
917  if (Triangulate(P1, P2, p3, p4, X)==0) {
918  // construct total weight
919  register double weight = wl*cov_weights_[xr+1][yr+1];
920  // n++;
921 #ifdef BIAS_TRIANG_DEBUG
922  covpoints_.push_back(BIAS::HomgPoint3D(X));
923  covweights_.push_back(weight);
924 #endif
925  //cerr << "p3: " << p3 << "\tp4: " << p4 << "\tX "
926  // << X << "\tweight " << weight << endl;
927 
928 
929  // optimized outer product implementation does this (I hope so):
930  // for (unsigned int i=0;i<3;i++)
931  // for (unsigned int j=0;j<3;j++) {
932  // covariance[i][j] +=
933  // (X[i] - point3D[i]) * (X[j] - point3D[j]) * weight ;
934  // }
935  register double DiffVec[3]; double tmp;
936  // unrolled loop to compute diagonal elements:
937  DiffVec[0] = X[0]-point3D[0];
938  covariance[0][0] += DiffVec[0] * DiffVec[0] * weight;
939  DiffVec[1] = X[1]-point3D[1];
940  covariance[1][1] += DiffVec[1] * DiffVec[1] * weight;
941  DiffVec[2] = X[2]-point3D[2];
942  covariance[2][2] += DiffVec[2] * DiffVec[2] * weight;
943 
944  // non-diag elements of symmetric matrix:
945  covariance[0][1] += tmp = DiffVec[0] * DiffVec[1] * weight ;
946  covariance[1][0] += tmp;
947  covariance[0][2] += tmp = DiffVec[0] * DiffVec[2] * weight ;
948  covariance[2][0] += tmp;
949  covariance[2][1] += tmp = DiffVec[2] * DiffVec[1] * weight ;
950  covariance[1][2] += tmp;
951 
952  sumweight += weight;
953  }
954  }
955  }
956  // cerr << endl;
957  // normalize Cov
958  if (sumweight != 0){
959  //cout << "covariance "<<covariance<<endl;
960  covariance = covariance*(1.0/sumweight);
961  // for (int i=0; i<3; i++)
962  // cerr << i<<": " << covariance[i][i] << " ";
963  // cerr <<endl;
964  //cerr << "point3D: "<<point3D<<" calculated cov from " << n << " points " << endl;
965  //cout << "covariance calculation succeeded ("<<sumweight<<")"<<covariance<<endl;
966  return 0;
967  } else {
968  //cout << "covariance calculation failed"<<endl;
969  return -1;
970  }
971 }
972 
973 
974 Vector3<double> Triangulation::
975 Intersect(Vector3<double> &a1, Vector3<double> &a2,
976  Vector3<double> &b1, Vector3<double> &b2)
977 {
978  Vector3<double> dirA=a2-a1, dirB=b2-b1; // direction vectors
979 #ifdef BIAS_DEBUG
980  if ( (dirA.NormL2()<=DBL_EPSILON) || (dirB.NormL2()<=DBL_EPSILON) )
981  BIASERR("no valid intersection because of identical points");
982 #endif
983  dirA.Normalize();
984  dirB.Normalize();
985  Vector3<double> res;
986  Triangulation::Intersect(a1,dirA, b1,dirB, res);
987  return res;
988 }
989 
990 
991 int Triangulation::
992 GetCovariance(PMatrix& P1, PMatrix& P2, const HomgPoint2D& p1,
993  const Matrix2x2<double>& cov1, const HomgPoint2D& p2,
994  const Matrix2x2<double>& cov2, HomgPoint3D& p,
995  Matrix3x3<double>& cov, const int num, bool quasiEuclidean)
996 {
997  int res=0;
998  Random ran((unsigned (time(NULL))));
999  BIASASSERT(!p1.IsAtInfinity());
1000  BIASASSERT(!p2.IsAtInfinity());
1001  BIASASSERT(p1.IsHomogenized());
1002  BIASASSERT(p2.IsHomogenized());
1003 
1004  Vector2<double> vp1(p1[0], p1[1]), vp2(p2[0], p2[1]);
1005  vector<Vector2<double> > ran1, ran2;
1006 
1007  if (ran.GetNormalDistributed(vp1, cov1, num, ran1)!=0){
1008  BIASERR("invalid cov1 "<<cov1<<endl);
1009  res=-1;
1010  }
1011  if (ran.GetNormalDistributed(vp2, cov2, num, ran2)!=0){
1012  BIASERR("invalid cov2 "<<cov2<<endl);
1013  res=-2;
1014  }
1015  if (res!=0) return res;
1016 
1017 #ifdef BIAS_DEBUG_T
1018  cout << "input1: "<<vp1<<"\n"<<cov1<<endl;
1019  Vector2<double> mr1(0.0, 0.0), mr2(0.0, 0.0);
1020  //cout << "ran1: ";
1021  for (int i=0; i<num; i++){
1022  //cout << ran1[i]<<" ";
1023  mr1+=ran1[i];
1024  }
1025  mr1/=(double)num;
1026  Matrix2x2<double> theCov(MatrixZero);
1027  for (int i=0; i<num; i++){
1028  //cout << ran1[i]<<" ";
1029  theCov += (ran1[i]-mr1).OuterProduct(ran1[i]-mr1);
1030  }
1031  theCov /= double(num-1);
1032  cout << " mean1 "<<mr1<<" cov1="<<theCov<<endl;
1033 
1034  cout << "input2: "<<vp2<<"\n"<<cov2<<endl;
1035  //cout << "ran2: ";
1036  for (int i=0; i<num; i++){
1037  //cout << ran2[i]<<" ";
1038  mr2+=ran2[i];
1039  }
1040  theCov.SetZero();
1041  mr2/=(double)num;
1042  for (int i=0; i<num; i++){
1043  //cout << ran2[i]<<" ";
1044  theCov += (ran2[i]-mr2).OuterProduct(ran2[i]-mr2);
1045  }
1046  theCov /= double(num-1);
1047  cout << " mean2 "<<mr2<<" cov2="<<theCov<<endl;
1048 #endif
1049 
1050 
1051  HomgPoint2D tp1, tp2;
1052  vector<HomgPoint3D> vec3d;
1053  vec3d.reserve(num*num);
1054  Vector3<double> eucl3d;
1055  HomgPoint3D tp3d;
1056 
1057  p.Set(0.0, 0.0, 0.0, 0.0);
1058 
1059  int i, j;
1060  for (i=num-1; i>=0; i--){
1061  for (j=num-1; j>=0; j--){
1062  tp1.Set(ran1[i][0], ran1[i][1]);
1063  tp2.Set(ran2[j][0], ran2[j][1]);
1064 
1065  int res = 0;
1066  if(quasiEuclidean){
1067  res = TriangulateLinear(P1, P2, tp1, tp2, tp3d);
1068  } else {
1069  res = Triangulate(P1, P2, tp1, tp2, eucl3d);
1070  }
1071 
1072  if (res==0) {
1073  if(!quasiEuclidean) tp3d.Set(eucl3d);
1074  // implicitly count number of successfull triangulation in
1075  // w component of p
1076  p+=tp3d;
1077  //cout<<"tp3d "<<tp3d<<endl;
1078  vec3d.push_back(tp3d);
1079  } else {
1080  //cout<<"INF";
1081  }
1082  }
1083  }
1084 
1085  int num_successfull = (int)rint(p[3]);
1086  BIASASSERT(num_successfull==(int)vec3d.size()); // just for debugging
1087 
1088  // get mean by dividing by w component of p. It contains the number of
1089  // successfull triangulations
1090  p.Homogenize();
1091 #ifdef BIAS_DEBUG_T
1092  if(quasiEuclidean){
1093  res = TriangulateLinear(P1, P2, p1, p2, tp3d);
1094  } else {
1095  res = Triangulate(P1, P2, p1, p2, eucl3d)
1096  }
1097  if (res !=0) {
1098  BIASERR("failed");
1099  }
1100  cout<<"mean of cloud is "<<p<<endl;
1101  if(!quasiEuclidean)
1102  cout<<"exact is "<<eucl3d<<endl;
1103  else
1104  cout<<"exact is "<<tp3d<<endl;
1105 #endif
1106 
1107  // now calculate variance
1108  cov.SetZero();
1109  vector<HomgPoint3D>::iterator it;
1110  register double dx, dy, dz;
1111  for (it=vec3d.begin(); it!=vec3d.end(); it++){
1112  dx = p[0]-(*it)[0];
1113  dy = p[1]-(*it)[1];
1114  dz = p[2]-(*it)[2];
1115 
1116  cov[0][0] += dx*dx;
1117  cov[1][1] += dy*dy;
1118  cov[2][2] += dz*dz;
1119  cov[0][1] += dx*dy;
1120  cov[0][2] += dx*dz;
1121  cov[1][2] += dy*dz;
1122  }
1123  cov[1][0] = cov[0][1];
1124  cov[2][0] = cov[0][2];
1125  cov[2][1] = cov[1][2];
1126 
1127  if (num_successfull>1) {
1128  cov /= double(num_successfull-1);
1129  }
1130  res = num_successfull;
1131 
1132  return res;
1133 }
1134 
1135 int Triangulation::
1136 GetCovarianceProjective(PMatrix& P1, PMatrix& P2,
1137  const HomgPoint2D& p1, const HomgPoint2DCov& cov1,
1138  const HomgPoint2D& p2, const HomgPoint2DCov& cov2,
1139  HomgPoint3D& p, HomgPoint3DCov& cov) {
1140 
1141  // duplication matrix for symmetric 4x4 matrices
1142  Matrix<double> D(16, 10);
1143  D.SetZero();
1144  D[0][0] = 1;
1145  D[1][1] = 1;
1146  D[2][3] = 1;
1147  D[3][6] = 1;
1148  D[4][1] = 1;
1149  D[5][2] = 1;
1150  D[6][4] = 1;
1151  D[7][7] = 1;
1152  D[8][3] = 1;
1153  D[9][4] = 1;
1154  D[10][5] = 1;
1155  D[11][8] = 1;
1156  D[12][6] = 1;
1157  D[13][7] = 1;
1158  D[14][8] = 1;
1159  D[15][9] = 1;
1160 
1161  // build A
1162  Matrix<double> A(12, 10);
1163  Vector<double> row1, row2, row3, kron1, rowA;
1164  Vector<double> b(12);
1165  vector<HomgPoint2DCov> covs2D;
1166  covs2D.push_back(cov1);
1167  covs2D.push_back(cov2);
1168  vector<PMatrix> pmatrices;
1169  pmatrices.push_back(P1);
1170  pmatrices.push_back(P2);
1171  for (unsigned int i = 0; i < 2; i++) {
1172 
1173  // get rows of pmatrix i
1174  row1 = pmatrices[i].GetRow(0);
1175  row2 = pmatrices[i].GetRow(1);
1176  row3 = pmatrices[i].GetRow(2);
1177 
1178  // compute rows of matrix A via kronecker product and vech
1179  // (P1 x P1T)D4
1180  row1.KroneckerProduct(row1, kron1);
1181  D.MultLeft(kron1, rowA);
1182  A.SetRow(i*6, rowA);
1183 
1184  // (P1 x P2T)D4
1185  row2.KroneckerProduct(row1, kron1);
1186  D.MultLeft(kron1, rowA);
1187  A.SetRow(i*6+1, rowA);
1188 
1189  // (P2 x P2T)D4
1190  row2.KroneckerProduct(row2, kron1);
1191  D.MultLeft(kron1, rowA);
1192  A.SetRow(i*6+2, rowA);
1193 
1194  // (P3 x P1T)D4
1195  row1.KroneckerProduct(row3, kron1);
1196  D.MultLeft(kron1, rowA);
1197  A.SetRow(i*6+3, rowA);
1198 
1199  // (P2 x P3T)D4
1200  row3.KroneckerProduct(row2, kron1);
1201  D.MultLeft(kron1, rowA);
1202  A.SetRow(i*6+4, rowA);
1203 
1204  // (P3 x P3T)D4
1205  row3.KroneckerProduct(row3, kron1);
1206  D.MultLeft(kron1, rowA);
1207  A.SetRow(i*6+5, rowA);
1208 
1209  // construct vector b out of 2D covs
1210  b[0+(i*6)] = (covs2D[i])[0][0];
1211  b[1+(i*6)] = (covs2D[i])[1][0];
1212  b[2+(i*6)] = (covs2D[i])[1][1];
1213  b[3+(i*6)] = (covs2D[i])[2][0];
1214  b[4+(i*6)] = (covs2D[i])[2][1];
1215  b[5+(i*6)] = (covs2D[i])[2][2];
1216 
1217  } // end for each P build A part
1218 
1219 // cout << "matrix A " << A << endl;
1220 // cout << "vector b " << b << endl;
1221  // compute SVD from A to generate cov
1222  BIAS::SVD A_SVD(A);
1223 
1224  BIAS::Matrix<double> U, VT;
1225  BIAS::Matrix<double> S;
1226  BIAS::Vector<double> s;
1227 
1228  // solve equation system and compute least squares
1229  VT = A_SVD.GetVT();
1230 
1231 // cout << "singular values of A " << A_SVD.GetS() << endl;
1232 
1233  Matrix<double> A_inv(10,12);
1234  A_inv = A_SVD.Invert();
1235 
1236  Vector<double> res = A_inv * b;
1237 
1238  // get quadrics for each PMatrix and enforce rank 3
1239  cov[0][0] = res[0];
1240  cov[0][1] = cov[1][0] = res[1];
1241  cov[1][1] = res[2];
1242  cov[0][2] = cov[2][0] = res[3];
1243  cov[1][2] = cov[2][1] = res[4];
1244  cov[2][2] = res[5];
1245  cov[0][3] = cov[3][0] = res[6];
1246  cov[1][3] = cov[3][1] = res[7];
1247  cov[2][3] = cov[3][2] = res[8];
1248  cov[3][3] = res[9];
1249 
1250 
1251  // triangulate point using optimal method
1252  // deconst 2D points
1253  HomgPoint2D p1Deconst = HomgPoint2D(p1);
1254  HomgPoint2D p2Deconst = HomgPoint2D(p2);
1255  if(Optimal(P1, P2, p1Deconst, p2Deconst, p)){
1256  return -1;
1257  }
1258 
1259  return 0;
1260 }
1261 
1262 int Triangulation::
1263 Intersect(Vector3<double> &pA, Vector3<double> &dirA, Vector3<double> &pB,
1264  Vector3<double> &dirB, Vector3<double> &res)
1265 {
1266  //AddDebugLevel(D_TR_INTERSECT);
1267 #ifdef BIAS_DEBUG
1268  if ( (dirA.NormL2()<=DBL_EPSILON) || (dirB.NormL2()<=DBL_EPSILON) ) {
1269  BIASERR("no valid intersection because direction vector has length zero");
1270  return -2;
1271  }
1272 #endif
1273 
1274  // get planes through origin, pA and dirA (also for B)
1275  Vector3<double> momA, momB, x;
1276  pA.CrossProduct(dirA,momA); //momA = a1 x dirA;
1277  pB.CrossProduct(dirB,momB); //momB = b1 x dirB;
1278 
1279  // check whether dirA and dirB conincide
1280  dirA.CrossProduct(dirB,res);
1281  register double dummy,lambda;
1282  dummy = res.NormL2();
1283  lambda = dummy * dummy;
1284 
1285  if (lambda<=DBL_EPSILON) {
1286 #ifdef BIAS_DEBUG
1287  // BIASERR("cant intersect, lines are parallel");
1288 #endif
1289  BCDOUT(D_TR_INTERSECT, "cant intersect, lines are parallel");
1290  res[0]=0;res[1]=0;res[2]=0;
1291  return TRIANG_RES_PARALLEL;
1292  }
1293 
1294  // old code, slow but possibly more readable:
1295  // res= ( (res * momB) -
1296  // ( (dirA * dirB) * (x * momA)) )
1297  // ) /lambda
1298  // ) * dirA + dirA.CrossProduct(momA);
1299 
1300  dummy = ( (res * momB) - ( (dirA * dirB) * (res * momA)) ) / lambda;
1301  dirA.Multiply(dummy,res);
1302  dirA.CrossProduct(momA,x);
1303  res += x;
1304 
1305  // res is the point with minimum distance on line A
1306  // now get the corresponding point on B:
1307  lambda = (res-pB)*dirB;
1308  x = pB + lambda*dirB;
1309 
1310  // set intersection as the point in the middle of x and res
1311  res = 0.5*(res+x);
1312 
1313  // if lambda<0 the point is in negative dirB,
1314  // that is, for dirB=optical axis: point is behind camera B
1315  if (lambda <0.0 ) {
1316  BCDOUT(D_TR_INTERSECT, "point "<<res<<" is behind camera B: cen: "
1317  <<pB<<" dir:"<<dirB<<" (lambda = "<<lambda<<")\n");
1318  return TRIANG_RES_BEHIND_CAMERA1;
1319  }
1320  // check if point is behind camera A
1321  if (dirA.ScalarProduct(res-pA)<0.0) {
1322  BCDOUT(D_TR_INTERSECT, "point "<<res<<" is behind camera A: cen: "
1323  <<pA<<" dir:"<<dirA<<" scp = "<<dirA.ScalarProduct(res-pA)
1324  <<")\n");
1325  return TRIANG_RES_BEHIND_CAMERA2;
1326  }
1327 
1328  return 0;
1329 }
1330 
1331 
1332 int Triangulation::
1333 Intersect(Vector3<double> &pA, Vector3<double> &dirA, Vector3<double> &pB,
1334  Vector3<double> &dirB, Vector3<double> &res, double &dist)
1335 {
1336 #ifdef BIAS_DEBUG
1337  if ( (dirA.NormL2()<=DBL_EPSILON) || (dirB.NormL2()<=DBL_EPSILON) ) {
1338  BIASERR("no valid intersection because direction vector has length zero");
1339  return -2;
1340  }
1341 #endif
1342  Vector3<double> momA, momB, x;
1343  pA.CrossProduct(dirA,momA); //momA = a1 x dirA;
1344  pB.CrossProduct(dirB,momB); //momA = a1 x dirA;
1345  dirA.CrossProduct(dirB,res);
1346  register double dummy,lambda;
1347  dummy = res.NormL2();
1348  lambda = dummy * dummy;
1349 
1350  if (lambda<=DBL_EPSILON) {
1351 #ifdef BIAS_DEBUG
1352  //BIASERR("cant intersect, lines are parallel");
1353 #endif
1354  res[0]=0;res[1]=0;res[2]=0;
1355  return TRIANG_RES_PARALLEL;
1356  }
1357 
1358  // res= ( (res * momB) -
1359  // ( (dirA * dirB) * (x * momA)) )
1360  // ) /lambda
1361  // ) * dirA + dirA.CrossProduct(momA);
1362  dummy = ( (res * momB) - ( (dirA * dirB) * (res * momA)) ) / lambda;
1363 
1364  dirA.Multiply(dummy,res);
1365  dirA.CrossProduct(momA,x);
1366  res += x;
1367 
1368 
1369  // res is the point with minimum distance on line A
1370  // now get the correpsonding point on B:
1371  lambda=(res-pB)*dirB;
1372  x = pB + lambda*dirB;
1373  dist=(res-x).NormL2();
1374  res = 0.5*(res+x);
1375  if (lambda <0.0 ){
1376  BCDOUT(D_TR_INTERSECT, "point "<<res<<" is behind camera B "
1377  <<pB<<"\t"<<dirB);
1378  return TRIANG_RES_BEHIND_CAMERA1;
1379  }
1380 
1381  // check if point is behind camera A
1382  if (dirA.ScalarProduct(res-pA)<0.0) {
1383  BCDOUT(D_TR_INTERSECT, "point "<<res<<" is behind camera A "
1384  <<pA<<"\t"<<dirA);
1385  return TRIANG_RES_BEHIND_CAMERA2;
1386  }
1387 
1388  return 0;
1389 }
1390 
1391 
1392 
1393 
1394 
BIAS::ProjectionParametersBase::GetC
virtual BIAS::Vector3< double > GetC() const
Get projection center.
Definition: ProjectionParametersBase.hh:464
BIAS::Triangulation::_dTriangDeltaPixels
double _dTriangDeltaPixels
value to go up/down/left/right for &quot;manual&quot; covariance computation
Definition: Triangulation.hh:353
BIAS::CoordinateTransform3D::GetR
BIAS::RMatrixBase GetR() const
Get orientation of local coordinate system as rotation matrix mapping from local coordinates to globa...
Definition: CoordinateTransform3D.hh:246
BIAS::Vector3::Set
void Set(const T *pv)
copy the array of vectorsize beginning at *T to this-&gt;data_
Definition: Vector3.hh:532
BIAS::PolynomialSolve::NonLinearRefine
int NonLinearRefine(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< std::complex< POLYNOMIALSOLVE_TYPE > > &sol)
does a non linear refinement using Powel from minpack for real roots (non imaginary) only ...
Definition: PolynomialSolve.cpp:725
BIAS::HomgPoint2D
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
Definition: HomgPoint2D.hh:67
BIAS::Debug::AddDebugLevel
void AddDebugLevel(const long int lv)
Definition: Debug.hh:355
BIAS::HomgPoint2DCov
class representing the covariance matrix of a homogenous point 2D
Definition: HomgPoint2DCov.hh:40
BIAS::SVD
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
Definition: SVD.hh:92
BIAS::PolynomialSolve::CheckCoefficients
void CheckCoefficients(std::vector< POLYNOMIALSOLVE_TYPE > &coeff, double eps=DBL_EPSILON)
removes leading zeros in coefficients
Definition: PolynomialSolve.cpp:624
BIAS::Triangulation::GetCovarianceAnalytic
int GetCovarianceAnalytic(PMatrix &P1, PMatrix &P2, HomgPoint2D &p1, HomgPoint2D &p2, HomgPoint3D &point3D, Matrix3x3< double > &covariance, const double &triangdeltapixel=TRIANGULATION_DEFAULT_DELTAPIXEL, const double &sigma=TRIANGULATION_DEFAULT_SIGMA)
Calculates and returns the uncertainty as normalized covariance matrix in &#39;covariance&#39;.
Definition: Triangulation.cpp:851
BIAS::Matrix3x3::TransposedMult
void TransposedMult(const Vector3< T > &argvec, Vector3< T > &destvec) const
multiplies matrix from left with transposed argvec, resulting in transposed destvec ...
Definition: Matrix3x3.hh:323
BIAS::Triangulation::GetCovariance2D
int GetCovariance2D(PMatrix &P1, PMatrix &P2, HomgPoint2D &p1, HomgPoint2D &p2, HomgPoint3D &point3D, Matrix3x3< double > &covariance, const double &triangdeltapixel=TRIANGULATION_DEFAULT_DELTAPIXEL)
Calculates and returns the uncertainty as normalized covariance matrix in &#39;covariance&#39;.
Definition: Triangulation.cpp:748
BIAS::PolynomialSolve::Numeric
int Numeric(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< POLYNOMIALSOLVE_TYPE > &sol)
solves polynomial of arbitrary order n numerically coeff[n]x^n+...+coeff[2]x^2+coeff[1]x+coeff[0]=0 c...
Definition: PolynomialSolve.cpp:115
BIAS::Vector3::ScalarProduct
void ScalarProduct(const Vector3< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
Definition: Vector3.hh:603
BIAS::CoordinateTransform3D::GetC
const Vector3< double > & GetC() const
Set origin of local coordinate system in global coordinates.
Definition: CoordinateTransform3D.hh:226
BIAS::HomgPoint3D::Set
void Set(const HOMGPOINT3D_TYPE &x, const HOMGPOINT3D_TYPE &y, const HOMGPOINT3D_TYPE &z)
set elementwise with given 3 euclidean scalar values.
Definition: HomgPoint3D.hh:321
BIAS::HomgPoint2D::Homogenize
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
Definition: HomgPoint2D.hh:215
BIAS::Triangulation::Triangulate2D
int Triangulate2D(PMatrix &P1, PMatrix &P2, HomgPoint2D &p1, HomgPoint2D &p2, HomgPoint3D &point3d, double &dist, double &angle)
does not use BackprojectPseudoInverse but get NormRayWorldCoo
Definition: Triangulation.cpp:622
BIAS::Vector3::Multiply
void Multiply(const T &scalar, Vector3< T > &dest) const
Multiplication with a scalar, storing results in destination vector.
Definition: Vector3.hh:698
BIAS::HomgPoint3D::Homogenize
void Homogenize()
homogenize class data member elements to W==1 by divison by W
Definition: HomgPoint3D.hh:308
BIAS::ProjectionParametersBase::UnProjectToRay
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...
Definition: ProjectionParametersBase.hh:325
BIAS::Quaternion::MultVec
int MultVec(const Vector3< QUAT_TYPE > &vec, Vector3< QUAT_TYPE > &res) const
rotates the given Vector qith the quaternion ( q v q* ) the resulting vector is given in res ...
Definition: Quaternion.hh:136
BIAS::PolynomialSolve
base class for solving polynomial equations
Definition: PolynomialSolve.hh:50
BIAS::Triangulation::CorrectCorrespondences
int CorrectCorrespondences(const FMatrix &F, HomgPoint2D &p1, HomgPoint2D &p2, bool refine=false)
optimize 2d correspondences regarding F for triangulation
Definition: Triangulation.cpp:191
BIAS::PolynomialSolve::EvaluatePolynomial
POLYNOMIALSOLVE_TYPE EvaluatePolynomial(const POLYNOMIALSOLVE_TYPE x, const std::vector< POLYNOMIALSOLVE_TYPE > &coeff) const
numerically robust way to evaluate a polynomial at position x, uses Horner scheme (same principle as ...
Definition: PolynomialSolve.hh:244
BIAS::Matrix::SetRow
void SetRow(const int row, const Vector< T > &data)
set a row of matrix from vector
Definition: Matrix.cpp:265
BIAS::Triangulation::TriangulateProjective
int TriangulateProjective(const ProjectionParametersBase *P1, const ProjectionParametersBase *P2, const HomgPoint2D &p1, const HomgPoint2D &p2, HomgPoint3D &point3d)
Triangulation for metric Poses (using C and R)
Definition: Triangulation.cpp:506
BIAS::Debug::DebugLevelIsSet
bool DebugLevelIsSet(const long int lv) const
Definition: Debug.hh:341
BIAS::Vector4
class Vector4 contains a Vector of dim.
Definition: Vector4.hh:65
BIAS::Pose
Represents 3d pose transformations, parametrized as Euclidean translation and unit quaternion orienta...
Definition: Pose.hh:73
BIAS::Matrix::SetZero
void SetZero()
Sets all values to zero.
Definition: Matrix.hh:856
BIAS::Vector3::CrossProduct
void CrossProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
cross product of two vectors destvec = this x argvec
Definition: Vector3.hh:594
BIAS::Triangulation::GetCovarianceProjective
int GetCovarianceProjective(PMatrix &P1, PMatrix &P2, const HomgPoint2D &p1, const HomgPoint2DCov &cov1, const HomgPoint2D &p2, const HomgPoint2DCov &cov2, HomgPoint3D &p, HomgPoint3DCov &cov)
Definition: Triangulation.cpp:1136
BIAS::HomgPoint2D::IsAtInfinity
bool IsAtInfinity() const
Definition: HomgPoint2D.hh:165
BIAS::SVD::GetVT
const Matrix< double > & GetVT() const
return VT (=transposed(V))
Definition: SVD.hh:177
BIAS::FMatrix
class representing a Fundamental matrix
Definition: FMatrix.hh:46
BIAS::Triangulation::Triangulate
int Triangulate(PMatrix &P1, PMatrix &P2, const HomgPoint2D &p1, const HomgPoint2D &p2, BIAS::Vector3< double > &point3d)
Triangulation for metric PMatrices (using C and Hinf)
Definition: Triangulation.cpp:570
BIAS::Triangulation::TriangulateLinear
int TriangulateLinear(PMatrix &P1, PMatrix &P2, HomgPoint2D &p1, HomgPoint2D &p2, HomgPoint3D &point3d)
Triangulates a 3d point without decomposition of P matrix, use this method in selfcalibration scenari...
Definition: Triangulation.cpp:87
BIAS::PMatrix::GetNullVector
int GetNullVector(HomgPoint3D &C)
this is another way of getting C, especially interesting for non-metric cameras
Definition: PMatrix.cpp:137
BIAS::Matrix::GetRow
Vector< T > GetRow(const int &row) const
return a copy of row &quot;row&quot; of this matrix, zero based counting
Definition: Matrix.cpp:233
BIAS::Debug::NewDebugLevel
long int NewDebugLevel(const std::string &name)
creates a new debuglevel
Definition: Debug.hh:474
BIAS::Triangulation::cov_weights_
Matrix3x3< double > cov_weights_
neighbourhood weights for covariance computation
Definition: Triangulation.hh:351
BIAS::HomgPoint3D
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
Definition: HomgPoint3D.hh:61
BIAS::Triangulation::GetCovariance
int GetCovariance(PMatrix &P1, PMatrix &P2, const HomgPoint2D &p1, const Matrix2x2< double > &cov1, const HomgPoint2D &p2, const Matrix2x2< double > &cov2, HomgPoint3D &p, Matrix3x3< double > &cov, const int num, bool quasiEuclidean=false)
returns an approximation to the 3D point p with uncertainty cov as derived from the 2D points p1 and ...
Definition: Triangulation.cpp:992
BIAS::Triangulation::Optimal
int Optimal(PMatrix &P1, PMatrix &P2, HomgPoint2D &p1, HomgPoint2D &p2, HomgPoint3D &point3d)
method from Hartley Zisserman chapter 12.5 pp 315 first uses CorrectCorrespondences() and then calls ...
Definition: Triangulation.cpp:68
BIAS::HomgPoint3DCov
class representing the covariance matrix of a homogenous point 3D
Definition: HomgPoint3DCov.hh:41
BIAS::Vector::KroneckerProduct
void KroneckerProduct(const Vector< T > &arg, Vector< T > &dst) const
kronecker product
Definition: Vector.cpp:111
BIAS::PMatrix::GetC
int GetC(Vector3< double > &C)
computes translation vector origin world coo -&gt; origin camera coo (center), uses decomposition, which is cached
Definition: PMatrix.cpp:165
BIAS::Matrix3x3::Transpose
Matrix3x3< T > Transpose() const
returns transposed matrix tested 12.06.2002
Definition: Matrix3x3.cpp:167
BIAS::Triangulation::_UpdateCovWeights
void _UpdateCovWeights(const double &TriangDeltaPixels=TRIANGULATION_DEFAULT_DELTAPIXEL, const double &sigma=TRIANGULATION_DEFAULT_SIGMA)
called by constructor to set up neighbourhood weights for covariance computation
Definition: Triangulation.cpp:711
BIAS::PMatrix::GetHinf
int GetHinf(BIAS::Matrix3x3< double > &Hinf)
Definition: PMatrix.cpp:151
BIAS::SVD::Invert
Matrix< double > Invert()
returns pseudoinverse of A = U * S * V^T A^+ = V * S^+ * U^T
Definition: SVD.cpp:214
BIAS::PMatrix::GetNormRayWorldCoo
Vector3< double > GetNormRayWorldCoo(const HomgPoint2D &point)
returns normed vector from C to point analytical inverse of K is used
Definition: PMatrix.cpp:656
BIAS::Matrix::MultLeft
void MultLeft(const Matrix< T > &arg)
in Place matrix multiplication this is equal to M = arg*M, but faster
Definition: Matrix.hh:947
BIAS::FMatrix::GetEpipoles
int GetEpipoles(HomgPoint2D &Epipole1, HomgPoint2D &Epipole2) const
Computes the epipoles from this F Matrix.
Definition: FMatrix.cpp:69
BIAS::RMatrixBase
Implements a 3D rotation matrix.
Definition: RMatrixBase.hh:44
BIAS::Random::GetNormalDistributed
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
BIAS::PMatrix
describes a projective 3D -&gt; 2D mapping in homogenous coordinates
Definition: PMatrix.hh:88
BIAS::HomgPoint3D::IsHomogenized
bool IsHomogenized() const
Definition: HomgPoint3D.hh:147
BIAS::ProjectionParametersBase
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
Definition: ProjectionParametersBase.hh:74
BIAS::Matrix::NormalizeRows
void NormalizeRows()
Normalizes each row to L2 norm one.
Definition: Matrix.cpp:178
BIAS::HomgPoint2D::Set
void Set(const HOMGPOINT2D_TYPE &x, const HOMGPOINT2D_TYPE &y)
set elementwise with given 2 euclidean scalar values.
Definition: HomgPoint2D.hh:174
BIAS::Matrix2x2::SetZero
void SetZero()
set the elements of this matrix to zero
Definition: Matrix2x2.hh:258
BIAS::HomgPoint2D::IsHomogenized
bool IsHomogenized() const
Definition: HomgPoint2D.hh:202
BIAS::Vector3::Normalize
Vector3< T > & Normalize()
normalize this vector to length 1
Definition: Vector3.hh:663
BIAS::Random
class for producing random numbers from different distributions
Definition: Random.hh:51
BIAS::Vector3::NormL2
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
Definition: Vector3.hh:633
BIAS::HomgPoint2D
class BIASGeometryBase_EXPORT HomgPoint2D
Definition: Triangulation.hh:59
BIAS::Triangulation::Intersect
int Intersect(Vector3< double > &pA, Vector3< double > &dirA, Vector3< double > &pB, Vector3< double > &dirB, Vector3< double > &res)
analytic correct intersection of two lines, point returned has minimum distance to both lines...
Definition: Triangulation.cpp:1263
BIAS::Matrix3x3::SetIdentity
void SetIdentity()
set the elements of this matrix to the identity matrix (possibly overriding the inherited method) ...
Definition: Matrix3x3.hh:429
Generated on Tue Feb 6 2018 02:01:37 for Basic Image AlgorithmS Library by   doxygen 1.8.5