static void intraCamCovWeightedLMStep(const double K[9],
const double R[9],
const double t[3],
int npts,
const double covs[],
const double Ws[],
const double Ms[],
const double ms[],
double param[6],
const double lambda) {
double sA[36], A[36], invSA[36], sB[6], B[6], rm[2], rerr[2];
double Jw[6], Jt[6];
memset(sA, 0, sizeof(double) * 36);
memset(sB, 0, sizeof(double) * 6);
const double* pMs = Ms;
const double* pcovs = covs;
const double* pms = ms;
for (int i = 0; i < npts; i++, pMs += 3, pcovs += 9, pms += 2) {
double var[4], ivar[4];
project(K, R, t, pMs, rm);
getProjectionCovMat(K, R, t, pMs, pcovs, var, 1.0);
mat22Inv(var, ivar);
_perspectiveSO3JacobiNum(K, R, t, pMs, pms, rm, Jw);
_perspectiveTJacobiNum(K, R, t, pMs, pms, rm, Jt);
//J:\in R ^{2x6}
double J[12] = { Jw[0], Jw[1], Jw[2], Jt[0], Jt[1], Jt[2], Jw[3], Jw[4], Jw[5], Jt[3], Jt[4], Jt[5] };
//\sum_i w[i]*(J_\omega^T *ivar[i] * J_\omega)
double Jivar[12];
matATB(2, 6, 2, 2, J, ivar, Jivar);
matAB(6, 2, 2, 6, Jivar, J, A);
mat66scale(A, Ws[i]);
mat66sum(sA, A, sA);
//-\sum_i w[i]*( J_\omega^T *ivar[i] *\epsilon_i)
rerr[0] = (-rm[0] + pms[0]);
rerr[1] = (-rm[1] + pms[1]);
matAB(6, 2, 2, 1, Jivar, rerr, B);
mat16scale(B, Ws[i]);
mat16sum(sB, B, sB);
}
sA[0] += lambda;
sA[7] += lambda;
sA[14] += lambda;
sA[21] += lambda;
sA[28] += lambda;
sA[35] += lambda;
matInv(6, sA, invSA);
matAB(6, 6, 6, 1, invSA, sB, param);
}
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 getProjectionCovMat(const double* K, const double* R, const double* t,
const double x[3], const double cov[9], double var[4], double sigma) {
double J[6];
computePerspectiveJacobian(K, R, t, x, J);
double tmp[6];
matABT(3, 3, 2, 3, cov, J, tmp);
matAB(2, 3, 3, 2, J, tmp, var);
var[0] += sigma * sigma;
var[3] += sigma * sigma;
}
static void intraCamWeightedLMStep(const double K[9],
const double R[9],
const double t[3],
int npts,
const double Ws[],
const double Ms[],
const double ms[],
double param[6],
const double lambda) {
double sA[36], A[36], invSA[36], sB[6], B[6], rm[2], rerr[2];
double Jw[6], Jt[6];
memset(sA, 0, sizeof(double) * 36);
memset(sB, 0, sizeof(double) * 6);
const double* pMs = Ms;
const double* pms = ms;
for (int i = 0; i < npts; i++, pMs += 3, pms += 2) {
project(K, R, t, pMs, rm);
_perspectiveSO3JacobiNum(K, R, t, pMs, pms, rm, Jw);
_perspectiveTJacobiNum(K, R, t, pMs, pms, rm, Jt);
double J[12] = { Ws[i] * Jw[0], Ws[i] * Jw[1], Ws[i] * Jw[2], Ws[i] * Jt[0], Ws[i] * Jt[1], Ws[i] * Jt[2],
Ws[i] * Jw[3], Ws[i] * Jw[4], Ws[i] * Jw[5], Ws[i] * Jt[3], Ws[i] * Jt[4], Ws[i] * Jt[5] };
matATB(2, 6, 2, 6, J, J, A);
mat66sum(sA, A, sA);
rerr[0] = (-rm[0] + pms[0]) * Ws[i];
rerr[1] = (-rm[1] + pms[1]) * Ws[i];
matATB(2, 6, 2, 1, J, rerr, B);
mat16sum(sB, B, sB);
}
sA[0] += lambda;
sA[7] += lambda;
sA[14] += lambda;
sA[21] += lambda;
sA[28] += lambda;
sA[35] += lambda;
matInv(6, sA, invSA);
matAB(6, 6, 6, 1, invSA, sB, param);
}
/**
* compute a new parameter for each iterative step
*/
static void intraCamLMStep(const double K[9], const double R[9], //current parameter
const double t[3],
int npts,
const double Ms[], //corresponding 3D-2D points
const double ms[],
double param[6], //new parameter
const double lambda //damping coefficient
) {
double sA[36], A[36], invSA[36], sB[6], B[6], rm[2], rerr[2];
double Jw[6], Jt[6];
memset(sA, 0, sizeof(double) * 36);
memset(sB, 0, sizeof(double) * 6);
const double* pMs = Ms;
const double* pms = ms;
for (int i = 0; i < npts; i++, pMs += 3, pms += 2) {
project(K, R, t, pMs, rm);
_perspectiveSO3JacobiNum(K, R, t, pMs, pms, rm, Jw);
_perspectiveTJacobiNum(K, R, t, pMs, pms, rm, Jt);
double J[12] = { Jw[0], Jw[1], Jw[2], Jt[0], Jt[1], Jt[2], Jw[3], Jw[4], Jw[5], Jt[3], Jt[4], Jt[5] };
matATB(2, 6, 2, 6, J, J, A);
mat66sum(sA, A, sA);
rerr[0] = -rm[0] + pms[0];
rerr[1] = -rm[1] + pms[1];
matATB(2, 6, 2, 1, J, rerr, B);
mat16sum(sB, B, sB);
}
sA[0] += lambda;
sA[7] += lambda;
sA[14] += lambda;
sA[21] += lambda;
sA[28] += lambda;
sA[35] += lambda;
matInv(6, sA, invSA);
matAB(6, 6, 6, 1, invSA, sB, param);
}
static void intraCamEpiLMStep(const double K[9], const double invK[9], const double R[9], //current parameter (rotation and translation)
const double t[3],
int n2D3Ds, //number of 2D-3D correspondences
const double Ms[], //3D points
const double ms[], //2D points
int n2D2Ds, //number of 2D-2D correspondences
const double uRpre[], //camera poses at former frames
const double utpre[],
const double umspre[], //unmapped feature points at former frames
const double ums[], //unmapped feature points the current frames
double param[6], //new parameter
const double lambda // damping coefficient
) {
double sA[36], A[36], invSA[36], sB[6], B[6], rm[2], rerr[2], eerr;
double Jw[6], Jt[6];
memset(sA, 0, sizeof(double) * 36);
memset(sB, 0, sizeof(double) * 6);
const double* pMs = Ms;
const double* pms = ms;
for (int i = 0; i < n2D3Ds; i++, pMs += 3, pms += 2) {
project(K, R, t, pMs, rm);
_perspectiveSO3JacobiNum(K, R, t, pMs, pms, rm, Jw);
_perspectiveTJacobiNum(K, R, t, pMs, pms, rm, Jt);
double J[12] = { Jw[0], Jw[1], Jw[2], Jt[0], Jt[1], Jt[2], Jw[3], Jw[4], Jw[5], Jt[3], Jt[4], Jt[5] };
matATB(2, 6, 2, 6, J, J, A);
mat66sum(sA, A, sA);
rerr[0] = -rm[0] + pms[0];
rerr[1] = -rm[1] + pms[1];
matATB(2, 6, 2, 1, J, rerr, B);
mat16sum(sB, B, sB);
}
const double* pms1 = umspre;
const double* pms2 = ums;
const double* pRpre = uRpre;
const double* ptpre = utpre;
for (int j = 0; j < n2D2Ds; j++, pms1 += 2, pms2 += 2, pRpre += 9, ptpre += 3) {
//compute the epipolar error
double E[9], F[9], Ew[3], Et[3];
formEMat(R, t, pRpre, ptpre, E);
getFMat(invK, invK, E, F);
eerr = epipolarError(F, pms2, pms1);
_epiDistSO3JacobiNum(invK, pRpre, ptpre, pms1, R, t, pms2, eerr, Ew);
_epiDistTJacobiNum(invK, pRpre, ptpre, pms1, R, t, pms2, eerr, Et);
double J[6] = { Ew[0], Ew[1], Ew[2], Et[0], Et[1], Et[2] };
matATB(1, 6, 1, 6, J, J, A);
mat66sum(sA, A, sA);
B[0] = -J[0] * eerr;
B[1] = -J[1] * eerr;
B[2] = -J[2] * eerr;
B[3] = -J[3] * eerr;
B[4] = -J[4] * eerr;
B[5] = -J[5] * eerr;
mat16sum(sB, B, sB);
}
sA[0] += lambda;
sA[7] += lambda;
sA[14] += lambda;
sA[21] += lambda;
sA[28] += lambda;
sA[35] += lambda;
matInv(6, sA, invSA);
matAB(6, 6, 6, 1, invSA, sB, param);
}
