13 inline bool isnan(
double d) {
return !(d==d);}
16 #define cout if(mgvnBundleCout) cout
22 for(
int r=0; r<6; r++)
23 for(
int c=0; c<=r; c++)
24 m6U(r,c)+= m26A.T()(r,0)*m26A(0,c) + m26A.T()(r,1)*m26A(1,c);
28 for(
int r=0; r<3; r++)
29 for(
int c=0; c<=r; c++)
30 m3V(r,c)+= m23B.T()(r,0)*m23B(0,c) + m23B.T()(r,1)*m23B(1,c);
50 c.
se3CfW = se3CamFromWorld;
83 assert(nPoint < (
int)
mvPoints.size());
85 mvPoints[nPoint].sCameras.insert(nCam);
97 for(
size_t i=0; i<
mvPoints.size(); ++i)
129 double lastMdSigmaSquared = 0;
137 if(gvsMEstimator ==
"Cauchy")
138 bNoError = Do_LM_Step<Cauchy>(pbAbortSignal, &lastMdSigmaSquared);
139 else if(gvsMEstimator ==
"Tukey")
140 bNoError = Do_LM_Step<Tukey>(pbAbortSignal, &lastMdSigmaSquared);
141 else if(gvsMEstimator ==
"Huber")
142 bNoError = Do_LM_Step<Huber>(pbAbortSignal, &lastMdSigmaSquared);
145 cout <<
"Invalid BundleMEstimator selected !! " << endl;
146 cout <<
"Defaulting to Tukey." << endl;
147 gvsMEstimator =
"Tukey";
148 bNoError = Do_LM_Step<Tukey>(pbAbortSignal, &lastMdSigmaSquared);
157 cout <<
" Hit max iterations." << endl;
173 if(meas.
v3Cam[2] <= 0)
186 template<
class MEstimator>
195 vector<double> vdErrorSquared;
212 const double dMinSigmaSquared = gvdMinSigma * gvdMinSigma;
227 double dCurrentError = 0.0;
238 dCurrentError += 1.0;
254 dCurrentError += 1.0;
264 const double dOneOverCameraZ = 1.0 / meas.
v3Cam[2];
276 v2CamFrameMotion[0] = (v4Motion[0] - v4Cam[0] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
277 v2CamFrameMotion[1] = (v4Motion[1] - v4Cam[1] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
287 v2CamFrameMotion[0] = (v3Motion[0] - v4Cam[0] * v3Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
288 v2CamFrameMotion[1] = (v3Motion[1] - v4Cam[1] * v3Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
314 double dNewError = dCurrentError + 9999;
318 for(vector<ExtendedMapPoint>::iterator itr =
mvPoints.begin(); itr!=
mvPoints.end(); itr++)
322 if(m3VStar[0][0] * m3VStar[1][1] * m3VStar[2][2] == 0)
327 m3VStar[0][1] = m3VStar[1][0];
328 m3VStar[0][2] = m3VStar[2][0];
329 m3VStar[1][2] = m3VStar[2][1];
331 for(
int i=0; i<3; i++)
351 for(
unsigned int j=0; j<
mvCameras.size(); j++)
354 if(cam_j.
bFixed)
continue;
359 for(
int r=0; r<6; r++)
361 for(
int c=0; c<r; c++)
362 m6[r][c] = m6[c][r] = cam_j.
m6U[r][c];
363 m6[r][r] = cam_j.
m6U[r][r];
366 for(
int nn = 0; nn< 6; nn++)
373 for(
unsigned int i=0; i<
mvPoints.size(); i++)
375 Meas* pMeas = vMeasLUTj[i];
376 if(pMeas == NULL || pMeas->
bBad)
381 mS.slice(nCamJStartRow, nCamJStartRow, 6, 6) = m6;
382 vE.slice(nCamJStartRow,6) = v6;
387 for(
unsigned int i=0; i<
mvPoints.size(); i++)
402 if(pMeas_ik == NULL || pMeas_ik->
bBad)
407 if(pMeas_ij == NULL || pMeas_ij->
bBad)
415 mS.slice(nJRow, nKRow, 6, 6) -= m63_MIJW_times_m3VStarInv * pMeas_ik->
m63W.T();
417 Matrix<6> m = mS.slice(nJRow, nKRow, 6, 6);
418 m -= m63_MIJW_times_m3VStarInv * pMeas_ik->
m63W.T();
419 mS.slice(nJRow, nKRow, 6, 6) = m;
421 assert(nKRow < nJRow);
428 for(
int i=0; i<mS.num_rows(); i++)
429 for(
int j=0; j<i; j++)
434 Vector<> vCamerasUpdate(mS.num_rows());
439 for(
unsigned int i=0; i<
mvPoints.size(); i++)
443 for(
unsigned int j=0; j<
mvCameras.size(); j++)
449 if(pMeas == NULL || pMeas->
bBad)
451 v3Sum+=pMeas->
m63W.T() * vCamerasUpdate.slice(cam.
nStartRow,6);
454 vMapUpdates.slice(i * 3, 3) =
mvPoints[i].m3VStarInv * v3;
465 double dSumSquaredUpdate = vCamerasUpdate * vCamerasUpdate + vMapUpdates * vMapUpdates;
466 LOGV(
LOG_TAG,
"Info: dSumSquaredUpdate: %g lastMdSigmaSquared: %g, difference: %g", dSumSquaredUpdate, *lastMdSigmaSquared, dSumSquaredUpdate - (*lastMdSigmaSquared));
467 *lastMdSigmaSquared = dSumSquaredUpdate;
476 for(
unsigned int j=0; j<
mvCameras.size(); j++)
483 for(
unsigned int i=0; i<
mvPoints.size(); i++)
486 dNewError = FindNewError<MEstimator>();
488 cout <<setprecision(1) <<
"L" <<
mdLambda << setprecision(3) <<
"\tOld " << dCurrentError <<
" New " << dNewError <<
" Diff " << dCurrentError - dNewError <<
"\t";
492 if(dNewError > dCurrentError)
494 cout <<
" TRY AGAIN " << endl;
504 if(dNewError < dCurrentError)
506 cout <<
" WINNER ------------ " << endl;
509 for(
unsigned int j=0; j<
mvCameras.size(); j++)
511 for(
unsigned int i=0; i<
mvPoints.size(); i++)
517 vector<list<Meas>::iterator> vit;
527 for(
unsigned int i=0; i<vit.size(); i++)
530 cout <<
"Nuked " << vit.size() <<
" measurements." << endl;
536 template<
class MEstimator>
540 double dNewError = 0;
541 vector<double> vdErrorSquared;
556 double dErrorSquared = v2Error * v2Error;
557 dNewError += MEstimator::ObjectiveScore(dErrorSquared,
mdSigmaSquared);
568 for(
unsigned int nCam = 0; nCam <
mvCameras.size(); nCam++)
582 for(
unsigned int i=0; i<
mvPoints.size(); i++)
586 for(set<int>::iterator it_j = p.
sCameras.begin(); it_j!=p.
sCameras.end(); it_j++)
592 assert(pMeas_j != NULL);
594 for(set<int>::iterator it_k = p.
sCameras.begin(); it_k!=it_j; it_k++)
601 assert(pMeas_k != NULL);
621 mdLambdaFactor = mdLambdaFactor * 2;
638 set<int>::iterator hint = sOutliers.begin();
639 for(
unsigned int i=0; i<
mvPoints.size(); i++)
643 hint = sOutliers.insert(hint, i);
void ModifyLambda_GoodStep()
int AddPoint(Vector< 3 > v3Pos)
#define LOGV(LOG_TAG,...)
int Compute(bool *pbAbortSignal)
SO3< Precision > & get_rotation()
Returns the rotation part of the transformation as a SO3.
Vector< 2 > Project(const Vector< 2 > &camframe)
double mgvdUpdateConvergenceLimit
const std::string BundleMEstimator
void ProjectAndFindSquaredError(Meas &meas)
Vector<(Size==Dynamic?Dynamic:Size+1), Precision > unproject(const Vector< Size, Precision, Base > &v)
bool Do_LM_Step(bool *pbAbortSignal, double *lastMdSigmaSquared)
Matrix< Size, Size, Precision > get_inverse()
int AddCamera(SE3<> se3CamFromWorld, bool bFixed)
void GenerateOffDiagScripts()
std::list< Meas > mMeasList
void AddMeas(int nCam, int nPoint, Vector< 2 > v2Pos, double dSigmaSquared)
Bundle(const ATANCamera &TCam)
Matrix< R, C, P > exp(const Matrix< R, C, P, B > &m)
std::vector< Camera > mvCameras
const double BundleMinTukeySigma
void ModifyLambda_BadStep()
std::set< int > GetOutliers()
const double BundleUpdateSquaredConvergenceLimit
std::vector< OffDiagScriptEntry > vOffDiagonalScript
void BundleTriangle_UpdateM3V_LL(Matrix< 3 > &m3V, Matrix< 2, 3 > &m23B)
Vector< 3 > GetPoint(int n)
std::vector< ExtendedMapPoint > mvPoints
std::vector< std::pair< int, int > > mvOutlierMeasurementIdx
Matrix< 2, 2 > GetProjectionDerivs()
std::vector< std::vector< Meas * > > mvMeasLUTs
static Operator< Internal::Zero > Zeros
Vector<(Size==Dynamic?Dynamic:Size-1), Precision > project(const Vector< Size, Precision, Base > &v)
bool isnan(const Vector< S, P, B > &v)
void BundleTriangle_UpdateM6U_LL(Matrix< 6 > &m6U, Matrix< 2, 6 > &m26A)
std::vector< std::pair< int, int > > GetOutlierMeasurements()
const int BundleMaxIterations