12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed Hessian by whitening...

release/4.3a0
dellaert 2015-03-11 06:33:16 -07:00
parent 2d1126019a
commit 766a9622e8
1 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/slam/SmartProjectionFactor.h
View File

@ -204,11 +204,15 @@ public:
Gs, gs, 0.0); Gs, gs, 0.0);
} }
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
std::vector<typename Base::MatrixZD> Fblocks; std::vector<typename Base::MatrixZD> Fblocks;
Matrix E; Matrix E;
Vector b; Vector b;
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
// Whiten using noise model
Base::whitenJacobians(Fblocks, E, b);
// build augmented hessian // build augmented hessian
SymmetricBlockMatrix augmentedHessian = // SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);