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) = ...
}
/// 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
*/
@ -497,11 +506,8 @@ public:
Matrix E;
Vector b;
computeJacobians(F, E, b, cameras, point);
noiseModel_->WhitenSystem(E, b);
whitenJacobians(F, E, b);
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);
}

1
gtsam/slam/SmartProjectionFactor.h
View File

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