void getTriangulateCovMat(int nview, const double* K, const double* R,
const double* t, const double x[3], double cov[9], double sigma) {
double * J = new double[nview * 12];
for (int i = 0; i < nview; i++) {
computePerspectiveJacobian(K + 9 * i, R + 9 * i, t + 3 * i, x,
J + 6 * i);
}
double tmp[9];
matATB(nview * 2, 3, nview * 2, 3, J, J, tmp);
matInv(3, tmp, cov);
sigma *= sigma;
cov[0] *= sigma;
cov[1] *= sigma;
cov[2] *= sigma;
cov[3] *= sigma;
cov[4] *= sigma;
cov[5] *= sigma;
cov[6] *= sigma;
cov[7] *= sigma;
cov[8] *= sigma;
delete[] J;
}
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);
}
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);
}
/**
* 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);
}
void getBinTriangulateCovMat(const double* K1, const double* R1,
const double* t1, const double* K2, const double* R2, const double* t2,
double x[3], double cov[9], double sigma) {
double J[12];
computePerspectiveJacobian(K1, R1, t1, x, J);
computePerspectiveJacobian(K2, R2, t2, x, J + 6);
double tmp[9];
matATB(4, 3, 4, 3, J, J, tmp);
matInv(3, tmp, cov);
sigma *= sigma;
cov[0] *= sigma;
cov[1] *= sigma;
cov[2] *= sigma;
cov[3] *= sigma;
cov[4] *= sigma;
cov[5] *= sigma;
cov[6] *= sigma;
cov[7] *= sigma;
cov[8] *= sigma;
}
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);
}
