1 #include "CamPoseCalib.hh"
4 #include <MathAlgo/Lapack.hh>
5 #include <Geometry/SimilarityTransform.hh>
6 #include <Base/Common/CompareFloatingPoint.hh>
58 ofstream up(
"poseUpVector.log", ios_base::app);
59 ofstream axis(
"poseOptAxis.log", ios_base::app);
60 ofstream transX(
"poseTransX.log", ios_base::app);
61 ofstream transY(
"poseTransY.log", ios_base::app);
62 ofstream transZ(
"poseTransZ.log", ios_base::app);
63 ofstream transAbs(
"poseTransAbs.log", ios_base::app);
68 up<<iteration<<
" "<<angle/M_PI*180.0<<endl;
71 axis<<iteration<<
" "<<angle/M_PI*180.0<<endl;
78 ofstream svdOut(
"poseSingularValues.log",ios_base::app);
79 svdOut<<iteration<<
" ";
83 for (
unsigned int i=0;i<J2.
GetRows(); i++) {
94 if (res==-2)
return -5;
99 BIASERR(
"No points !");
103 for (
unsigned int i=0; i<
origMps_.size(); i++) {
116 for (
unsigned int j=0; j<
initialMps_.size(); j++) {
133 BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_STEPS,
"solveMatrix.size:"<<
137 if ( (
mps_.size() !=
tps_.size() ) ) {
138 BIASERR(
"different number of model points and target points");
145 double transChange=1, rotChange=1,w;
160 BIASDOUT(D_BIAS_CAMPOSE_SECD,
"Second Derivative Matrix s:"<<
secDMatrix_);
166 BIASDOUT(D_BIAS_CAMPOSE_EQSYS,
"solveMatrix:"<<
solveMatrix_<<endl<<
168 BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_STEPS,
"iteration:"<<i<<
"/"<<iterations);
177 for (
unsigned int i=0; i<
weights_.size()*2; i++) {
192 BIASERR(
"number of weights is not equal to nr of correspondences");
211 BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_EQSYS,
"result:"<<result);
221 BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_ITER,
"iteration:"<<i<<
"/"<<iterations
222 <<
" transChange:"<<transChange<<
" rotChange:"<<rotChange
229 BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_ITER,
"too many iterations in CamPoseCalib::Estimate: "<<i
230 <<
" last changes were t="<<transChange<<
" r="<<rotChange);
242 unsigned int col,row2;
247 for (
unsigned int row=0;row<
mps_.size();row++) {
256 BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_STEPS,
"mp:"<<mp<<
" tp:"<<target);
260 matrixRow[col]=
sx_ / mp[2];
268 matrixRow[col]=
sx_* -mp[0]/(mp2mp2);
271 matrixRow[col]=
sx_* -mp[0] * mp[1] / (mp2mp2);
273 matrixRow[col]=
sx_* (mp2mp2 + mp[0]*mp[0])/(mp2mp2);
275 matrixRow[col]=
sx_* -mp[1]/mp[2];
284 matrixRowP1[col]=
sy_ / mp[2];
288 matrixRowP1[col]=
sy_* -mp[1]/(mp2mp2);
291 matrixRowP1[col]=
sy_* (-mp2mp2 - mp[1]*mp[1])/(mp2mp2);
293 matrixRowP1[col]=
sy_* ( mp[1]*mp[0])/(mp2mp2);
295 matrixRowP1[col]=
sy_* mp[0]/mp[2];
300 matrixRow[col] = mp[0]/mp[2];
301 matrixRowP1[col]= mp[1]/mp[2];
304 matrixRow[col] = mp[0]/mp[2];
306 matrixRowP1[col] = mp[1]/mp[2];
313 matrixRowP1[col] = 1;
316 BIASDOUT(D_BIAS_CAMPOSE_EQSYS,
"mp:"<<mp<<
" tp:"<<
tps_[row]<<
317 " point distance to cam center too small:"<<mp[2]);
327 unsigned int col,row2;
337 for (
unsigned int row=0;row<
mpsLine_.size();row++) {
343 diff[0]= target[0] - (mp[0]/mp[2] *
sx_ +
cx_);
344 diff[1]= target[1] - (mp[1]/mp[2] *
sy_ +
cy_);
347 BIASDOUT(D_BIAS_CAMPOSE_ESTIMATE_STEPS,
"mp:"<<mp<<
" tp:"<<target);
348 double *matrixRow = rowEntryX.GetData();
351 matrixRow[col]=
sx_ / mp[2];
359 matrixRow[col]=
sx_* -mp[0]/(mp2mp2);
362 matrixRow[col]=
sx_* -mp[0] * mp[1] / (mp2mp2);
364 matrixRow[col]=
sx_* (mp2mp2 + mp[0]*mp[0])/(mp2mp2);
366 matrixRow[col]=
sx_* -mp[1]/mp[2];
368 double *matrixRowP1 = rowEntryY.GetData();
375 matrixRowP1[col]=
sy_ / mp[2];
379 matrixRowP1[col]=
sy_* -mp[1]/(mp2mp2);
382 matrixRowP1[col]=
sy_* (-mp2mp2 - mp[1]*mp[1])/(mp2mp2);
384 matrixRowP1[col]=
sy_* ( mp[1]*mp[0])/(mp2mp2);
386 matrixRowP1[col]=
sy_* mp[0]/mp[2];
391 matrixRow[col] = mp[0]/mp[2];
392 matrixRowP1[col]= mp[1]/mp[2];
395 matrixRow[col] = mp[0]/mp[2];
397 matrixRowP1[col] = mp[1]/mp[2];
404 matrixRowP1[col] = 1;
410 rowEntryX[j]*targetNormal[0] +
411 rowEntryY[j]*targetNormal[1];
415 BIASDOUT(D_BIAS_CAMPOSE_EQSYS,
"mp:"<<mp<<
" tp:"<<
tps_[row]<<
416 " point distance to cam center too small:"<<mp[2]);
444 BIASDOUT(D_BIAS_CAMPOSE_APPLY_SOL,
"resulting paramter change:"<<result);
447 double alpha=result[row];
448 double beta =result[row+1];
449 double gamma=result[row+2];;
472 R.
SetXYZ(alpha,beta,gamma);
478 resultTrans_[0]+= result[row];
482 resultTrans_[1]+= result[row];
485 if (estimateTransZ_) {
486 resultTrans_[2]+= result[row];
490 BIASDOUT(D_BIAS_CAMPOSE_APPLY_SOL,
"result trans:"<<resultTrans_<<endl
493 for (
unsigned int mpIdx=0;mpIdx<
mps_.size();mpIdx++) {
496 for (
unsigned int mpIdx=0;mpIdx<
mpsLine_.size();mpIdx++) {
548 K[0][0]=
sx_; K[0][2]=
cx_;
549 K[1][1]=
sy_; K[1][2]=
cy_;
565 for (
int i=0; i<3; i++){
577 unsigned int i=0,iteration=0;
580 bool errorIncreased=
true;
582 double fxOrig,fyOrig,cxOrig,cyOrig;
585 std::vector<BIAS::Vector3<double> > mpsOrig,mpsOrigLine;
592 BIASDOUT(D_BIAS_CAMPOSE_SECD,
"Hessian of Gauss-Newton:"<<
coVarMat_);
594 BIASDOUT(D_BIAS_CAMPOSE_SECD,
"Hessian of Newton:"<<
coVarMat_);
597 BIASDOUT(D_BIAS_CAMPOSE_SECD,
"EigValues of Hessian:"<<eigValues);
599 mu_=-5*(eigValues[0]);
604 while ((errorIncreased) && (iteration<iterations) && (
mu_<1E50)) {
606 BIASDOUT( D_BIAS_CAMPOSE_LM,
"solving for mu:"<<
mu_);
608 for (i=0;i<(
unsigned int)J2temp.
num_rows();i++)
629 BIASDOUT( D_BIAS_CAMPOSE_LM,
"result:"<<result);
631 BIASDOUT( D_BIAS_CAMPOSE_LM,
"last SquaredError:"<<
lastError_<<
637 BIASDOUT( D_BIAS_CAMPOSE_LM,
"rotChange Matrix:"<<rotChange);
639 BIASDOUT( D_BIAS_CAMPOSE_LM,
"rotChange="<<angle);
645 errorIncreased=
false;
652 if (errorIncreased) {
682 for (i=0;i<
mps_.size();i++) {
687 err= sqrt(dx*dx + dy*dy);
716 dx = tp[0] - (mp[0]/mp[2] *
sx_ +
cx_);
717 dy = tp[1] - (mp[1]/mp[2] *
sy_ +
cy_);
718 err= fabs(dx*normal[0]+dy*normal[1]);
746 unsigned int i=0,count=0;
749 for (i=0;i<
mps_.size();i++) {
753 dx = tp[0] - (mp[0]/mp[2] *
sx_ +
cx_);
754 dy = tp[1] - (mp[1]/mp[2] *
sy_ +
cy_);
757 diff[0]=dx; diff[1]=dy;
758 err+= diff*(var*diff);
760 err+= sqrt(dx*dx + dy*dy);
771 dx = tp[0] - (mp[0]/mp[2] *
sx_ +
cx_);
772 dy = tp[1] - (mp[1]/mp[2] *
sy_ +
cy_);
773 err+= fabs(dx*normal[0]+dy*normal[1]);
784 unsigned int i=0,count=0;
785 const unsigned int size=
mps_.size();
787 for (i=0; i<size; i++) {
791 dx = tp[0] - (mp[0]/mp[2] *
sx_ +
cx_);
792 dy = tp[1] - (mp[1]/mp[2] *
sy_ +
cy_);
799 for (i=0; i<size; i++) {
803 dx = tp[0] - (mp[0]/mp[2] *
sx_ +
cx_);
804 dy = tp[1] - (mp[1]/mp[2] *
sy_ +
cy_);
818 dx = tp[0] - (mp[0]/mp[2] *
sx_ +
cx_);
819 dy = tp[1] - (mp[1]/mp[2] *
sy_ +
cy_);
820 errNormal= fabs(dx*normal[0]+dy*normal[1]);
821 err+= errNormal*errNormal;
837 for (
unsigned int mpI=0; mpI <
mps_.size(); mpI++) {
853 (-
sx_/mp2mp2 * mp[1]);
855 secDMatrix_[0][4]+= errX *
856 (
sx_/mp2mp2 * mp[0]);
869 secDMatrix_[1][2]+= errY *
872 secDMatrix_[1][3]+= errY *
873 (-
sy_/mp2mp2 * mp[1]);
875 secDMatrix_[1][4]+= errY *
876 (
sy_/mp2mp2 * mp[0]);
883 secDMatrix_[2][0]+= errX *
887 secDMatrix_[2][2]+= errX *
888 (
sx_ * mp[0]*2 / (mp2mp2*mp[2]));
890 secDMatrix_[2][3]+= errX *
891 (
sx_ * mp[0]* mp[1]*2 / (mp2mp2*mp[2]));
893 secDMatrix_[2][4]+= errX *
894 (-
sx_ / mp[2] - 2*
sx_*mp[0]*mp[0] / (mp2mp2*mp[2]));
896 secDMatrix_[2][5]+= errX *
897 (
sx_ *mp[1] / mp2mp2);
903 secDMatrix_[2][1]+= errY *
906 secDMatrix_[2][2]+= errY *
907 (
sy_ * mp[1]*2 / (mp2mp2*mp[2]));
909 secDMatrix_[2][3]+= errY *
910 (
sy_ / mp[2] +
sy_*2*mp[1]*mp[1] / (mp2mp2*mp[2]));
912 secDMatrix_[2][4]+= errY *
913 (-
sy_ * mp[0]* mp[1]*2 / (mp2mp2*mp[2]));
915 secDMatrix_[2][5]+= errY *
916 (-
sy_ * mp[0] / mp2mp2);
922 secDMatrix_[3][0]+= errX *
923 (-
sx_ / mp2mp2 * mp[1]);
926 secDMatrix_[3][2]+= errX *
927 (
sx_ * mp[0]* mp[1]*2 / (mp2mp2*mp[2]));
929 secDMatrix_[3][3]+= errX *
930 (
sx_*mp[0] / mp[2] + 2*
sx_*mp[0]*mp[1]*mp[1] / (mp2mp2*mp[2]));
932 secDMatrix_[3][4]+= errX *
933 (-
sx_ * mp[1]/mp[2]-
sx_*2*mp[0]*mp[0] *mp[1] / (mp2mp2*mp[2]));
935 secDMatrix_[3][5]+= errX *
936 (
sx_/mp2mp2 * (-mp[0]*mp[0] + mp[1]*mp[1]));
941 secDMatrix_[3][1]+= errY *
942 (-
sy_ * mp[1] / mp2mp2);
944 secDMatrix_[3][2]+= errY *
945 (
sy_ /mp[2] + 2*
sy_*mp[1]*mp[1] / (mp2mp2*mp[2]));
947 secDMatrix_[3][3]+= errY *
948 (2*
sy_*( mp[1] / mp[2] + mp[1]*mp[1]*mp[1] / (mp2mp2*mp[2]) ));
950 secDMatrix_[3][4]+= errY *
951 (-2*
sy_*mp[1]*mp[1]*mp[0]/ (mp2mp2*mp[2]));
953 secDMatrix_[3][5]+= errY *
954 (-2*
sy_ * mp[0] *mp[1] / mp2mp2);
960 secDMatrix_[4][0]+= errX *
961 (
sx_*mp[0] / mp2mp2);
964 secDMatrix_[4][2]+= errX *
965 (-
sx_ / mp[2] - 2*
sx_*mp[0]*mp[0] / (mp2mp2*mp[2]));
967 secDMatrix_[4][3]+= errX *
968 (-
sx_*mp[1]/mp[2] - 2*
sx_*mp[0]*mp[0]*mp[1] / (mp[2]*mp2mp2));
970 secDMatrix_[4][4]+= errX *
971 (2*
sx_/mp[2] * (mp[0] + mp[0]*mp[0]*mp[0] / mp2mp2));
973 secDMatrix_[4][5]+= errX *
974 (-2*
sx_*mp[1]*mp[0] / mp2mp2);
980 secDMatrix_[4][1]+= errY *
983 secDMatrix_[4][2]+= errY *
984 (-2*
sy_*mp[0]*mp[1]/(mp2mp2*mp[2]));
986 secDMatrix_[4][3]+= errY *
987 (-2*
sy_*mp[0]*mp[1]*mp[1] / (mp2mp2*mp[2]));
989 secDMatrix_[4][4]+= errY *
990 (
sy_/mp2mp2*(mp[2]*mp[1] + 2*mp[0]*mp[0]*mp[1]/mp[2]));
992 secDMatrix_[4][5]+= errY *
993 (
sy_/mp2mp2*(-mp[1]*mp[1] + mp[0]*mp[0]));
1002 secDMatrix_[5][2]+= errX *
1005 secDMatrix_[5][3]+= errX *
1006 (
sx_/mp2mp2*(-mp[0]*mp[0] + mp[1]*mp[1]));
1008 secDMatrix_[5][4]+= errX *
1009 (-2*
sx_*mp[0]*mp[1] / mp2mp2);
1011 secDMatrix_[5][5]+= errX *
1019 secDMatrix_[5][2]+= errY *
1020 -
sy_*mp[0] / mp2mp2;
1022 secDMatrix_[5][3]+= errY *
1023 (-2*
sy_ * mp[0] *mp[1] / mp2mp2);
1025 secDMatrix_[5][4]+= errY *
1026 (
sy_/mp2mp2* (mp[0]*mp[0] - mp[1]*mp[1]));
1028 secDMatrix_[5][5]+= errY *
1038 BIASERR(
"covariance only for LM estimation yet.");
1047 coVar = sigma2 * svd.
Invert();
1051 for (
int i=0; i<coVar.
num_rows(); i++) {
1052 for (
int j=i; j<coVar.
num_cols(); j++) {
1053 if (BIAS_ISINF(coVar[i][j]) || BIAS_ISNAN(coVar[i][j])){
1057 << endl << coVar << endl;
1059 INFNANCHECK(coVar[i][j]);
1073 for (
unsigned int i=0; i<src.
Size(); i++) {
1074 INFNANCHECK(src[i]);
1087 for (
int r=0; r<3; r++){
1089 for (
int c=0; c<3; c++){
1095 for (
int i=0; i<3; i++) sC[i] = src[i];
1098 if (
Equal(dC[3], 0.0)){
1099 BIASASSERT(
"H5N1" &&
false);
1102 for (
int i=0; i<3; i++) {
1105 INFNANCHECK(dst[i]);
1110 sR.
SetXYZ(src[3], src[4], src[5]);
1115 #include <MathAlgo/UnscentedTransform.hh>
1132 GuessCoo2WoorlCoo g2w(
this);
1136 BIASASSERT(
"H5N1" &&
false);
1139 for (
int i=0; i<gc.
num_rows(); i++) {
1140 for (
int j=i; j<gc.
num_cols(); j++) {
1141 gc[j][i] = gc[i][j] = 0.5*(gc[i][j] + gc[j][i]);
1143 if (BIAS_ISINF(gc[i][j]) || BIAS_ISNAN(gc[i][j])){
1146 INFNANCHECK(gc[i][j]);
1154 for (
unsigned int i=0; i<g.
Size(); i++) {
1158 g2w.Transform(g, gc, w, wc);
1209 double dist=0,newDist;
1214 for (i=0; i<
tps_.size(); i++) {
1215 newDist=(
tps_[i]-c).Length();
1217 i0=
tps_[i]; dist=newDist;
1225 for (i=0; i<
tps_.size(); i++) {
1226 newDist = (
tps_[i]-i0).Length();
1227 if ( newDist > dist) {
1228 i1=
tps_[i]; dist=newDist;
1237 for (i=0; i<
tps_.size(); i++) {
1240 newDist = fabs(diff0*diff1);
1241 if ( newDist > dist) {
1242 i2=
tps_[i]; dist=newDist;
1246 if (index<0)
return -2;
1250 BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,
"found image points:"<<
1251 i0<<
" "<<i1<<
" "<<i2);
1252 BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,
"corresponding obj points:"<<
1253 obj0<<
" "<<obj1<<
" "<<obj2);
1256 double sxy= (
sx_+
sy_) *0.5;
1257 if (
sx_<=0) sxy=600;
1259 iw1(i1[0]-
cx_,i1[1]-
cy_,sxy),
1260 iw2(i2[0]-
cx_,i2[1]-
cy_,sxy), delta_iw0,delta_iw1,delta_iw2;
1263 if (delta_iw0.
NormL2()<1e-10)
return -1;
1265 if (delta_iw2.
NormL2()<1e-10)
return -1;
1267 if (delta_iw1.
NormL2()<1e-10)
return -1;
1269 delta_obj0 = obj1 - obj0;
1270 if (delta_obj0.
NormL2()<1e-10)
return -1;
1272 if (delta_obj2.
NormL2()<1e-10)
return -1;
1274 if (delta_obj1.
NormL2()<1e-10)
return -1;
1276 BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,
"delta Vecs image:"<<
1277 delta_iw0<<
" "<< delta_iw1<<
" "<< delta_iw2<<
1278 endl<<
" and objects:"<<endl<<
1279 delta_obj0<<
" "<< delta_obj1<<
" "<< delta_obj2);
1288 BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,
"normalized delta Vecs image:"<<
1289 delta_iw0<<
" "<< delta_iw1<<
" "<< delta_iw2<<
1290 endl<<
" and objects:"<<endl<<
1291 delta_obj0<<
" "<< delta_obj1<<
" "<< delta_obj2);
1296 BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,
"T_ci*(0,0,0) gives:"<<T_ci*
1301 BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,
"T_co^-1* obj0 gives:"<<T_co*obj0);
1302 finalT = T_ci * T_co;
1304 BIASDOUT(D_BIAS_CAMPOSE_INITGUESS,
"finalT * obj0 gives:"<<finalT*obj0);
1307 double mu = delta_obj0.
Length() / (delta_iw0.
Length() * iw0.Length());
1308 moveT.
SetC(mu * iw0);
1309 finalT = moveT * finalT;
1313 camR_=finalT.GetR();
1315 camC_=finalT.GetC();
1347 solveVector_[row+1]= s10*solveVector_[row] + s11*solveVector_[row+1];
1348 solveVector_[row]=dummyVal;
1353 solveMatrix_[row+1][col]=
1354 s10*solveMatrix_[row][col] + s11* solveMatrix_[row+1][col];
1355 solveMatrix_[row][col]=dummyVal;
std::vector< double > weightsLine_
std::vector< BIAS::Vector2< double > > tpsLineNormal_
double transChangeEpsilon_
std::vector< BIAS::Vector3< double > > initialMpsLine_
std::vector< BIAS::Vector3< double > > origMps_
void Set(const T *pv)
copy the array of vectorsize beginning at *T to this->data_
void SetXYZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order XYZ.
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
Subscript num_cols() const
Vector< double > Solve(const Vector< double > &y) const
double Length() const
returns the Euclidean Length of the Vector
int GetCoVarMatrix(BIAS::Matrix< double > &coVar)
returns the covariance of the last estimate, which is (H=Hessian): H^{-1} * eps^T*eps * 1/(2n-p) The ...
void ScalarProduct(const Vector2< T > &argvec, T &result) const
scalar product of two vectors, storing the result in result
Matrix< T > Transpose() const
transpose function, storing data destination matrix
BIAS::Vector< double > GetResult() const
returns the result vector, corresponding to the covariance matrix
int ApplySolution_(BIAS::Vector< double > &result)
helper function for Estimate
BIAS::Matrix< double > coVarMat_
int Solve_(BIAS::Matrix< double > &J, BIAS::Vector< double > &eps, BIAS::Vector< double > &result)
Matrix< T > & newsize(Subscript M, Subscript N)
BIAS::Vector3< double > GetTranslation() const
this returns the translation from the initial camera center to the new camera center ...
BIAS::Vector< double > solveVector_
BIAS::Vector< double > GetEigenValues()
Call this after Compute(), returns the eigenvalues of the matrix in ascending order (smallest first)...
unsigned int Size() const
length of the vector
std::vector< BIAS::Vector3< double > > mpsLine_
int GetR(Matrix3x3< double > &R)
this classs allows estimation of the internal (except skew) and external camera parameters by an iter...
void SetZero()
equivalent to matrix call
bool doLM_
for LM algorithm extension
double GetAvgError()
returns the avg error of the estimation, which is the average difference of the projected points to t...
BIAS::Vector< double > Lapack_LU_linear_solve(const BIAS::Matrix< double > &A, const BIAS::Vector< double > &b)
solve linear equations using LU factorization
void Reset()
clears the correspondences, such that u can do a new estimate,
void GetP(BIAS::PMatrix &P)
returns the estimated new projection matrix
double GetAvgSquaredWeightedError_()
returns the avg error of the estimation, which is the weighted average squared difference of the proj...
void outputPose_(int iteration)
double Dist(const Vector3< T > &vec) const
Return the euclidean distance of 2 vectors.
class Vector4 contains a Vector of dim.
Vector< T > & newsize(Subscript N)
BIAS::Vector3< double > truthUpVec_
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
std::vector< BIAS::Vector2< double > > tps_
unsigned int GetRows() const
const double GetSingularValue(int index) const
return one singular value (which may be zero).
int TransformVec(const BIAS::Vector< double > &src, BIAS::Vector< double > &dst)
int SetInitialCamera(const BIAS::KMatrix &K)
activates an automatic initial guess for the projection matrix, according to Christian Perwass' habil...
double errorIncreaseForLM_
BIAS::Vector3< double > camC_
BIAS::Matrix< double > solveMatrix_
BIAS::RMatrix GetRotation() const
this return the relative rotation from the initial camera orientation to the new estimated camera rot...
int SolveLM_(BIAS::Matrix< double > &J, BIAS::Vector< double > &eps, BIAS::Vector< double > &result, unsigned int iterations)
int CreateEquationSys_()
helper function for Estimate, creates the Jacobi matrix
BIAS::Vector3< double > resultTrans_
std::vector< BIAS::Vector3< double > > mps_
bool Equal(const T left, const T right, const T eps)
comparison function for floating point values See http://www.boost.org/libs/test/doc/components/test_...
unsigned int GetCols() const
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
std::vector< BIAS::Vector3< double > > origMpsLine_
double scaleNumStabiltity_
int GetRotationAnglesXYZ(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order XYZ.
Matrix< double > Invert()
returns pseudoinverse of A = U * S * V^T A^+ = V * S^+ * U^T
std::vector< BIAS::Vector3< double > > initialMps_
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
Implements a 3D rotation matrix.
std::vector< BIAS::Vector2< double > > tpsLinePoint_
Subscript num_rows() const
int GetRotationAxisAngle(Vector3< ROTATION_MATRIX_TYPE > &axis, ROTATION_MATRIX_TYPE &angle) const
Calculates angle and rotation axis representation for this rotation matrix.
BIAS::Matrix< double > secDMatrix_
describes a projective 3D -> 2D mapping in homogenous coordinates
void SetFromColumnVectors(const BIAS::Vector3< T > &v0, const BIAS::Vector3< T > &v1, const BIAS::Vector3< T > &v2)
set this matrix from 3 vectors each representating a column
int SetWeightsByError_()
for robust estimation
void MakeSymmetric()
componentwise: this = 0.5(this + this^T) yields symmetric matrix only allowed for square shaped matri...
BIAS::Vector3< double > truthOptAxis_
std::vector< double > weights_
void GetSystemMatrix(Matrix< T > &dest) const
compute square system matrix dest = A^T * A
std::vector< BIAS::Matrix2x2< double > > variances_
int AddConstraints2DLine_()
adds rows for 3D-point-2D-point corrs
int AddCorr(const BIAS::Vector3< double > &x, const BIAS::Vector2< double > &y)
add a corresponding point pair
Vector3< T > & Normalize()
normalize this vector to length 1
BIAS::Vector3< double > truthCenter_
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
void GetCov(BIAS::Matrix< double > &cov)
returns the covarinace of the last estimate in the world coo system
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' | -K R' C ] with R' = transpose(R) ...
void SetIdentity()
set the elements of this matrix to the identity matrix (possibly overriding the inherited method) ...
int Estimate(unsigned int iterations=20)
Does the calculation with a certain number of iterations.
int GetK(KMatrix &K)
calibration matrix