/*
* The main process of levenberg-marquart optimization
*/
bool intraCamLMEpiProc(const double K[9],
const double invK[9],
const double R0[9],
const double t0[3],
int n3D2D,
const double Ms[],
const double ms[],
int n2D2D,
const double Rpre[],
const double tpre[],
const double umspre[],
const double ums[],
double R_opt[9],
double t_opt[3],
IntraCamPoseOption* opt) {
assert(opt);
double param[6];
//compute the reprojection error
opt->npts = n3D2D;
opt->lambda = opt->lambda0;
opt->err0 = reprojError2(K, R0, t0, n3D2D, Ms, ms) + _epiError2(invK, n2D2D, Rpre, tpre, umspre, R0, t0, ums);
opt->err = opt->err0;
if (opt->verboseLM > 0)
printf("[%d]err:%lf, lambda:%lf\n", -1, opt->err, opt->lambda);
double R[9], t[3];
doubleArrCopy(R, 0, R0, 9);
doubleArrCopy(t, 0, t0, 3);
opt->retTypeLM = 1;
int i = 0;
for (; i < opt->maxIterLM; i++) {
intraCamEpiLMStep(K, invK, R, t, n3D2D, Ms, ms, n2D2D, Rpre, tpre, umspre, ums, param, opt->lambda);
intraCamUpdatePose(R, t, param, R_opt, t_opt);
double err = reprojError2(K, R_opt, t_opt, n3D2D, Ms, ms) + _epiError2(invK, n2D2D, Rpre, tpre, umspre, R_opt,
t_opt, ums);
if (opt->verboseLM > 0)
printf("[%d]err:%lf, lambda:%lf\n", i, err, opt->lambda);
if (fabs(err - opt->err) < opt->epsErrorChangeLM) {
opt->retTypeLM = 0;
break;
}
if (err < opt->err) {
doubleArrCopy(R, 0, R_opt, 9);
doubleArrCopy(t, 0, t_opt, 3);
opt->err = err;
opt->lambda /= 10;
} else {
opt->lambda *= 10;
if (opt->lambda > 1e+4) {
//cannot find the optimum
opt->retTypeLM = -1;
break;
}
}
}
opt->nIterLM = i;
return opt->retTypeLM >= 0;
}
bool isRegistable(MapPoint* mp, FeaturePoint* new_fp, double pixelVar) {
if (mp->featPts.size() == 0)
return false;
size_t nview = mp->featPts.size();
Mat_d Ks(nview + 1, 9), Rs(nview + 1, 9), ts(nview + 1, 3), ms(nview + 1,
2), nms(nview + 1, 2);
double iK[9];
for (size_t c = 0; c < nview; c++) {
const FeaturePoint* fp = mp->featPts[c];
assert(fp->K && fp->cam);
const double* K = fp->K;
const double* R = fp->cam->R;
const double* t = fp->cam->t;
const double* m = fp->m;
doubleArrCopy(Ks, c, K, 9);
doubleArrCopy(Rs, c, R, 9);
doubleArrCopy(ts, c, t, 3);
doubleArrCopy(ms, c, m, 2);
getInvK(K, iK);
normPoint(iK, m, nms + 2 * c);
}
doubleArrCopy(Ks, nview, new_fp->K, 9);
doubleArrCopy(Rs, nview, new_fp->cam->R, 9);
doubleArrCopy(ts, nview, new_fp->cam->t, 3);
doubleArrCopy(ms, nview, new_fp->m, 2);
normPoint(iK, new_fp->m, nms + 2 * nview);
double M[3], m[2];
triangulateMultiView((int) (nview + 1), Rs, ts, nms, M);
//check the re-projection error
for (size_t c = 0; c < nview; c++) {
const FeaturePoint* fp = mp->featPts[c];
assert(fp->K && fp->cam);
project(fp->K, fp->cam->R, fp->cam->t, M, m);
if (dist2(m, fp->m) > pixelVar)
return false;
}
project(new_fp->K, new_fp->cam->R, new_fp->cam->t, M, m);
if (dist2(m, new_fp->m) > pixelVar)
return false;
return true;
}
void SingleSLAM::refineTriangulation(const FeaturePoint* fp, double M[3],
double cov[9]) {
double Ks[18], Rs[18], ts[6], ms[4], nms[4];
double iK[9];
getInvK(K.data, iK);
const double* R0 = fp->cam->R;
const double* t0 = fp->cam->t;
doubleArrCopy(Ks, 0, K.data, 9);
doubleArrCopy(Rs, 0, R0, 9);
doubleArrCopy(ts, 0, t0, 3);
doubleArrCopy(ms, 0, fp->m, 2);
normPoint(iK, fp->m, nms);
double C0[3];
getCameraCenter(R0, t0, C0);
const FeaturePoint* maxFp = 0;
double maxAngle = 0;
//search a camera pose that has the largest difference from the view direction (R0,t0)
fp = fp->preFrame;
while (fp && fp->cam) {
double C[3];
getCameraCenter(fp->cam->R, fp->cam->t, C);
double angle = getAbsRadiansBetween(M, C0, C);
if (angle > maxAngle) {
maxAngle = angle;
maxFp = fp;
}
fp = fp->preFrame;
}
if (maxFp) {
doubleArrCopy(Ks, 1, K.data, 9);
doubleArrCopy(Rs, 1, maxFp->cam->R, 9);
doubleArrCopy(ts, 1, maxFp->cam->t, 3);
doubleArrCopy(ms, 1, maxFp->m, 2);
normPoint(iK, maxFp->m, nms + 2);
triangulateMultiView(2, Rs, ts, nms, M);
getTriangulateCovMat(2, Ks, Rs, ts, M, cov, Const::PIXEL_ERR_VAR);
}
}
void updatePointUncertainty(MapPoint* mp, FeaturePoint* new_fp,
double pixelVar) {
if (mp->featPts.size() == 0)
return;
size_t nview = mp->featPts.size();
Mat_d Ks(nview + 1, 9), Rs(nview + 1, 9), ts(nview + 1, 3), ms(nview + 1,
2), nms(nview + 1, 2);
double iK[9];
for (size_t c = 0; c < nview; c++) {
const FeaturePoint* fp = mp->featPts[c];
assert(fp->K && fp->cam);
const double* K = fp->K;
const double* R = fp->cam->R;
const double* t = fp->cam->t;
const double* m = fp->m;
doubleArrCopy(Ks, c, K, 9);
doubleArrCopy(Rs, c, R, 9);
doubleArrCopy(ts, c, t, 3);
doubleArrCopy(ms, c, m, 2);
getInvK(K, iK);
normPoint(iK, m, nms + 2 * c);
}
doubleArrCopy(Ks, nview, new_fp->K, 9);
doubleArrCopy(Rs, nview, new_fp->cam->R, 9);
doubleArrCopy(ts, nview, new_fp->cam->t, 3);
doubleArrCopy(ms, nview, new_fp->m, 2);
normPoint(iK, new_fp->m, nms + 2 * nview);
getTriangulateCovMat((int) (nview + 1), Ks, Rs, ts, mp->M, mp->cov,
pixelVar);
}
int SingleSLAM::newMapPoints(std::vector<MapPoint*>& mapPts, double maxEpiErr,
double maxNcc) {
std::vector<FeaturePoint*> vecFeatPts;
getUnMappedAndTrackedFeatPts(vecFeatPts, 0, Param::nMinFeatTrkLen);
mapPts.clear();
mapPts.reserve(4096);
double M[3], m1[2], m2[2];
//reconstruct 3D map points
int numRecons = 0;
for (size_t k = 0; k < vecFeatPts.size(); k++) {
FeaturePoint* cur_fp = vecFeatPts[k];
FeaturePoint* pre_fp = cur_fp;
while (pre_fp->preFrame && pre_fp->preFrame->cam) {
if (pre_fp->type == TYPE_FEATPOINT_DYNAMIC) {
break;
}
pre_fp = pre_fp->preFrame;
}
if (pre_fp->type == TYPE_FEATPOINT_DYNAMIC || !pre_fp->cam)
continue;
normPoint(iK.data, pre_fp->m, m1);
normPoint(iK.data, cur_fp->m, m2);
//triangulate the two feature points to get the 3D point
binTriangulate(pre_fp->cam->R, pre_fp->cam->t, cur_fp->cam->R,
cur_fp->cam->t, m1, m2, M);
if (isAtCameraBack(cur_fp->cam->R, cur_fp->cam->t, M))
continue;
double cov[9], org[3];
getBinTriangulateCovMat(K.data, pre_fp->cam->R, pre_fp->cam->t, K.data,
cur_fp->cam->R, cur_fp->cam->t, M, cov, Const::PIXEL_ERR_VAR);
getCameraCenter(cur_fp->cam->R, cur_fp->cam->t, org);
double s = fabs(cov[0] + cov[4] + cov[8]);
if (dist3(org, M) < sqrt(s))
continue;
//check the reprojection error
double err1 = reprojErrorSingle(K.data, pre_fp->cam->R, pre_fp->cam->t,
M, pre_fp->m);
double err2 = reprojErrorSingle(K.data, cur_fp->cam->R, cur_fp->cam->t,
M, cur_fp->m);
if (err1 < maxEpiErr && err2 < maxEpiErr) {
//a new map point is generated
refineTriangulation(cur_fp, M, cov);
err1 = reprojErrorSingle(K.data, pre_fp->cam->R, pre_fp->cam->t, M,
pre_fp->m);
err2 = reprojErrorSingle(K.data, cur_fp->cam->R, cur_fp->cam->t, M,
cur_fp->m);
if (isAtCameraBack(cur_fp->cam->R, cur_fp->cam->t, M)
|| isAtCameraBack(pre_fp->cam->R, pre_fp->cam->t, M))
continue;
if (err1 < maxEpiErr && err2 < maxEpiErr) {
MapPoint* pM = new MapPoint(M[0], M[1], M[2], pre_fp->f);
doubleArrCopy(pM->cov, 0, cov, 9);
mapPts.push_back(pM);
pM->lastFrame = cur_fp->f;
for (FeaturePoint* p = cur_fp; p; p = p->preFrame)
p->mpt = pM;
//add the feature point
pM->addFeature(camId, cur_fp);
pM->setLocalStatic();
//compute the NCC block
pM->nccBlks[camId].computeScaled(m_lastKeyPos->imgSmall,
m_lastKeyPos->imgScale, cur_fp->x, cur_fp->y);
int x = max(0, min((int) cur_fp->x, m_rgb.w - 1));
int y = max(0, min((int) cur_fp->y, m_rgb.h - 1));
pM->setColor(m_rgb(x, y));
numRecons++;
}
}
}
return numRecons;
}
int SingleSLAM::fastPoseUpdate3D() {
propagateFeatureStates();
//get the feature points corresponding to the map points
std::vector<Track2DNode*> nodes;
int num = getStaticMappedTrackNodes(nodes);
if (num < 5) {
repErr(
"[camera id:%d]intra-camera pose update failed! less than five static map points (%d)",
camId, num);
return -1;
}
//choose the feature points for pose estimation
std::vector<FeaturePoint*> featPts;
int iChoose = chooseStaticFeatPts(featPts);
//test
logInfo("number of chosen features :%d\n", iChoose);
std::vector<FeaturePoint*> mappedFeatPts;
std::vector<FeaturePoint*> unmappedFeatPts;
mappedFeatPts.reserve(nRowBlk * nColBlk * 2);
unmappedFeatPts.reserve(nRowBlk * nColBlk * 2);
for (size_t i = 0; i < featPts.size(); i++) {
if (featPts[i]->mpt)
mappedFeatPts.push_back(featPts[i]);
else if (featPts[i]->preFrame)
unmappedFeatPts.push_back(featPts[i]);
}
//get the 2D-3D corresponding points
int n3D2Ds = mappedFeatPts.size();
Mat_d ms(n3D2Ds, 2), Ms(n3D2Ds, 3), repErrs(n3D2Ds, 1);
for (int i = 0; i < n3D2Ds; i++) {
FeaturePoint* fp = mappedFeatPts[i];
ms.data[2 * i] = fp->x;
ms.data[2 * i + 1] = fp->y;
Ms.data[3 * i] = fp->mpt->x;
Ms.data[3 * i + 1] = fp->mpt->y;
Ms.data[3 * i + 2] = fp->mpt->z;
repErrs.data[i] = fp->reprojErr;
}
//get the 2D-2D corresponding points
int n2D2Ds = unmappedFeatPts.size();
Mat_d ums(n2D2Ds, 2), umspre(n2D2Ds, 2), Rpre(n2D2Ds, 9), tpre(n2D2Ds, 3);
for (int i = 0; i < n2D2Ds; i++) {
FeaturePoint* fp = unmappedFeatPts[i];
ums.data[2 * i] = fp->x;
ums.data[2 * i + 1] = fp->y;
//travel back to the frame of the first appearance
//while (fp->preFrame) {
fp = fp->preFrame;
//}
assert(fp);
umspre.data[2 * i] = fp->x;
umspre.data[2 * i + 1] = fp->y;
doubleArrCopy(Rpre.data, i, fp->cam->R, 9);
doubleArrCopy(tpre.data, i, fp->cam->t, 3);
}
//estimate the camera pose using both 2D-2D and 3D-2D correspondences
double R[9], t[3];
double* cR = m_camPos.current()->R;
double* cT = m_camPos.current()->t;
// //test
// logInfo("==============start of camId:%d=================\n", camId);
// logInfo("n3D2D:%d, n2D2D:%d\n", n3D2Ds, n2D2Ds);
//
// write(K, "/home/tsou/data/K.txt");
// write(cR, 3, 3, "/home/tsou/data/%d_R0.txt", camId);
// write(cT, 3, 1, "/home/tsou/data/%d_T0.txt", camId);
// write(repErrs, "/home/tsou/data/%d_errs.txt", camId);
// write(Ms, "/home/tsou/data/%d_Ms.txt", camId);
// write(ms, "/home/tsou/data/%d_ms.txt", camId);
// write(Rpre, "/home/tsou/data/%d_Rpre.txt", camId);
// write(tpre, "/home/tsou/data/%d_tpre.txt", camId);
// write(umspre, "/home/tsou/data/%d_umspre.txt", camId);
// write(ums, "/home/tsou/data/%d_ums.txt", camId);
//
// //test
// printMat(3, 3, cR);
// printMat(3, 1, cT);
IntraCamPoseOption opt;
double R_tmp[9], t_tmp[3];
intraCamEstimate(K.data, cR, cT, n3D2Ds, repErrs.data, Ms.data, ms.data, 6,
R_tmp, t_tmp, &opt);
if (getCameraDistance(R_tmp, t_tmp, Rpre.data, tpre.data) > 1000) {
opt.verboseLM = 1;
intraCamEstimateEpi(K.data, R_tmp, t_tmp, n3D2Ds, repErrs.data, Ms.data,
ms.data, n2D2Ds, 0, Rpre.data, tpre.data, umspre.data, ums.data,
6, R, t, &opt);
} else {
doubleArrCopy(R, 0, R_tmp, 9);
doubleArrCopy(t, 0, t_tmp, 3);
}
// printMat(3, 3, cR);
// printMat(3, 1, cT);
// printMat(3, 3, R);
// printMat(3, 1, cT);
// logInfo("==============end of camId:%d=================\n", camId);
// intraCamEstimate(K.data,cR,cT,n3D2Ds, repErrs.data,Ms.data,ms.data,6.0,R,t,&opt);
// find outliers
int numOut = 0;
double rm[2], var[4], ivar[4];
for (int i = 0; i < num; i++) {
double* pM = nodes[i]->pt->mpt->M;
double* pCov = nodes[i]->pt->mpt->cov;
project(K, R, t, pM, rm);
getProjectionCovMat(K, R, t, pM, pCov, var, Const::PIXEL_ERR_VAR);
mat22Inv(var, ivar);
double err = mahaDist2(rm, nodes[i]->pt->m, ivar);
if (err < 1) { //inlier
nodes[i]->pt->reprojErr = err;
seqTriangulate(K, R, t, nodes[i]->pt->m, pM, pCov,
Const::PIXEL_ERR_VAR);
project(K, R, t, pM, rm);
getProjectionCovMat(K, R, t, pM, pCov, var, Const::PIXEL_ERR_VAR);
mat22Inv(var, ivar);
err = mahaDist2(rm, nodes[i]->pt->m, ivar);
if (err >= 1) {
nodes[i]->pt->mpt->setFalse();
}
} else {
//outliers
numOut++;
double repErr = dist2(rm, nodes[i]->pt->m);
nodes[i]->pt->reprojErr = repErr;
nodes[i]->pt->mpt->setUncertain();
}
}
CamPoseItem* camPos = m_camPos.add(currentFrame(), camId, R, t);
updateCamParamForFeatPts(K, camPos);
return num;
}
bool intraCamCovEstimate(const double K[9],
const double R0[9],
const double t0[3],
int npts,
const double prevErrs[],
const double covs[],
const double Ms[],
const double ms[],
const double tau,
double R_opt[9],
double t_opt[3],
IntraCamPoseOption* opt) {
if (opt->verboseRW)
printf("=============start of intra-camera pose estimation ===========\n");
double* Ws = new double[npts];
for (int i = 0; i < npts; i++) {
if (prevErrs == 0)
Ws[i] = 1.0;
else {
double e = fabs(prevErrs[i]);
if (e >= tau) {
Ws[i] = 0;
} else {
e /= tau;
e = 1 - e * e;
Ws[i] = e * e;
}
}
}
double R[9], t[3];
doubleArrCopy(R, 0, R0, 9);
doubleArrCopy(t, 0, t0, 3);
bool ret = true;
int k = 0;
opt->errRW = -1;
for (; k < opt->maxIterRW; k++) {
if (!intraCamCovWeightedLMProc(K, R, t, npts, covs, Ws, Ms, ms, R_opt, t_opt, opt)) {
ret = false;
goto returnPoint;
}
opt->lambda0 = opt->lambda;
if (opt->errRW < 0)
opt->errRW = opt->err;
else {
if (fabs(opt->err - opt->errRW) < opt->epsErrorChangeRW) {
ret = true;
goto returnPoint;
} else
opt->errRW = opt->err;
}
if (opt->verboseRW) {
printf(">>rw:[%d]:err:%lf\n", k, opt->errRW);
}
doubleArrCopy(R, 0, R_opt, 9);
doubleArrCopy(t, 0, t_opt, 3);
//compute the new weights
for (int i = 0; i < npts; i++) {
double rm[2];
project(K, R, t, Ms + 3 * i, rm);
double dx = rm[0] - ms[2 * i];
double dy = rm[1] - ms[2 * i + 1];
double e = sqrt(dx * dx + dy * dy);
if (e >= tau)
Ws[i] = 0;
else {
e /= tau;
e = 1 - e * e;
Ws[i] = e * e;
}
}
}
returnPoint: opt->nIterRW = k;
delete[] Ws;
if (opt->verboseRW)
printf("-------------end of intra-camera pose estimation ---------\n");
return ret;
}
bool intraCamCovWeightedLMProc(const double K[9],
const double R0[9],
const double t0[3],
int npts,
const double covs[],
const double Ws[],
const double Ms[],
const double ms[],
double R_opt[9],
double t_opt[3],
IntraCamPoseOption* opt) {
assert(opt);
double param[6];
//compute the reprojection error
opt->npts = npts;
opt->lambda = opt->lambda0;
opt->err0 = reprojError2CovWeighted(K, R0, t0, npts, covs, Ws, Ms, ms);
opt->err = opt->err0;
if (opt->verboseLM > 0)
printf("[%d]err:%lf, lambda:%lf, (%lf %lf %lf %lf %lf %lf)\n", -1, opt->err, opt->lambda, param[0], param[1],
param[2], param[3], param[4], param[5]);
double R[9], t[3], R_tmp[9], t_tmp[3];
doubleArrCopy(R, 0, R0, 9);
doubleArrCopy(t, 0, t0, 3);
opt->retTypeLM = 1;
int i = 0;
double err = opt->err0;
for (; i < opt->maxIterLM; i++) {
intraCamCovWeightedLMStep(K, R, t, npts, covs, Ws, Ms, ms, param, opt->lambda);
intraCamUpdatePose(R, t, param, R_opt, t_opt);
if (param[0] * param[0] + param[1] * param[1] + param[2] * param[2] + param[3] * param[3] + param[4] * param[4]
+ param[5] * param[5] < opt->epsParamChangeLM) {
doubleArrCopy(R, 0, R_opt, 9);
doubleArrCopy(t, 0, t_opt, 3);
opt->retTypeLM = 0;
break;
}
err = reprojError2CovWeighted(K, R_opt, t_opt, npts, covs, Ws, Ms, ms);
if (opt->verboseLM > 0)
printf("[%d]err:%lf, lambda:%lf, (%lf %lf %lf %lf %lf %lf)\n", i, err, opt->lambda, param[0], param[1],
param[2], param[3], param[4], param[5]);
if (fabs(err - opt->err) < opt->epsErrorChangeLM) {
opt->retTypeLM = 0;
break;
}
if (err <= opt->err) {
doubleArrCopy(R, 0, R_opt, 9);
doubleArrCopy(t, 0, t_opt, 3);
doubleArrCopy(R_tmp, 0, R_opt, 9);
doubleArrCopy(t_tmp, 0, t_opt, 3);
opt->err = err;
opt->lambda /= 10;
} else {
opt->lambda *= 10;
if (opt->lambda > 1e+18) {
//cannot find the optimum
opt->retTypeLM = -1;
break;
}
}
}
if (opt->retTypeLM == -1) {
doubleArrCopy(R_opt, 0, R_tmp, 9);
doubleArrCopy(t_opt, 0, t_tmp, 3);
}
opt->err = err;
opt->nIterLM = i;
if (opt->verboseLM)
opt->printLM();
return opt->retTypeLM >= 0;
}
bool intraCamEstimateEpi(const double K[9],
const double R0[9],
const double t0[3],
int n3D2Ds,
const double preRepErrs[],
const double Ms[],
const double ms[],
int n2D2Ds,
const double preEpiErrs[],
const double Rpre[],
const double tpre[],
const double umspre[],
const double ums[],
const double tau,
double R_opt[9],
double t_opt[3],
IntraCamPoseOption* opt) {
double invK[9];
mat33Inv(K, invK);
if (opt->verboseRW)
printf("=============start of intra-camera pose estimation ===========\n");
double* Ws = new double[n3D2Ds];
for (int i = 0; i < n3D2Ds; i++) {
if (preRepErrs == 0)
Ws[i] = 1.0;
else {
double e = fabs(preRepErrs[i]);
if (e >= tau) {
Ws[i] = 0;
} else {
e /= tau;
e = 1 - e * e;
Ws[i] = e * e;
}
}
}
double* uWs = new double[n2D2Ds];
for (int j = 0; j < n2D2Ds; j++) {
if (preEpiErrs == 0) {
uWs[j] = 1.0;
} else {
double e = fabs(preEpiErrs[j]);
if (e >= tau) {
uWs[j] = 0;
} else {
e /= tau;
e = 1 - e * e;
uWs[j] = e * e;
}
}
}
double R[9], t[3];
doubleArrCopy(R, 0, R0, 9);
doubleArrCopy(t, 0, t0, 3);
bool ret = true;
int k = 0;
opt->errRW = -1;
for (; k < opt->maxIterRW; k++) {
intraCamLMEpiWeightedProc(K, invK, R, t, n3D2Ds, Ws, Ms, ms, n2D2Ds, Rpre, tpre, uWs, umspre, ums, R_opt,
t_opt, opt);
opt->lambda0 = opt->lambda;
if (opt->errRW < 0)
opt->errRW = opt->err;
else {
if (fabs(opt->err - opt->errRW) < opt->epsErrorChangeRW) {
ret = true;
goto returnPoint;
} else
opt->errRW = opt->err;
}
if (opt->verboseRW) {
printf(">> rw:[%d]:err:%lf\n", k, opt->errRW);
}
doubleArrCopy(R, 0, R_opt, 9);
doubleArrCopy(t, 0, t_opt, 3);
//compute the new weights
for (int i = 0; i < n3D2Ds; i++) {
double rm[2];
project(K, R, t, Ms + 3 * i, rm);
double dx = rm[0] - ms[2 * i];
double dy = rm[1] - ms[2 * i + 1];
double e = sqrt(dx * dx + dy * dy);
if (e >= tau)
Ws[i] = 0;
else {
e /= tau;
e = 1 - e * e;
Ws[i] = e * e;
}
}
}
returnPoint: opt->nIterRW = k;
delete[] Ws;
if (opt->verboseRW) {
printf("err:%lf-->%lf\n", opt->err0, opt->err);
printf("-------------end of intra-camera pose estimation ---------\n");
}
return ret;
}
bool intraCamLMEpiWeightedProc(const double K[9],
const double invK[9],
const double R0[9],
const double t0[3],
int n3D2D,
const double Ws[],
const double Ms[],
const double ms[],
int n2D2D,
const double Rpre[],
const double tpre[],
const double uWs[],
const double umspre[],
const double ums[],
double R_new[9],
double t_new[3],
IntraCamPoseOption* opt) {
assert(opt);
double param[6];
//compute the reprojection error
opt->npts = n3D2D;
opt->lambda = opt->lambda0;
double err1 = reprojError2Weighted(K, R0, t0, n3D2D, Ws, Ms, ms);
double err2 = _epiError2Weighted(invK, n2D2D, uWs, Rpre, tpre, umspre, R0, t0, ums);
opt->err0 = err1 + err2;
opt->err = opt->err0;
if (opt->verboseLM > 0) {
printf("[%d]err:%lf (%lf,%lf), lambda:%lf\n", -1, opt->err, err1, err2, opt->lambda);
}
double R[9], t[3], R_opt[9], t_opt[3];
doubleArrCopy(R, 0, R0, 9);
doubleArrCopy(t, 0, t0, 3);
opt->retTypeLM = 1;
int i = 0;
for (; i < opt->maxIterLM; i++) {
intraCamEpiWeightedLMStep(K, invK, R, t, n3D2D, Ws, Ms, ms, n2D2D, Rpre, tpre, uWs, umspre, ums, param,
opt->lambda);
intraCamUpdatePose(R, t, param, R_new, t_new);
err1 = reprojError2Weighted(K, R_new, t_new, n3D2D, Ws, Ms, ms);
err2 = _epiError2Weighted(invK, n2D2D, uWs, Rpre, tpre, umspre, R_new, t_new, ums);
double err = err1 + err2;
if (opt->verboseLM > 0)
printf("[%d]err:%lf (%lf,%lf), lambda:%lf\n", i, err, err1, err2, opt->lambda);
if (fabs(err - opt->err) < opt->epsErrorChangeLM) {
opt->retTypeLM = 0;
break;
}
if (err < opt->err) {
//accept the new parameter
doubleArrCopy(R, 0, R_new, 9);
doubleArrCopy(t, 0, t_new, 3);
doubleArrCopy(R_opt, 0, R_new, 9);
doubleArrCopy(t_opt, 0, t_new, 3);
opt->err = err;
opt->lambda /= 10;
} else {
opt->lambda *= 100;
if (opt->lambda > 1e+4) {
//cannot find the optimum
opt->retTypeLM = -1;
break;
}
}
}
// if (opt->retTypeLM == -1) {
// doubleArrCopy(R_new, 0, R_opt, 9);
// doubleArrCopy(t_new, 0, t_opt, 3);
// }
opt->nIterLM = i;
return opt->retTypeLM >= 0;
}
