From 2b1a53347797436a02b06c190583cdbbe5367ab9 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 7 Aug 2013 13:29:35 +0000 Subject: [PATCH] Fixing issues with boost::assign --- gtsam/inference/BayesTreeCliqueBase-inst.h | 3 ++- gtsam/inference/ISAM-inst.h | 3 ++- gtsam/linear/HessianFactor.cpp | 17 +++++++++++++++-- gtsam/linear/JacobianFactor-inl.h | 14 ++++++++++++-- gtsam/linear/JacobianFactor.cpp | 3 ++- gtsam/linear/KalmanFilter.cpp | 6 ++++-- 6 files changed, 37 insertions(+), 9 deletions(-) diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 90ca98863..104d7f26f 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -160,7 +160,8 @@ namespace gtsam { gttic(BayesTreeCliqueBase_separatorMarginal); gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); // now add the parent conditional - p_Cp += parent->conditional_; // P(Fp|Sp) +// p_Cp += parent->conditional_; // P(Fp|Sp) // FIXME: assign error? + p_Cp.push_back(parent->conditional_); // The variables we want to keepSet are exactly the ones in S std::vector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index d73673755..43f82fb1b 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -41,7 +41,8 @@ namespace gtsam { // Add the orphaned subtrees BOOST_FOREACH(const sharedClique& orphan, orphans) - factors += boost::make_shared >(orphan); + factors.push_back(boost::make_shared >(orphan)); +// factors += boost::make_shared >(orphan); // eliminate into a Bayes net const VariableIndex varIndex(factors); diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 7302b0880..fc3061be3 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -168,13 +168,26 @@ GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), } /* ************************************************************************* */ -namespace { DenseIndex _getSizeHF(const Vector& m) { return m.size(); } } +namespace { +DenseIndex _getSizeHF(const Vector& m) { return m.size(); } +} /* ************************************************************************* */ HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, const std::vector& gs, double f) : -GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))) +GaussianFactor(js) //, info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))) { + +// // FIXME: Issue with boost::range::join here - replacing with simpler version + info_ = SymmetricBlockMatrix(br::join(gs | br::transformed(&_getSizeHF), std::vector(1,1))); +// info_ = SymmetricBlockMatrix(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))); // Original + // Alternate implementation +// std::vector dims(gs.size() + 1, 1); +// size_t i=0; +// BOOST_FOREACH(const Vector& m, gs) +// dims[i++] = m.size(); +// info_ = SymmetricBlockMatrix(dims); + // Get the number of variables size_t variable_count = js.size(); diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index fb66be882..94b3583dc 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -82,9 +82,19 @@ namespace gtsam { // a single '1' to add a dimension for the b vector. { using boost::adaptors::transformed; - using boost::assign::cref_list_of; +// using boost::assign::cref_list_of; namespace br = boost::range; - Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), cref_list_of<1,DenseIndex>(1)), b.size()); + // FIXME: compile error with type mismatch in boost::join +// Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), cref_list_of<1,DenseIndex>(1)), b.size()); // Original + Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), std::vector(1,1)), b.size()); + +// // Simple implementation +// std::vector dims(terms.size() + 1, 1); +// size_t i=0; +// typedef std::pair KeyMatrixPair; +// BOOST_FOREACH(const KeyMatrixPair& p, terms) +// dims[i++] = p.second.cols(); +// Ab_ = VerticalBlockMatrix(dims, b.size()); } // Check and add terms diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index b1cc49181..e94d860fc 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -265,7 +265,8 @@ namespace gtsam { // Allocate matrix and copy keys in order gttic(allocate); - Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1,DenseIndex>(1)), m); // Allocate augmented matrix + Ab_ = VerticalBlockMatrix(boost::join(varDims, std::vector(1,1)), m); // Allocate augmented matrix +// Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1,DenseIndex>(1)), m); // Allocate augmented matrix - original Base::keys_.resize(orderedSlots.size()); boost::range::copy( // Get variable keys orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin()); diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index c37df5ad2..aea379857 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -57,12 +57,14 @@ namespace gtsam { } /* ************************************************************************* */ - KalmanFilter::State fuse(const KalmanFilter::State& p, GaussianFactor::shared_ptr newFactor, + KalmanFilter::State fuse(KalmanFilter::State p, GaussianFactor::shared_ptr newFactor, const GaussianFactorGraph::Eliminate& function) { // Create a factor graph GaussianFactorGraph factorGraph; - factorGraph += p, newFactor; + factorGraph.push_back(p); + factorGraph.push_back(newFactor); +// factorGraph += p, newFactor; // FIXME: bad insert at some point here - fills in a template argument with a shared_ptr where a non-pointer should go // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) return solve(factorGraph, function);