diff --git a/gtsam/inference/GenericMultifrontalSolver-inl.h b/gtsam/inference/GenericMultifrontalSolver-inl.h index 192436959..db18f2093 100644 --- a/gtsam/inference/GenericMultifrontalSolver-inl.h +++ b/gtsam/inference/GenericMultifrontalSolver-inl.h @@ -47,13 +47,16 @@ namespace gtsam { /* ************************************************************************* */ template - typename BayesTree::shared_ptr GenericMultifrontalSolver::eliminate(Eliminate function) const { + typename BayesTree::shared_ptr + GenericMultifrontalSolver::eliminate(Eliminate function) const { // eliminate junction tree, returns pointer to root - typename BayesTree::sharedClique root = junctionTree_->eliminate(function); + typename BayesTree::sharedClique + root = junctionTree_->eliminate(function); // create an empty Bayes tree and insert root clique - typename BayesTree::shared_ptr bayesTree(new BayesTree); + typename BayesTree::shared_ptr + bayesTree(new BayesTree); bayesTree->insert(root); // return the Bayes tree @@ -65,7 +68,8 @@ namespace gtsam { typename FactorGraph::shared_ptr GenericMultifrontalSolver::jointFactorGraph( const std::vector& 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" diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index cc7e6146c..53dee3dcd 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -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, *factors_) - if (factor) factor->permuteWithInverse(*permutationInverse); + if (factor) factor->permuteWithInverse(*permutationInverse); // Eliminate all variables typename BayesNet::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, *factors_) - if (factor) factor->permuteWithInverse(*permutation); + if (factor) factor->permuteWithInverse(*permutation); // Take the joint marginal from the Bayes net. sharedFactorGraph joint(new FactorGraph ); joint->reserve(js.size()); typename BayesNet::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, *joint) - factor->permuteWithInverse(*permutation); + factor->permuteWithInverse(*permutation); return joint; } diff --git a/gtsam/inference/IndexFactor.cpp b/gtsam/inference/IndexFactor.cpp index f74aa4ff5..b04338d5b 100644 --- a/gtsam/inference/IndexFactor.cpp +++ b/gtsam/inference/IndexFactor.cpp @@ -58,7 +58,7 @@ namespace gtsam { /* ************************************************************************* */ void IndexFactor::permuteWithInverse(const Permutation& inversePermutation) { BOOST_FOREACH(Index& key, keys()) - key = inversePermutation[key]; + key = inversePermutation[key]; assertInvariants(); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index e2537b7b4..b1fd9338e 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -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 */ diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 6820b3e05..17fba2432 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -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 gfg = factors_.linearize(newLinPoint, ordering_); // Just recreate the whole BayesTree