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

Cleaned up information/augmentedInformation for GaussianFactor and GaussianConditional

release/4.3a0
Richard Roberts 2012-10-08 22:40:37 +00:00
parent 550e683cad
commit eb21cf0911
14 changed files with 79 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
gtsam.h
View File

@ -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, Vector> 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, Vector> 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 {

13
gtsam/linear/GaussianConditional.h
View File

@ -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 */

5
gtsam/linear/GaussianDensity.cpp
View File

@ -45,11 +45,6 @@ namespace gtsam {
return x[k];
}
/* ************************************************************************* */
Matrix GaussianDensity::information() const {
return computeInformation();
}
/* ************************************************************************* */
Matrix GaussianDensity::covariance() const {
return inverse(information());

3
gtsam/linear/GaussianDensity.h
View File

@ -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;

7
gtsam/linear/GaussianFactor.h
View File

@ -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;

2
gtsam/linear/GaussianISAM.cpp
View File

@ -42,7 +42,7 @@ BayesNet<GaussianConditional>::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();
}
/* ************************************************************************* */

2
gtsam/linear/GaussianMultifrontalSolver.cpp
View File

@ -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();
}
}

2
gtsam/linear/GaussianSequentialSolver.cpp
View File

@ -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();
}
/* ************************************************************************* */

7
gtsam/linear/HessianFactor.cpp
View File

@ -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<Eigen::Upper>();
}
/* ************************************************************************* */
Matrix HessianFactor::information() const {
return info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>();
}
/* ************************************************************************* */
double HessianFactor::error(const VectorValues& c) const {
// error 0.5*(f - 2*x'*g + x'*G*x)

7
gtsam/linear/HessianFactor.h
View File

@ -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;

9
gtsam/linear/JacobianFactor.cpp
View File

@ -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());

7
gtsam/linear/JacobianFactor.h
View File

@ -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

6
gtsam/linear/tests/testGaussianConditional.cpp
View File

@ -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();

4
gtsam/nonlinear/Marginals.cpp
View File

@ -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();
}
/* ************************************************************************* */