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;
}
