diff --git a/gtsam.h b/gtsam.h index f67cb6303..9f6b0d49b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1078,11 +1078,12 @@ class GaussianConditional { size_t dim() const; //Advanced Interface - Matrix computeInformation() const; - gtsam::JacobianFactor* toFactor() const; - void solveInPlace(gtsam::VectorValues& x) const; - void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + Matrix information() const; + Matrix augmentedInformation() const; + gtsam::JacobianFactor* toFactor() const; + void solveInPlace(gtsam::VectorValues& x) const; + void solveTransposeInPlace(gtsam::VectorValues& gy) const; + void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; }; class GaussianDensity { @@ -1136,6 +1137,8 @@ virtual class GaussianFactor { bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; gtsam::GaussianFactor* negate() const; + Matrix augmentedInformation() const; + Matrix information() const; size_t size() const; }; @@ -1161,31 +1164,29 @@ virtual class JacobianFactor : gtsam::GaussianFactor { double error(const gtsam::VectorValues& c) const; //Standard Interface - gtsam::GaussianFactor* negate() const; - bool empty() const; - Matrix getA() const; - Vector getb() const; - size_t rows() const; - size_t cols() const; - size_t numberOfRows() const; - bool isConstrained() const; - pair matrix() const; - Matrix matrix_augmented() const; + bool empty() const; + Matrix getA() const; + Vector getb() const; + size_t rows() const; + size_t cols() const; + size_t numberOfRows() const; + bool isConstrained() const; + pair matrix() const; + Matrix matrix_augmented() const; - gtsam::GaussianConditional* eliminateFirst(); - gtsam::GaussianConditional* eliminate(size_t nrFrontals); - gtsam::GaussianFactor* negate() const; - Matrix computeInformation() const; - void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; - gtsam::JacobianFactor whiten() const; - gtsam::GaussianConditional* eliminateFirst(); - gtsam::GaussianConditional* eliminate(size_t nFrontals); - gtsam::GaussianConditional* splitConditional(size_t nFrontals); + gtsam::GaussianConditional* eliminateFirst(); + gtsam::GaussianConditional* eliminate(size_t nrFrontals); + gtsam::GaussianFactor* negate() const; + void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; + gtsam::JacobianFactor whiten() const; + gtsam::GaussianConditional* eliminateFirst(); + gtsam::GaussianConditional* eliminate(size_t nFrontals); + gtsam::GaussianConditional* splitConditional(size_t nFrontals); - void setModel(bool anyConstrained, const Vector& sigmas); - void assertInvariants() const; + void setModel(bool anyConstrained, const Vector& sigmas); + void assertInvariants() const; - //gtsam::SharedDiagonal& get_model(); + //gtsam::SharedDiagonal& get_model(); }; virtual class HessianFactor : gtsam::GaussianFactor { @@ -1211,16 +1212,14 @@ virtual class HessianFactor : gtsam::GaussianFactor { //Standard Interface size_t rows() const; - gtsam::GaussianFactor* negate() const; Matrix info() const; double constantTerm() const; Vector linearTerm() const; - Matrix computeInformation() const; //Advanced Interface void partialCholesky(size_t nrFrontals); - gtsam::GaussianConditional* splitEliminatedFactor(size_t nrFrontals); - void assertInvariants() const; + gtsam::GaussianConditional* splitEliminatedFactor(size_t nrFrontals); + void assertInvariants() const; }; class GaussianFactorGraph { diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 10834d55a..46365ff6b 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -31,7 +31,7 @@ class eliminate2JacobianFactorTest; class constructorGaussianConditionalTest; class eliminationGaussianFactorGraphTest; class complicatedMarginalGaussianJunctionTreeTest; -class computeInformationGaussianConditionalTest; +class informationGaussianConditionalTest; class isGaussianFactorGaussianConditionalTest; namespace gtsam { @@ -147,8 +147,15 @@ public: /** dimension of multivariate variable */ size_t dim() const { return rsd_.rows(); } + /** Compute the augmented information matrix as + * \f$ [ R S d ]^T [ R S d ] \f$ + */ + Matrix augmentedInformation() const { + return rsd_.full().transpose() * rsd_.full().transpose(); + } + /** Compute the information matrix */ - Matrix computeInformation() const { + Matrix information() const { return get_R().transpose() * get_R(); } @@ -217,7 +224,7 @@ private: friend class ::constructorGaussianConditionalTest; friend class ::eliminationGaussianFactorGraphTest; friend class ::complicatedMarginalGaussianJunctionTreeTest; - friend class ::computeInformationGaussianConditionalTest; + friend class ::informationGaussianConditionalTest; friend class ::isGaussianFactorGaussianConditionalTest; /** Serialization function */ diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index 748914680..a8f2fc895 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -45,11 +45,6 @@ namespace gtsam { return x[k]; } - /* ************************************************************************* */ - Matrix GaussianDensity::information() const { - return computeInformation(); - } - /* ************************************************************************* */ Matrix GaussianDensity::covariance() const { return inverse(information()); diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 01b6fcdf0..0a737d501 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -60,9 +60,6 @@ namespace gtsam { /// Mean \f$ \mu = R^{-1} d \f$ Vector mean() const; - /// Information matrix \f$ \Lambda = R^T R \f$ - Matrix information() const; - /// Covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$ Matrix covariance() const; diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index a8c69aba5..8b3fc3678 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -101,7 +101,12 @@ namespace gtsam { * augmented information matrix is described in more detail in HessianFactor, * which in fact stores an augmented information matrix. */ - virtual Matrix computeInformation() const = 0; + virtual Matrix augmentedInformation() const = 0; + + /** Return the non-augmented information matrix represented by this + * GaussianFactor. + */ + virtual Matrix information() const = 0; /** Clone a factor (make a deep copy) */ virtual GaussianFactor::shared_ptr clone() const = 0; diff --git a/gtsam/linear/GaussianISAM.cpp b/gtsam/linear/GaussianISAM.cpp index 396ae139a..17371ffc1 100644 --- a/gtsam/linear/GaussianISAM.cpp +++ b/gtsam/linear/GaussianISAM.cpp @@ -42,7 +42,7 @@ BayesNet::shared_ptr GaussianISAM::marginalBayesNet(Index j /* ************************************************************************* */ Matrix GaussianISAM::marginalCovariance(Index j) const { GaussianConditional::shared_ptr conditional = marginalBayesNet(j)->front(); - return conditional->computeInformation().inverse(); + return conditional->information().inverse(); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianMultifrontalSolver.cpp b/gtsam/linear/GaussianMultifrontalSolver.cpp index 97bfa118e..84eadec86 100644 --- a/gtsam/linear/GaussianMultifrontalSolver.cpp +++ b/gtsam/linear/GaussianMultifrontalSolver.cpp @@ -88,7 +88,7 @@ Matrix GaussianMultifrontalSolver::marginalCovariance(Index j) const { fg.push_back(Base::marginalFactor(j,&EliminatePreferCholesky)); conditional = EliminatePreferCholesky(fg,1).first; } - return conditional->computeInformation().inverse(); + return conditional->information().inverse(); } } diff --git a/gtsam/linear/GaussianSequentialSolver.cpp b/gtsam/linear/GaussianSequentialSolver.cpp index d631a2a63..ab6984cd6 100644 --- a/gtsam/linear/GaussianSequentialSolver.cpp +++ b/gtsam/linear/GaussianSequentialSolver.cpp @@ -104,7 +104,7 @@ Matrix GaussianSequentialSolver::marginalCovariance(Index j) const { fg.push_back(Base::marginalFactor(j, &EliminatePreferCholesky)); conditional = EliminatePreferCholesky(fg, 1).first; } - return conditional->computeInformation().inverse(); + return conditional->information().inverse(); } /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index ff34ca9f9..9b97cf6bf 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -311,10 +311,15 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { } /* ************************************************************************* */ -Matrix HessianFactor::computeInformation() const { +Matrix HessianFactor::augmentedInformation() const { return info_.full().selfadjointView(); } +/* ************************************************************************* */ +Matrix HessianFactor::information() const { + return info_.range(0, this->size(), 0, this->size()).selfadjointView(); +} + /* ************************************************************************* */ double HessianFactor::error(const VectorValues& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 8dd3a6294..63dfb15a1 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -311,7 +311,12 @@ namespace gtsam { * representation of the augmented information matrix, which stores only the * upper triangle. */ - virtual Matrix computeInformation() const; + virtual Matrix augmentedInformation() const; + + /** Return the non-augmented information matrix represented by this + * GaussianFactor. + */ + virtual Matrix information() const; // Friend unit test classes friend class ::ConversionConstructorHessianFactorTest; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 529df2454..2ae8b3eb1 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -281,12 +281,19 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix JacobianFactor::computeInformation() const { + Matrix JacobianFactor::augmentedInformation() const { Matrix AbWhitened = Ab_.full(); model_->WhitenInPlace(AbWhitened); return AbWhitened.transpose() * AbWhitened; } + /* ************************************************************************* */ + Matrix JacobianFactor::information() const { + Matrix AWhitened = this->getA(); + model_->WhitenInPlace(AWhitened); + return AWhitened.transpose() * AWhitened; + } + /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { Vector Ax = zero(Ab_.rows()); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d3bd32fbe..61ba5f081 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -162,7 +162,12 @@ namespace gtsam { * augmented information matrix is described in more detail in HessianFactor, * which in fact stores an augmented information matrix. */ - virtual Matrix computeInformation() const; + virtual Matrix augmentedInformation() const; + + /** Return the non-augmented information matrix represented by this + * GaussianFactor. + */ + virtual Matrix information() const; /** * Construct the corresponding anti-factor to negate information diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index c4ab2b9a1..86d4b28b4 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -306,7 +306,7 @@ TEST( GaussianConditional, solveTranspose ) { } /* ************************************************************************* */ -TEST( GaussianConditional, computeInformation ) { +TEST( GaussianConditional, information ) { // Create R matrix Matrix R = (Matrix(4,4) << @@ -322,7 +322,7 @@ TEST( GaussianConditional, computeInformation ) { Matrix IExpected = R.transpose() * R; // Actual information matrix (conditional should permute R) - Matrix IActual = conditional.computeInformation(); + Matrix IActual = conditional.information(); EXPECT(assert_equal(IExpected, IActual)); } @@ -340,7 +340,7 @@ TEST( GaussianConditional, isGaussianFactor ) { GaussianConditional conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0)); // Expected information matrix computed by conditional - Matrix IExpected = conditional.computeInformation(); + Matrix IExpected = conditional.information(); // Expected information matrix computed by a factor JacobianFactor jf = *conditional.toFactor(); diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 25221a5a0..11a45cdd4 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -71,9 +71,7 @@ Matrix Marginals::marginalInformation(Key variable) const { marginalFactor = bayesTree_.marginalFactor(index, EliminateQR); // Get information matrix (only store upper-right triangle) - Matrix info = marginalFactor->computeInformation(); - const int dim = info.rows() - 1; - return info.topLeftCorner(dim,dim); // Take the non-augmented part of the information matrix + return marginalFactor->information(); } /* ************************************************************************* */