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

comments and formatting

release/4.3a0
Alex Cunningham 2012-06-21 15:46:39 +00:00
parent fcde937b62
commit 282dc6d788
5 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/inference/GenericMultifrontalSolver-inl.h
View File

@ -47,13 +47,16 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate(Eliminate function) const {
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate(Eliminate function) const {
// eliminate junction tree, returns pointer to root
typename BayesTree<typename FACTOR::ConditionalType>::sharedClique root = junctionTree_->eliminate(function);
typename BayesTree<typename FACTOR::ConditionalType>::sharedClique
root = junctionTree_->eliminate(function);
// create an empty Bayes tree and insert root clique
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr bayesTree(new BayesTree<typename FACTOR::ConditionalType>);
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr
bayesTree(new BayesTree<typename FACTOR::ConditionalType>);
bayesTree->insert(root);
// return the Bayes tree
@ -65,7 +68,8 @@ namespace gtsam {
typename FactorGraph<F>::shared_ptr GenericMultifrontalSolver<F, JT>::jointFactorGraph(
const std::vector<Index>& js, Eliminate function) const {
// We currently have code written only for computing the
// FIXME: joint for arbitrary sets of variables not present
// TODO: develop and implement theory for shortcuts of more than two variables
if (js.size() != 2) throw std::domain_error(
"*MultifrontalSolver::joint(js) currently can only compute joint marginals\n"

8
gtsam/inference/GenericSequentialSolver-inl.h
View File

@ -96,7 +96,7 @@ namespace gtsam {
// copies. Other parts of the code may hold shared_ptr's to these factors so
// we must undo the permutation before returning.
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
if (factor) factor->permuteWithInverse(*permutationInverse);
if (factor) factor->permuteWithInverse(*permutationInverse);
// Eliminate all variables
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr
@ -104,20 +104,20 @@ namespace gtsam {
// Undo the permuation on the original factors and on the structure.
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
if (factor) factor->permuteWithInverse(*permutation);
if (factor) factor->permuteWithInverse(*permutation);
// Take the joint marginal from the Bayes net.
sharedFactorGraph joint(new FactorGraph<FACTOR> );
joint->reserve(js.size());
typename BayesNet<typename FACTOR::ConditionalType>::const_reverse_iterator
conditional = bayesNet->rbegin();
conditional = bayesNet->rbegin();
for (size_t i = 0; i < js.size(); ++i)
joint->push_back((*(conditional++))->toFactor());
// Undo the permutation on the eliminated joint marginal factors
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *joint)
factor->permuteWithInverse(*permutation);
factor->permuteWithInverse(*permutation);
return joint;
}

2
gtsam/inference/IndexFactor.cpp
View File

@ -58,7 +58,7 @@ namespace gtsam {
/* ************************************************************************* */
void IndexFactor::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(Index& key, keys())
key = inversePermutation[key];
key = inversePermutation[key];
assertInvariants();
}
/* ************************************************************************* */

4
gtsam/linear/GaussianFactor.h
View File

@ -107,7 +107,9 @@ namespace gtsam {
* to already be inverted. This acts just as a change-of-name for each
* variable. The order of the variables within the factor is not changed.
*/
virtual void permuteWithInverse(const Permutation& inversePermutation) { IndexFactor::permuteWithInverse(inversePermutation); }
virtual void permuteWithInverse(const Permutation& inversePermutation) {
IndexFactor::permuteWithInverse(inversePermutation);
}
private:
/** Serialization function */

3
gtsam/nonlinear/NonlinearISAM.cpp
View File

@ -53,6 +53,7 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
linPoint_.insert(initialValues);
// Augment ordering
// TODO: allow for ordering constraints within the new variables
// FIXME: should just loop over new values
BOOST_FOREACH(const NonlinearFactorGraph::sharedFactor& factor, newFactors)
BOOST_FOREACH(Key key, factor->keys())
@ -77,9 +78,11 @@ void NonlinearISAM::reorder_relinearize() {
isam_.clear();
// Compute an ordering
// TODO: allow for constrained ordering here
ordering_ = *factors_.orderingCOLAMD(newLinPoint);
// Create a linear factor graph at the new linearization point
// TODO: decouple relinearization and reordering to avoid
boost::shared_ptr<GaussianFactorGraph> gfg = factors_.linearize(newLinPoint, ordering_);
// Just recreate the whole BayesTree