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>
76 BCDOUT(D_TR_OPT,
"p1: "<<p1<<
"tp2: "<<p2<<endl);
77 BCDOUT(D_TR_OPT,
"p1' * F * p2 = "<<r.
ScalarProduct(p1)<<endl);
80 BIASERR(
"CorrectCorrespondences failed "<<res);
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];
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];
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];
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];
127 if( pmatrices.size() != homgPoints2D.size() || pmatrices.size() < 2)
129 BIASERR(
"Not enough HomgPoints or PMatrices.");
134 int size = (int)homgPoints2D.size();
135 unsigned int rows = 4 * size * (size - 1) / 2;
140 for (
int i = 0; i < size; i++)
142 for (
int j = 0; j < i; j++)
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];
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];
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];
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];
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)
216 BCDOUT(D_TR_OPT_TRANSF,
"T: "<<T<<
"\nTp: "<<Tp<<
"\nTi: "<<Ti
217 <<
"\nTpi: "<<Tpi<<endl);
221 BCDOUT(D_TR_OPT_TRANSF,
"orig F: "<<F<<
"\nTpi.Transpose() * F * Ti = "
227 BCDOUT(D_TR_OPT_TRANSF,
"orig epipoles: "<<e<<
"\t"<<ep<<endl);
230 BCDOUT(D_TR_OPT_TRANSF,
"epipoles: "<<e<<
"\t"<<ep<<endl);
235 BCDOUT(D_TR_OPT_TRANSF,
"F*e = "<<mF*e<<
"\tep^T*F = "<<r<<endl);
242 se=sqrt(e[0]*e[0]+e[1]*e[1]);
243 sep=sqrt(ep[0]*ep[0]+ep[1]*ep[1]);
248 BCDOUT(D_TR_OPT_TRANSF,
"normalized epipoles: "<<e<<
"\t"<<ep<<endl);
254 R[0][0]=R[1][1]=e[0];
258 Rp[0][0]=Rp[1][1]=ep[0];
262 BCDOUT(D_TR_OPT_TRANSF,
"R*e = "<<R*e<<
"\tRp*ep = "<<Rp * ep<<endl);
266 BCDOUT(D_TR_OPT_TRANSF,
"Rp * Tpi.Transpose() * F * Ti * R.Transpose() = "
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): "
275 <<
"\t(Ti * R.Transpose() * (0,0,1))-p1 ="
277 BCDOUT(D_TR_OPT_TRANSF,
"Tpi * Rp.Transpose() * (0,0,1): "
278 <<Tpi * Rp.
Transpose() * r4<<
"\t(Tpi * Rp.Transpose() * r4)-p2 = "
282 double f, fp, a, b, c, d;
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);
293 BCDOUT(D_TR_OPT,
"a: "<<a<<
"\tb: "<<b<<
"\tc: "<<c<<
"\td: "<<d<<
"\tf: "<<f
294 <<
"\tfp: "<<fp<<endl);
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;
327 coeff[1]=b4 - a2*d2 + b2*c2 + d4*fp4 + 2*b2*d2*fp2;
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;
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;
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;
334 coeff[5]=a4 + c4*fp4 - a2*d2*f4 + b2*c2*f4 + 2*a2*c2*fp2;
335 coeff[6]=ab*c2*f4 - a2*cd*f4;
343 int psres=ps.
Numeric(coeff, csol);
344 BCDOUT(D_TR_OPT,
"csol.size(): "<<csol.size()<<
"\tpsres: "<<psres<<endl);
346 BIASERR(
"error solving polynomial "<<psres);
357 vector<double> s(csol.size()+1);
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;
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);
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);
374 double tmin=real(csol[0]);
380 for (
unsigned i=1; i<s.size(); i++){
381 if (s[i]<costmin) { costmin=s[i]; minres=i; tmin=real(csol[i]); }
384 BCDOUT(D_TR_OPT,
"costmin = "<<costmin<<
"\tat "<<minres<<
"\ttmin: "
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) = "
395 l.
Set(tmin*f, 1.0, -tmin);
396 lp.Set(-fp*(c*tmin+d), a*tmin+b, c*tmin+d);
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]);
406 BCDOUT(D_TR_OPT_F,
"optimized: xhp * mF * xh = "
407 <<(mF*xh).ScalarProduct(xhp)<<endl);
414 BCDOUT(D_TR_OPT,
"optimized points "<<p1<<
" <--> "<<p2<<endl);
417 BCDOUT(D_TR_OPT_F,
"optimized: p2^T * F * p1 = "<<(F*p1).ScalarProduct(p2)
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). ");
434 int res =
Triangulate(C1, Q1, C2, Q2, p1, p2, euc);
450 normray1 /= normray1.
NormL2();
451 normray2 /= normray2.
NormL2();
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). ");
476 BCDOUT(D_TR_RAY,
"point = 1: "<<p1<<
"\t"<<p2<<endl);
478 BCDOUT(D_TR_RAY,
"P1: "<<P1<<
"\tP2: "<<P2<<endl);
483 BCDOUT(D_TR_RAY,
"P1I: "<<P1I<<
"\tP2I: "<<P2I<<endl);
485 normray1 /= normray1.
NormL2();
487 normray2 /= normray2.
NormL2();
494 BCDOUT(D_TR_RAY,
"center/normray = 1: "<<center1<<
"/"<<normray1
495 <<
", 2: "<<center2<<
"/"<<normray2<<endl);
497 point3d[0] = vecX[0];
498 point3d[1] = vecX[1];
499 point3d[2] = vecX[2];
513 if (res == TRIANG_RES_PARALLEL){
515 point3d.
Set(euc[0], euc[1], euc[2], 0.);
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).");
548 center1 = P1->
GetC();
551 if(normray1.
NormL2() < 1e-12)
554 center2 = P2->
GetC();
557 if(normray2.
NormL2() < 1e-12)
561 BCDOUT(D_TR_RAY,
"center/normray = 1: "<<center1<<
"/"<<normray1
562 <<
", 2: "<<center2<<
"/"<<normray2<<endl);
576 BCDOUT(D_TR_RAY,
"point = p1: " << p1 <<
"\tp2: " << p2 << endl);
578 BCDOUT(D_TR_RAY,
"P1: " << P1 <<
"\tP2: " << P2 << endl);
582 BCDOUT(D_TR_RAY,
"P1I: " << P1I <<
"\tP2I: " << P2I << endl);
584 normray1 /= normray1.
NormL2();
586 normray2 /= normray2.
NormL2();
595 center1.
Set(c1[0], c1[1], c1[2]);
596 center2.
Set(c2[0], c2[1], c2[2]);
598 BCDOUT(D_TR_RAY,
"center/normray = 1: " << center1 <<
"/" << normray1
599 <<
", 2: " << center2 <<
"/" << normray2 << endl);
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). ");
624 double& dist,
double& angle)
627 BCDOUT(D_TR_ARG,
"P1: "<<P1<<
"\nP2: "<<P2<<
"\np1: "<<p1<<
"\tp2: "<<p2<<endl);
636 if ((P1.
GetC(C1) != 0) || (P2.
GetC(C2) != 0)){
637 BIASERR(
"error getting camera center "<<P1<<
" "<<C1<<
"\n"<<P2<<
" "<<C2);
643 BCDOUT(D_TR_TR,
"center1: "<<C1<<
"\t ray1: "<<ray1<<endl);
644 BCDOUT(D_TR_TR,
"center2: "<<C2<<
"\t ray2: "<<ray2<<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);
662 if ( tau < 0 || gamma < 0){
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) {
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;
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);
681 point3d=(p3d1+p3d2)/2.0;
690 BCDOUT(D_TR_INF,
"."); BIASFLUSHDOUT;
700 double& angle,
const double& triangdeltapixel)
703 if ((res=
Triangulate2D(P1, P2, p1, p2, point3D, dist, angle))==0)
704 res=
GetCovariance2D(P1, P2, p1, p2, point3D, CovMatrix, triangdeltapixel);
717 BCDOUT(D_TRIANG_COVWEIGHT,
"triangulating in window with "
718 <<TriangDeltaPixels<<
" using sigma "<<sigma<<endl);
720 for (
int y=-1; y <= 1; y++){
721 for (
int x=-1; x <= 1; x++) {
724 exp(-((((
double)y*TriangDeltaPixels)*((
double)y*TriangDeltaPixels)
725 +((
double)x*TriangDeltaPixels)*((
double)x*TriangDeltaPixels))/
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);
740 BCDOUT(D_TRIANG_COVWEIGHT,
"weight["<<x+1<<
"]["<<y+1<<
"] = "
751 const double &triangdeltapixel)
753 #ifdef BIAS_TRIANG_DEBUG
772 double sumweight = 0;
773 double dist, angle, wl, weight;
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);
787 for (
int yr=-1; yr <= 1; yr++)
788 for (
int xr=-1; xr <= 1; xr++) {
792 p4.
Set(p2[0] + (
double)xr*triangdeltapixel,
793 p2[1] + (
double)yr*triangdeltapixel);
800 #ifdef BIAS_TRIANG_DEBUG
801 covpoints_.push_back(X);
802 covweights_.push_back(weight);
807 for (
unsigned int i=0;i<3;i++) {
808 for (
unsigned int j=0;j<3;j++) {
810 ((X - point3D)[i]) * ((X - point3D)[j]) * weight ;
815 BCDOUT(D_TR_INF,
"B"); BIASFLUSHDOUT;
822 covariance = covariance*(1.0/sumweight);
835 #ifdef BIAS_TRIANG_DEBUG
837 GetCovPoints(vector<HomgPoint3D>& pvec, vector<double>& weight)
839 vector<HomgPoint3D>::iterator pit;
840 vector<double>::iterator dit;
841 for (dit=covweights_.begin(), pit=covpoints_.begin(); pit!=covpoints_.end();
843 pvec.push_back((*pit));
844 weight.push_back((*dit));
854 const double &triangdeltapixel,
861 BIASERR(
"Supplied unhomogenized point p1 for covariance computation !"<<
862 " Fix this, since this is only checked in DEBUG mode. Aborting.");
866 BIASERR(
"Supplied unhomogenized point p2 for covariance computation !"<<
867 " Fix this, since this is only checked in DEBUG mode. Aborting.");
871 BIASERR(
"Supplied unhomogenized 3d point for covariance computation !"<<
872 " Fix this, since this is only checked in DEBUG mode. Aborting.");
877 #ifdef BIAS_TRIANG_DEBUG
893 double sumweight = 0;
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);
907 for (
int yr=-1; yr <= 1; yr++)
908 for (
int xr=-1; xr <= 1; xr++) {
911 p4.
Set(p2[0] + (
double)xr*triangdeltapixel,
912 p2[1] + (
double)yr*triangdeltapixel);
921 #ifdef BIAS_TRIANG_DEBUG
923 covweights_.push_back(weight);
935 register double DiffVec[3];
double tmp;
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;
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;
960 covariance = covariance*(1.0/sumweight);
980 if ( (dirA.
NormL2()<=DBL_EPSILON) || (dirB.NormL2()<=DBL_EPSILON) )
981 BIASERR(
"no valid intersection because of identical points");
998 Random ran((
unsigned (time(NULL))));
1005 vector<Vector2<double> > ran1, ran2;
1008 BIASERR(
"invalid cov1 "<<cov1<<endl);
1012 BIASERR(
"invalid cov2 "<<cov2<<endl);
1015 if (res!=0)
return res;
1018 cout <<
"input1: "<<vp1<<
"\n"<<cov1<<endl;
1021 for (
int i=0; i<num; i++){
1027 for (
int i=0; i<num; i++){
1029 theCov += (ran1[i]-mr1).OuterProduct(ran1[i]-mr1);
1031 theCov /= double(num-1);
1032 cout <<
" mean1 "<<mr1<<
" cov1="<<theCov<<endl;
1034 cout <<
"input2: "<<vp2<<
"\n"<<cov2<<endl;
1036 for (
int i=0; i<num; i++){
1042 for (
int i=0; i<num; i++){
1044 theCov += (ran2[i]-mr2).OuterProduct(ran2[i]-mr2);
1046 theCov /= double(num-1);
1047 cout <<
" mean2 "<<mr2<<
" cov2="<<theCov<<endl;
1052 vector<HomgPoint3D> vec3d;
1053 vec3d.reserve(num*num);
1057 p.
Set(0.0, 0.0, 0.0, 0.0);
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]);
1073 if(!quasiEuclidean) tp3d.
Set(eucl3d);
1078 vec3d.push_back(tp3d);
1085 int num_successfull = (int)rint(p[3]);
1086 BIASASSERT(num_successfull==(
int)vec3d.size());
1100 cout<<
"mean of cloud is "<<p<<endl;
1102 cout<<
"exact is "<<eucl3d<<endl;
1104 cout<<
"exact is "<<tp3d<<endl;
1109 vector<HomgPoint3D>::iterator it;
1110 register double dx, dy, dz;
1111 for (it=vec3d.begin(); it!=vec3d.end(); it++){
1123 cov[1][0] = cov[0][1];
1124 cov[2][0] = cov[0][2];
1125 cov[2][1] = cov[1][2];
1127 if (num_successfull>1) {
1128 cov /= double(num_successfull-1);
1130 res = num_successfull;
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++) {
1174 row1 = pmatrices[i].GetRow(0);
1175 row2 = pmatrices[i].GetRow(1);
1176 row3 = pmatrices[i].GetRow(2);
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];
1240 cov[0][1] = cov[1][0] = res[1];
1242 cov[0][2] = cov[2][0] = res[3];
1243 cov[1][2] = cov[2][1] = res[4];
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];
1255 if(
Optimal(P1, P2, p1Deconst, p2Deconst, p)){
1268 if ( (dirA.
NormL2()<=DBL_EPSILON) || (dirB.
NormL2()<=DBL_EPSILON) ) {
1269 BIASERR(
"no valid intersection because direction vector has length zero");
1281 register double dummy,lambda;
1283 lambda = dummy * dummy;
1285 if (lambda<=DBL_EPSILON) {
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;
1300 dummy = ( (res * momB) - ( (dirA * dirB) * (res * momA)) ) / lambda;
1307 lambda = (res-pB)*dirB;
1308 x = pB + lambda*dirB;
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;
1322 BCDOUT(D_TR_INTERSECT,
"point "<<res<<
" is behind camera A: cen: "
1325 return TRIANG_RES_BEHIND_CAMERA2;
1337 if ( (dirA.
NormL2()<=DBL_EPSILON) || (dirB.
NormL2()<=DBL_EPSILON) ) {
1338 BIASERR(
"no valid intersection because direction vector has length zero");
1346 register double dummy,lambda;
1348 lambda = dummy * dummy;
1350 if (lambda<=DBL_EPSILON) {
1354 res[0]=0;res[1]=0;res[2]=0;
1355 return TRIANG_RES_PARALLEL;
1362 dummy = ( (res * momB) - ( (dirA * dirB) * (res * momA)) ) / lambda;
1371 lambda=(res-pB)*dirB;
1372 x = pB + lambda*dirB;
1373 dist=(res-x).NormL2();
1376 BCDOUT(D_TR_INTERSECT,
"point "<<res<<
" is behind camera B "
1378 return TRIANG_RES_BEHIND_CAMERA1;
1383 BCDOUT(D_TR_INTERSECT,
"point "<<res<<
" is behind camera A "
1385 return TRIANG_RES_BEHIND_CAMERA2;
virtual BIAS::Vector3< double > GetC() const
Get projection center.
double _dTriangDeltaPixels
value to go up/down/left/right for "manual" covariance computation
void Set(const T *pv)
copy the array of vectorsize beginning at *T to this->data_
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 ...
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
void AddDebugLevel(const long int lv)
class representing the covariance matrix of a homogenous point 2D
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
void CheckCoefficients(std::vector< POLYNOMIALSOLVE_TYPE > &coeff, double eps=DBL_EPSILON)
removes leading zeros in coefficients
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 'covariance'.
void TransposedMult(const Vector3< T > &argvec, Vector3< T > &destvec) const
multiplies matrix from left with transposed argvec, resulting in transposed destvec ...
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 'covariance'.
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...
void ScalarProduct(const Vector3< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
void Set(const HOMGPOINT3D_TYPE &x, const HOMGPOINT3D_TYPE &y, const HOMGPOINT3D_TYPE &z)
set elementwise with given 3 euclidean scalar values.
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
int Triangulate2D(PMatrix &P1, PMatrix &P2, HomgPoint2D &p1, HomgPoint2D &p2, HomgPoint3D &point3d, double &dist, double &angle)
does not use BackprojectPseudoInverse but get NormRayWorldCoo
void Multiply(const T &scalar, Vector3< T > &dest) const
Multiplication with a scalar, storing results in destination vector.
void Homogenize()
homogenize class data member elements to W==1 by divison by W
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...
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 ...
base class for solving polynomial equations
int CorrectCorrespondences(const FMatrix &F, HomgPoint2D &p1, HomgPoint2D &p2, bool refine=false)
optimize 2d correspondences regarding F for triangulation
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 ...
void SetRow(const int row, const Vector< T > &data)
set a row of matrix from vector
int TriangulateProjective(const ProjectionParametersBase *P1, const ProjectionParametersBase *P2, const HomgPoint2D &p1, const HomgPoint2D &p2, HomgPoint3D &point3d)
Triangulation for metric Poses (using C and R)
bool DebugLevelIsSet(const long int lv) const
class Vector4 contains a Vector of dim.
Represents 3d pose transformations, parametrized as Euclidean translation and unit quaternion orienta...
void SetZero()
Sets all values to zero.
void CrossProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
cross product of two vectors destvec = this x argvec
int GetCovarianceProjective(PMatrix &P1, PMatrix &P2, const HomgPoint2D &p1, const HomgPoint2DCov &cov1, const HomgPoint2D &p2, const HomgPoint2DCov &cov2, HomgPoint3D &p, HomgPoint3DCov &cov)
bool IsAtInfinity() const
const Matrix< double > & GetVT() const
return VT (=transposed(V))
class representing a Fundamental matrix
int Triangulate(PMatrix &P1, PMatrix &P2, const HomgPoint2D &p1, const HomgPoint2D &p2, BIAS::Vector3< double > &point3d)
Triangulation for metric PMatrices (using C and Hinf)
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...
int GetNullVector(HomgPoint3D &C)
this is another way of getting C, especially interesting for non-metric cameras
Vector< T > GetRow(const int &row) const
return a copy of row "row" of this matrix, zero based counting
long int NewDebugLevel(const std::string &name)
creates a new debuglevel
Matrix3x3< double > cov_weights_
neighbourhood weights for covariance computation
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
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 ...
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 ...
class representing the covariance matrix of a homogenous point 3D
void KroneckerProduct(const Vector< T > &arg, Vector< T > &dst) const
kronecker product
int GetC(Vector3< double > &C)
computes translation vector origin world coo -> origin camera coo (center), uses decomposition, which is cached
Matrix3x3< T > Transpose() const
returns transposed matrix tested 12.06.2002
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
int GetHinf(BIAS::Matrix3x3< double > &Hinf)
Matrix< double > Invert()
returns pseudoinverse of A = U * S * V^T A^+ = V * S^+ * U^T
Vector3< double > GetNormRayWorldCoo(const HomgPoint2D &point)
returns normed vector from C to point analytical inverse of K is used
void MultLeft(const Matrix< T > &arg)
in Place matrix multiplication this is equal to M = arg*M, but faster
int GetEpipoles(HomgPoint2D &Epipole1, HomgPoint2D &Epipole2) const
Computes the epipoles from this F Matrix.
Implements a 3D rotation matrix.
double GetNormalDistributed(const double mean, const double sigma)
on succesive calls return normal distributed random variable with mean and standard deviation sigma ...
describes a projective 3D -> 2D mapping in homogenous coordinates
double _dTriangSigmaPixels
bool IsHomogenized() const
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
void NormalizeRows()
Normalizes each row to L2 norm one.
void Set(const HOMGPOINT2D_TYPE &x, const HOMGPOINT2D_TYPE &y)
set elementwise with given 2 euclidean scalar values.
void SetZero()
set the elements of this matrix to zero
bool IsHomogenized() const
Vector3< T > & Normalize()
normalize this vector to length 1
class for producing random numbers from different distributions
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
class BIASGeometryBase_EXPORT HomgPoint2D
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...
void SetIdentity()
set the elements of this matrix to the identity matrix (possibly overriding the inherited method) ...