void GLImagePane::drawEpipolarLine(ImgRGB& img) {
assert(s_clicked_camid >= 0);
const double* R0 = m_pSLAM->slam[s_clicked_camid].m_camPos.current()->R;
const double* t0 = m_pSLAM->slam[s_clicked_camid].m_camPos.current()->t;
const double* iK0 = m_pSLAM->slam[s_clicked_camid].iK.data;
const double* R1 = m_pSLAM->slam[m_camId].m_camPos.current()->R;
const double* t1 = m_pSLAM->slam[m_camId].m_camPos.current()->t;
const double* iK1 = m_pSLAM->slam[m_camId].iK.data;
double E[9], F[9];
formEMat(R0, t0, R1, t1, E);
getFMat(iK1, iK0, E, F);
double l[3];
double fx, fy;
if (s_clicked_mappt) {
if (!s_clicked_mappt->pFeatures[s_clicked_camid])
return;
fx = s_clicked_mappt->pFeatures[s_clicked_camid]->m[0];
fy = s_clicked_mappt->pFeatures[s_clicked_camid]->m[1];
} else {
fx = s_clicked_featPt->x;
fy = s_clicked_featPt->y;
}
computeEpipolarLine(F, fx, fy, l);
drawLine(img, l, 255, 255, 255);
}
double _epiError2(const double invK[],
int n2D2D,
const double Rpre[],
const double tpre[],
const double umspre[],
const double R[],
const double t[],
const double ums[]) {
const double* pms1 = umspre;
const double* pms2 = ums;
const double* pRpre = Rpre;
const double* ptpre = tpre;
double s = 0;
for (int j = 0; j < n2D2D; j++, pms1 += 2, pms2 += 2, pRpre += 9, ptpre += 3) {
//compute the epipolar error
double E[9], F[9];
formEMat(R, t, pRpre, ptpre, E);
getFMat(invK, invK, E, F);
double err = epipolarError(F, pms2, pms1);
s += err * err;
}
return s;
}
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;
}
static void _epiDistTJacobiNum(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 Jt[3]) {
double E[9], F[9];
const double eps = 1e-16;
double T1[3] = { T[0] + eps, T[1], T[2] };
formEMat(Rpre, Tpre, R, T1, E);
getFMat(invK, invK, E, F);
double err = epipolarError(F, m, mpre);
Jt[0] = (err - err0) / eps;
T1[0] = T[0];
T1[1] = T[1] + eps;
formEMat(Rpre, Tpre, R, T1, E);
getFMat(invK, invK, E, F);
err = epipolarError(F, m, mpre);
Jt[1] = (err - err0) / eps;
T1[1] = T[1];
T1[2] = T[2] + eps;
formEMat(Rpre, Tpre, R, T1, E);
getFMat(invK, invK, E, F);
err = epipolarError(F, m, mpre);
Jt[2] = (err - err0) / eps;
}
int SingleSLAM::detectDynamicFeaturePoints(int maxLen, int minLen,
int minOutNum, double maxEpiErr) {
std::vector<Track2DNode*> nodes;
getUnMappedAndDynamicTrackNodes(nodes, minLen);
int k = 0;
for (size_t i = 0; i < nodes.size(); i++) {
FeaturePoint* fp = nodes[i]->pt;
const double* R0 = fp->cam->R;
const double* t0 = fp->cam->t;
const double* m0 = fp->m;
assert(fp->cam);
int nOut = 0;
double E[9], F[9];
int f = 0;
for (; fp && nOut <= minOutNum && f < maxLen; fp = fp->preFrame) {
if (!fp->cam)
continue;
const double* R1 = fp->cam->R;
const double* t1 = fp->cam->t;
const double* m1 = fp->m;
formEMat(R1, t1, R0, t0, E);
getFMat(iK.data, iK.data, E, F);
if (epipolarError(F, m0, m1) >= maxEpiErr) {
nOut++;
}
}
if (nOut > minOutNum) {
nodes[i]->pt->type = TYPE_FEATPOINT_DYNAMIC;
k++;
} else if (!nodes[i]->pt->mpt) {
nodes[i]->pt->type = TYPE_FEATPOINT_STATIC;
}
}
return k;
}
double _epiError2Weighted(const double invK[],
int n2D2D,
const double uWs[],
const double Rpre[],
const double tpre[],
const double umspre[],
const double R[],
const double t[],
const double ums[]) {
const double* pms1 = umspre;
const double* pms2 = ums;
const double* pRpre = Rpre;
const double* ptpre = tpre;
double E[9], F[9], l[3];
formEMat(R, t, pRpre, ptpre, E);
getFMat(invK, invK, E, F);
double s = 0;
for (int j = 0; j < n2D2D; j++, pms1 += 2, pms2 += 2) {
computeEpipolarLine(F, pms1[0], pms1[1], l);
double A = l[0];
double B = l[1];
double C = l[2];
double x0 = pms2[0];
double y0 = pms2[1];
double lambda = (-A * x0 - B * y0 - C);
double err2 = lambda * lambda / (A * A + B * B);
s += err2 * uWs[j];
}
return s;
}
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);
}