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

Reenabled some GFG functions

release/4.3a0
Richard Roberts 2013-08-05 22:31:01 +00:00
parent aa25f7629d
commit 00c1036814
2 changed files with 52 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -23,6 +23,7 @@
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <gtsam/base/debug.h>
@ -145,55 +146,22 @@ namespace gtsam {
}
/* ************************************************************************* */
//Matrix GaussianFactorGraph::augmentedHessian() const {
// // combine all factors and get upper-triangular part of Hessian
// HessianFactor combined(*this);
// Matrix result = combined.info();
// // Fill in lower-triangular part of Hessian
// result.triangularView<Eigen::StrictlyLower>() = result.transpose();
// return result;
//}
Matrix GaussianFactorGraph::augmentedHessian() const {
// combine all factors and get upper-triangular part of Hessian
HessianFactor combined(*this);
Matrix result = combined.info();
// Fill in lower-triangular part of Hessian
result.triangularView<Eigen::StrictlyLower>() = result.transpose();
return result;
}
/* ************************************************************************* */
//std::pair<Matrix,Vector> GaussianFactorGraph::hessian() const {
// Matrix augmented = augmentedHessian();
// return make_pair(
// augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
// augmented.col(augmented.rows()-1).head(augmented.rows()-1));
//}
/* ************************************************************************* */
//GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
// GaussianFactor>& factors, size_t nrFrontals) {
// const bool debug = ISDEBUG("EliminateCholesky");
// // Form Ab' * Ab
// gttic(combine);
// HessianFactor::shared_ptr combinedFactor(new HessianFactor(factors));
// gttoc(combine);
// // Do Cholesky, note that after this, the lower triangle still contains
// // some untouched non-zeros that should be zero. We zero them while
// // extracting submatrices next.
// gttic(partial_Cholesky);
// combinedFactor->partialCholesky(nrFrontals);
// gttoc(partial_Cholesky);
// // Extract conditional and fill in details of the remaining factor
// gttic(split);
// GaussianConditional::shared_ptr conditional =
// combinedFactor->splitEliminatedFactor(nrFrontals);
// if (debug) {
// conditional->print("Extracted conditional: ");
// combinedFactor->print("Eliminated factor (L piece): ");
// }
// gttoc(split);
// combinedFactor->assertInvariants();
// return make_pair(conditional, combinedFactor);
//}
std::pair<Matrix,Vector> GaussianFactorGraph::hessian() const {
Matrix augmented = augmentedHessian();
return make_pair(
augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
augmented.col(augmented.rows()-1).head(augmented.rows()-1));
}
/* ************************************************************************* */
VectorValues GaussianFactorGraph::optimize(OptionalOrdering ordering, const Eliminate& function) const
@ -236,6 +204,31 @@ namespace gtsam {
return g;
}
/* ************************************************************************* */
Errors GaussianFactorGraph::operator*(const VectorValues& x) const {
Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back((*Ai) * x);
}
return e;
}
/* ************************************************************************* */
void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const {
multiplyInPlace(x, e.begin());
}
/* ************************************************************************* */
void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const {
Errors::iterator ei = e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
*ei = (*Ai)*x;
ei++;
}
}
/* ************************************************************************* */
bool hasConstraints(const GaussianFactorGraph& factors) {
typedef JacobianFactor J;
@ -248,53 +241,6 @@ namespace gtsam {
return false;
}
/* ************************************************************************* */
//GaussianFactorGraph::EliminationResult EliminatePreferCholesky(
// const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
// // If any JacobianFactors have constrained noise models, we have to convert
// // all factors to JacobianFactors. Otherwise, we can convert all factors
// // to HessianFactors. This is because QR can handle constrained noise
// // models but Cholesky cannot.
// if (hasConstraints(factors))
// return EliminateQR(factors, nrFrontals);
// else {
// GaussianFactorGraph::EliminationResult ret;
// gttic(EliminateCholesky);
// ret = EliminateCholesky(factors, nrFrontals);
// gttoc(EliminateCholesky);
// return ret;
// }
//} // \EliminatePreferCholesky
///* ************************************************************************* */
//Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) {
// Errors e;
// BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
// JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
// e.push_back((*Ai)*x);
// }
// return e;
//}
///* ************************************************************************* */
//void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e) {
// multiplyInPlace(fg,x,e.begin());
//}
///* ************************************************************************* */
//void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) {
// Errors::iterator ei = e;
// BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
// JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
// *ei = (*Ai)*x;
// ei++;
// }
//}
/* ************************************************************************* */
// x += alpha*A'*e
void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const

67
gtsam/linear/GaussianFactorGraph.h
View File

@ -193,7 +193,7 @@ namespace gtsam {
and the negative log-likelihood is
\f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
*/
//Matrix augmentedHessian() const;
Matrix augmentedHessian() const;
/**
* Return the dense Hessian \f$ \Lambda \f$ and information vector
@ -201,7 +201,7 @@ namespace gtsam {
* is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
* GaussianFactorGraph::augmentedHessian.
*/
//std::pair<Matrix,Vector> hessian() const;
std::pair<Matrix,Vector> hessian() const;
/** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using
* the dense elimination function specified in \c function (default EliminatePreferCholesky),
@ -254,7 +254,7 @@ namespace gtsam {
* \f$ G \f$, returning
*
* \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] */
//VectorValues optimizeGradientSearch() const;
VectorValues optimizeGradientSearch() const;
/** x = A'*e */
VectorValues transposeMultiply(const Errors& e) const;
@ -265,6 +265,15 @@ namespace gtsam {
/** return A*x-b */
Errors gaussianErrors(const VectorValues& x) const;
///** return A*x */
Errors operator*(const VectorValues& x) const;
///** In-place version e <- A*x that overwrites e. */
void multiplyInPlace(const VectorValues& x, Errors& e) const;
/** In-place version e <- A*x that takes an iterator. */
void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
/// @}
private:
@ -283,60 +292,8 @@ namespace gtsam {
*/
GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);
/**
* Densely partially eliminate with Cholesky factorization. JacobianFactors
* are left-multiplied with their transpose to form the Hessian using the
* conversion constructor HessianFactor(const JacobianFactor&).
*
* If any factors contain constrained noise models (any sigmas equal to
* zero), QR factorization will be performed instead, because our current
* implementation cannot handle constrained noise models in Cholesky
* factorization. EliminateCholesky(), on the other hand, will fail if any
* factors contain constrained noise models.
*
* Variables are eliminated in the natural order of the variable indices of in
* the factors.
* @param factors Factors to combine and eliminate
* @param nrFrontals Number of frontal variables to eliminate.
* @return The conditional and remaining factor
* \addtogroup LinearSolving
*/
//GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminatePreferCholesky(const FactorGraph<
// GaussianFactor>& factors, size_t nrFrontals = 1);
/**
* Densely partially eliminate with Cholesky factorization. JacobianFactors
* are left-multiplied with their transpose to form the Hessian using the
* conversion constructor HessianFactor(const JacobianFactor&).
*
* If any factors contain constrained noise models, this function will fail
* because our current implementation cannot handle constrained noise models
* in Cholesky factorization. The function EliminatePreferCholesky()
* automatically does QR instead when this is the case.
*
* Variables are eliminated in the natural order of the variable indices of in
* the factors.
* @param factors Factors to combine and eliminate
* @param nrFrontals Number of frontal variables to eliminate.
* @return The conditional and remaining factor
* \addtogroup LinearSolving
*/
//GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
// GaussianFactor>& factors, size_t nrFrontals = 1);
/****** Linear Algebra Opeations ******/
///** return A*x */
//GTSAM_EXPORT Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x);
///** In-place version e <- A*x that overwrites e. */
//GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e);
///** In-place version e <- A*x that takes an iterator. */
//GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e);
///* matrix-vector operations */
//GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
//GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);