diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index af509375c..3b31906b2 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -152,6 +152,16 @@ namespace gtsam { fg.push_back(clique->conditional_); return 0; } + + template + struct _pushCliqueFunctor { + _pushCliqueFunctor(FactorGraph& graph_) : graph(graph_) {} + FactorGraph& graph; + int operator()(const boost::shared_ptr& clique, int dummy) { + graph.push_back(clique->conditional_); + return 0; + } + }; } /* ************************************************************************* */ @@ -160,7 +170,9 @@ namespace gtsam { { // Traverse the BayesTree and add all conditionals to this graph int data = 0; // Unused - treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique, boost::ref(graph), _1)); + _pushCliqueFunctor functor(graph); + treeTraversal::DepthFirstForest(*this, data, functor); // FIXME: sort of works? +// treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique, boost::ref(graph), _1)); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index eb8cc04ed..e8dbbacb0 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -118,9 +118,7 @@ namespace gtsam { Vector soln = get_R().triangularView().solve(xS); // Check for indeterminant solution - // FIXME: can't use std::isfinite() as a templated function with gcc - if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite), boost::lambda::_1)).any()) - throw IndeterminantLinearSystemException(keys().front()); + if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front()); // Insert solution into a VectorValues VectorValues result; @@ -164,9 +162,7 @@ namespace gtsam { frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); // Check for indeterminant solution - // FIXME: can't use isfinite() as a templated function with gcc -// if(frontalVec.unaryExpr(!boost::lambda::bind(std::ptr_fun(isfinite), boost::lambda::_1)).any()) -// throw IndeterminantLinearSystemException(this->keys().front()); + if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front()); for (const_iterator it = beginParents(); it!= endParents(); it++) gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 895f31bc5..866e79f7b 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -29,10 +29,17 @@ namespace gtsam { // Forward declarations class Ordering; class JacobianFactor; + class HessianFactor; class GaussianConditional; class GaussianBayesNet; class GaussianFactorGraph; + GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys); + + GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); + // Definition of Scatter, which is an intermediate data structure used when building a // HessianFactor incrementally, to get the keys in the right order. The "scatter" is a map from // global variable indices to slot indices in the union of involved variables. We also include diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d7728c7e0..f1d66db77 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -35,6 +35,10 @@ namespace gtsam { class HessianFactor; class VectorValues; class Ordering; + class JacobianFactor; + + GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); /** * A Gaussian factor in the squared-error form. diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp index 1a0ba206f..f633df4ce 100644 --- a/gtsam/linear/linearExceptions.cpp +++ b/gtsam/linear/linearExceptions.cpp @@ -15,10 +15,11 @@ * @author Richard Roberts * @date Aug 17, 2012 */ -#pragma once #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 162f353e8..1c96eed64 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -26,6 +26,8 @@ #include +#include + #include #include // Only so Eclipse finds class definition diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index a47e6b78b..9ba1282b8 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -24,6 +24,7 @@ #pragma once +#include #include #include #include