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

Augmented and regular information now implemented

release/4.3a0
dellaert 2015-03-01 17:15:42 +01:00
parent bb58814f1c
commit 05fcbe6556
2 changed files with 37 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
/**
* RegularImplicitSchurFactor
*/
template<size_t D, size_t Z=2> //
template<size_t D, size_t Z = 2> //
class RegularImplicitSchurFactor: public GaussianFactor {
public:
@ -170,8 +170,12 @@ public:
static void updateSparseSchurComplement(
const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P /*Point Covariance*/, const Vector& b, const double f,
const FastVector<Key>& keys, const FastMap<Key, size_t>& KeySlotMap,
const FastVector<Key>& allKeys, const FastVector<Key>& keys,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) {
FastMap<Key, size_t> KeySlotMap;
for (size_t slot = 0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
// Schur complement trick
// G = F' * F - F' * E * P * E' * F
// g = F' * (b - E * P * E' * b)
@ -232,15 +236,28 @@ public:
augmentedHessian(aug_m, aug_m)(0, 0) += f;
}
/// *Compute* full augmented information matrix
virtual Matrix augmentedInformation() const {
throw std::runtime_error(
"RegularImplicitSchurFactor::augmentedInformation non implemented");
return Matrix();
// Create a SymmetricBlockMatrix
int m = this->keys_.size();
size_t M1 = D * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Do the Schur complement
sparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian);
return augmentedHessian.matrix();
}
/// *Compute* full information matrix
virtual Matrix information() const {
throw std::runtime_error(
"RegularImplicitSchurFactor::information non implemented");
return Matrix();
Matrix augmented = augmentedInformation();
int m = this->keys_.size();
size_t M = D * m;
return augmented.block(0,0,M,M);
}
/// Return the diagonal of the Hessian for this factor

12
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -255,6 +255,18 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0]));
EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
// augmentedInformation (test just checks diagonals)
Matrix actualInfo = factor.augmentedInformation();
EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0)));
EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6)));
EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12)));
// information (test just checks diagonals)
Matrix actualInfo2 = factor.information();
EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0)));
EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6)));
EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12)));
}
/* ************************************************************************* */