void binTriangulate(const double K0[9], const double R0[9], const double t0[3], const double K1[9], const double R1[9], const double t1[3], const double m0[2], const double m1[2], double x[3]) { double Q0[9], q0[3], Q1[9], q1[3]; mat33AB(K0, R0, Q0); mat33ProdVec(K0, t0, q0); mat33AB(K1, R1, Q1); mat33ProdVec(K1, t1, q1); double A[12]; double b[4]; A[0] = Q0[0] - m0[0] * Q0[6]; A[1] = Q0[1] - m0[0] * Q0[7]; A[2] = Q0[2] - m0[0] * Q0[8]; A[3] = Q0[3] - m0[1] * Q0[6]; A[4] = Q0[4] - m0[1] * Q0[7]; A[5] = Q0[5] - m0[1] * Q0[8]; A[6] = Q1[0] - m1[0] * Q1[6]; A[7] = Q1[1] - m1[0] * Q1[7]; A[8] = Q1[2] - m1[0] * Q1[8]; A[9] = Q1[3] - m1[1] * Q1[6]; A[10] = Q1[4] - m1[1] * Q1[7]; A[11] = Q1[5] - m1[1] * Q1[8]; b[0] = q0[2] * m0[0] - q0[0]; b[1] = q0[2] * m0[1] - q0[1]; b[2] = q1[2] * m1[0] - q1[0]; b[3] = q1[2] * m1[1] - q1[1]; /* Find the least squares result */ dgelsyFor(4, 3, 1, A, b, x); /* Run a non-linear optimization to refine the result */ CamProjParam param[2]; param[0].set(K0, R0, t0, m0); param[1].set(K1, R1, t1, m1); const int maxIter = 5; double retInfo[LM_INFO_SZ]; dlevmar_dif(residual2View, x, 0, 3, 4, maxIter, 0, retInfo, 0, 0, param); }
static void _epiDistSO3JacobiNum(const double invK[9], const double Rpre[9], const double Tpre[3], const double mpre[2], const double R[9], const double T[3], const double m[2], const double err0, double Jw[3]) { double w[3] = { 0, 0, 0 }; double dR[9], R1[9], E[9], F[9]; const double eps = 1e-16; w[0] = eps; getSO3ExpMap(w, dR); mat33AB(R, dR, R1); formEMat(Rpre, Tpre, R1, T, E); getFMat(invK, invK, E, F); double err = epipolarError(F, m, mpre); Jw[0] = (err - err0) / eps; w[0] = 0; w[1] = eps; getSO3ExpMap(w, dR); mat33AB(R, dR, R1); formEMat(Rpre, Tpre, R1, T, E); getFMat(invK, invK, E, F); err = epipolarError(F, m, mpre); Jw[1] = (err - err0) / eps; w[1] = 0; w[2] = eps; getSO3ExpMap(w, dR); mat33AB(R, dR, R1); formEMat(Rpre, Tpre, R1, T, E); getFMat(invK, invK, E, F); err = epipolarError(F, m, mpre); Jw[2] = (err - err0) / eps; }
void aoGetOptRotation(const double M[], double R[]) { double U[9], S[9], VT[9]; dgesvdFor(3, 3, M, U, S, VT); if (mat33Det(U) * mat33Det(VT) < 0) { VT[6] = -VT[6]; VT[7] = -VT[7]; VT[8] = -VT[8]; } mat33AB(U, VT, R); assert(mat33Det(R) > 0); }
/* * use numerical differentiation to compute the Jacobian of rotation parameters */ static void _perspectiveSO3JacobiNum(const double K[9], const double R[9], const double t[3], const double M[3], const double m[2], const double rm0[2], double Jw[6]) { double w[3] = { 0, 0, 0 }; double rm[2]; double dR[9], R1[9]; const double eps = 1e-8; w[0] = eps; getSO3ExpMap(w, dR); mat33AB(R, dR, R1); project(K, R1, t, M, rm); Jw[0] = (rm[0] - rm0[0]) / eps; Jw[3] = (rm[1] - rm0[1]) / eps; w[0] = 0; w[1] = eps; getSO3ExpMap(w, dR); mat33AB(R, dR, R1); project(K, R1, t, M, rm); Jw[1] = (rm[0] - rm0[0]) / eps; Jw[4] = (rm[1] - rm0[1]) / eps; w[1] = 0; w[2] = eps; getSO3ExpMap(w, dR); mat33AB(R, dR, R1); project(K, R1, t, M, rm); Jw[2] = (rm[0] - rm0[0]) / eps; Jw[5] = (rm[1] - rm0[1]) / eps; }
void intraCamUpdatePose(const double ROld[9], const double tOld[3], const double param[6], double RNew[9], double tNew[3]) { double dR[9]; getSO3ExpMap(param, dR); mat33AB(ROld, dR, RNew); tNew[0] = tOld[0] + param[3]; tNew[1] = tOld[1] + param[4]; tNew[2] = tOld[2] + param[5]; }
void seqTriangulate(const double K[9], const double R[9], const double t[3], const double m[2], double M[3], double cov[3], double sigma) { double rm[2], dm[2]; project(K, R, t, M, rm); dm[0] = m[0] - rm[0]; dm[1] = m[1] - rm[1]; //compute Jacobian: double J[6]; computePerspectiveJacobian(K, R, t, M, J); //compute 2D covariance: double JC[6], S[4], iS[4]; //JC = J*cov //S = JC*J' // S = J*cov*J' matAB(2, 3, 3, 3, J, cov, JC); matABT(2, 3, 2, 3, JC, J, S); double sigma2 = sigma * sigma; S[0] += sigma2; S[3] += sigma2; //iS = (sigma+S)^-1 matInv(2, S, iS); //compute Kalman gain: double CJT[6], G[6]; //CJT = cov*J' //G = cov*J'*iS = cov*J'*(sigma+J*cov*J')^-1 matABT(3, 3, 2, 3, cov, J, CJT); matAB(3, 2, 2, 2, CJT, iS, G); //update 3D position double dM[3]; matAB(3, 2, 2, 1, G, dm, dM); M[0] += dM[0]; M[1] += dM[1]; M[2] += dM[2]; //update 3D covariance //cov - G*J^T*cov double GJ[9], GJC[9]; matAB(3, 2, 2, 3, G, J, GJ); mat33AB(GJ, cov, GJC); mat33Diff(cov, GJC, cov); }
void computePerspectiveJacobian(const double* K, const double* R, const double* t, const double* X, double* fjac) { double Q[9]; double T[3]; mat33AB(K, R, Q); mat33ProdVec(K, t, T); double a1 = (T[0] + Q[0] * X[0] + Q[1] * X[1] + Q[2] * X[2]); double a2 = (T[1] + Q[3] * X[0] + Q[4] * X[1] + Q[5] * X[2]); double a3 = (T[2] + Q[6] * X[0] + Q[7] * X[1] + Q[8] * X[2]); double a3a3 = a3 * a3; fjac[0] = Q[0] / a3 - (Q[6] * a1) / a3a3; fjac[1] = Q[1] / a3 - (Q[7] * a1) / a3a3; fjac[2] = Q[2] / a3 - (Q[8] * a1) / a3a3; fjac[3] = Q[3] / a3 - (Q[6] * a2) / a3a3; fjac[4] = Q[4] / a3 - (Q[7] * a2) / a3a3; fjac[5] = Q[5] / a3 - (Q[8] * a2) / a3a3; }