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