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

whitenJacobians

release/4.3a0
dellaert 2015-03-01 14:25:32 +01:00
parent 754e8447b1
commit 8fe612ca71
2 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/slam/SmartFactorBase.h
View File

@ -487,6 +487,15 @@ public:
updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<Dim,Dim> (i1,i2) = ... updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<Dim,Dim> (i1,i2) = ...
} }
/// Whiten the Jacobians computed by computeJacobians using noiseModel_
void whitenJacobians(std::vector<KeyMatrix2D>& F, Matrix& E,
Vector& b) const {
noiseModel_->WhitenSystem(E, b);
// TODO make WhitenInPlace work with any dense matrix type
BOOST_FOREACH(KeyMatrix2D& Fblock,F)
Fblock.second = noiseModel_->Whiten(Fblock.second);
}
/** /**
* Return Jacobians as RegularImplicitSchurFactor with raw access * Return Jacobians as RegularImplicitSchurFactor with raw access
*/ */
@ -497,11 +506,8 @@ public:
Matrix E; Matrix E;
Vector b; Vector b;
computeJacobians(F, E, b, cameras, point); computeJacobians(F, E, b, cameras, point);
noiseModel_->WhitenSystem(E, b); whitenJacobians(F, E, b);
Matrix3 P = PointCov(E, lambda, diagonalDamping); Matrix3 P = PointCov(E, lambda, diagonalDamping);
// TODO make WhitenInPlace work with any dense matrix type
BOOST_FOREACH(KeyMatrix2D& Fblock,F)
Fblock.second = noiseModel_->Whiten(Fblock.second);
return boost::make_shared<RegularImplicitSchurFactor<Dim> >(F, E, P, b); return boost::make_shared<RegularImplicitSchurFactor<Dim> >(F, E, P, b);
} }

1
gtsam/slam/SmartProjectionFactor.h
View File

@ -280,6 +280,7 @@ public:
{ {
std::vector<typename Base::KeyMatrix2D> Fblocks; std::vector<typename Base::KeyMatrix2D> Fblocks;
f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
Base::whitenJacobians(Fblocks,E,b);
Base::FillDiagonalF(Fblocks, F); // expensive ! Base::FillDiagonalF(Fblocks, F); // expensive !
} }