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

Added print, moved instance variables

release/4.3a0
Frank Dellaert 2012-05-22 17:58:27 +00:00
parent 67892e59e0
commit 9394ede66b
2 changed files with 32 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/nonlinear/Marginals.cpp
View File

@ -43,6 +43,14 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate();
}
/* ************************************************************************* */
void Marginals::print(const std::string& str, const KeyFormatter& keyFormatter) const {
ordering_.print(str+"Ordering: ", keyFormatter);
graph_.print(str+"Graph: ");
values_.print(str+"Solution: ", keyFormatter);
bayesTree_.print(str+"Bayes Tree: ");
}
/* ************************************************************************* */
Matrix Marginals::marginalCovariance(Key variable) const {
return marginalInformation(variable).inverse();

43
gtsam/nonlinear/Marginals.h
View File

@ -40,6 +40,16 @@ public:
QR
};
protected:
GaussianFactorGraph graph_;
Ordering ordering_;
Values values_;
Factorization factorization_;
GaussianBayesTree bayesTree_;
public:
/** Construct a marginals class.
* @param graph The factor graph defining the full joint density on all variables.
* @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer).
@ -47,6 +57,9 @@ public:
*/
Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
/** print */
void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Compute the marginal covariance of a single variable */
Matrix marginalCovariance(Key variable) const;
@ -60,14 +73,6 @@ public:
/** Compute the joint marginal information of several variables */
JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
protected:
GaussianFactorGraph graph_;
Values values_;
Ordering ordering_;
Factorization factorization_;
GaussianBayesTree bayesTree_;
};
/**
@ -76,7 +81,17 @@ protected:
class JointMarginal {
protected:
typedef SymmetricBlockView<Matrix> BlockView;
typedef SymmetricBlockView<Matrix> BlockView;
Matrix fullMatrix_;
BlockView blockView_;
Ordering indices_;
JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const Ordering& indices) :
fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {}
friend class Marginals;
public:
/** A block view of the joint marginal - this stores a reference to the
@ -101,16 +116,6 @@ public:
*/
Block operator()(Key iVariable, Key jVariable) const {
return blockView_(indices_[iVariable], indices_[jVariable]); }
protected:
Matrix fullMatrix_;
BlockView blockView_;
Ordering indices_;
JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const Ordering& indices) :
fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {}
friend class Marginals;
};
} /* namespace gtsam */