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

Got rid of hardcoded Matrix3, now call dispatched Schur complement

release/4.3a0
dellaert 2015-03-10 22:08:39 -07:00
parent eedfaabe37
commit dd7d9cd6fc
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -38,7 +38,7 @@ protected:
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
const std::vector<MatrixZD> FBlocks_; ///< All ZDim*D F blocks (one for each camera) const std::vector<MatrixZD> FBlocks_; ///< All ZDim*D F blocks (one for each camera)
const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (ZDim*ZDim if degenerate) const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
const Vector b_; ///< 2m-dimensional RHS vector const Vector b_; ///< 2m-dimensional RHS vector
@ -50,7 +50,7 @@ public:
/// Construct from blocks of F, E, inv(E'*E), and RHS vector b /// Construct from blocks of F, E, inv(E'*E), and RHS vector b
RegularImplicitSchurFactor(const FastVector<Key>& keys, RegularImplicitSchurFactor(const FastVector<Key>& keys,
const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P, const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix& P,
const Vector& b) : const Vector& b) :
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
} }
@ -71,7 +71,7 @@ public:
return b_; return b_;
} }
const Matrix3& getPointCovariance() const { const Matrix& getPointCovariance() const {
return PointCovariance_; return PointCovariance_;
} }
@ -124,8 +124,8 @@ public:
virtual Matrix augmentedInformation() const { virtual Matrix augmentedInformation() const {
// Do the Schur complement // Do the Schur complement
SymmetricBlockMatrix augmentedHessian = Set::SchurComplement(FBlocks_, E_, SymmetricBlockMatrix augmentedHessian = //
PointCovariance_, b_); Set::SchurComplement(FBlocks_, E_, b_);
return augmentedHessian.matrix(); return augmentedHessian.matrix();
} }
@ -436,7 +436,7 @@ public:
VectorValues g; VectorValues g;
for (size_t k = 0; k < size(); ++k) { for (size_t k = 0; k < size(); ++k) {
Key key = keys_[k]; Key key = keys_[k];
g.insert(key, - FBlocks_[k].transpose() * e2[k]); g.insert(key, -FBlocks_[k].transpose() * e2[k]);
} }
// return it // return it