diff --git a/inference/FactorGraph-inl.h b/inference/FactorGraph-inl.h index 6cebefca9..9a6fe8bff 100644 --- a/inference/FactorGraph-inl.h +++ b/inference/FactorGraph-inl.h @@ -40,7 +40,7 @@ #define INSTANTIATE_FACTOR_GRAPH(F) \ template class FactorGraph; \ - template FactorGraph combine(const FactorGraph&, const FactorGraph&); + template FactorGraph combine(const FactorGraph&, const FactorGraph&) using namespace std; diff --git a/linear/GaussianFactorGraph.cpp b/linear/GaussianFactorGraph.cpp index aabe8cfcf..86d7efbe9 100644 --- a/linear/GaussianFactorGraph.cpp +++ b/linear/GaussianFactorGraph.cpp @@ -38,7 +38,7 @@ using namespace boost::assign; namespace gtsam { // Explicitly instantiate so we don't have to include everywhere -template class FactorGraph; +INSTANTIATE_FACTOR_GRAPH(GaussianFactor); /* ************************************************************************* */ GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : @@ -46,8 +46,7 @@ GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : } /* ************************************************************************* */ -std::set, boost::fast_pool_allocator > -GaussianFactorGraph::keys() const { +GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { std::set, boost::fast_pool_allocator > keys; BOOST_FOREACH(const sharedFactor& factor, *this) { if(factor) keys.insert(factor->begin(), factor->end()); } @@ -135,199 +134,6 @@ VectorValues GaussianFactorGraph::gradient(const VectorValues& x) const { return g; } -///* ************************************************************************* */ -//set GaussianFactorGraph::find_separator(Index key) const -//{ -// set separator; -// BOOST_FOREACH(const sharedFactor& factor,factors_) -// factor->tally_separator(key,separator); -// -// return separator; -//} - -///* ************************************************************************* */ -//template -//std::pair combineFactorsAndCreateMatrix( -// const Factors& factors, -// const Ordering& order, const Dimensions& dimensions) { -// // find the size of Ab -// size_t m = 0, n = 1; -// -// // number of rows -// BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) { -// m += factor->numberOfRows(); -// } -// -// // find the number of columns -// BOOST_FOREACH(Index key, order) { -// n += dimensions.at(key); -// } -// -// // Allocate the new matrix -// Matrix Ab = zeros(m,n); -// -// // Allocate a sigmas vector to make into a full noisemodel -// Vector sigmas = ones(m); -// -// // copy data over -// size_t cur_m = 0; -// bool constrained = false; -// bool unit = true; -// BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) { -// // loop through ordering -// size_t cur_n = 0; -// BOOST_FOREACH(Index key, order) { -// // copy over matrix if it exists -// if (factor->involves(key)) { -// insertSub(Ab, factor->get_A(key), cur_m, cur_n); -// } -// // move onto next element -// cur_n += dimensions.at(key); -// } -// // copy over the RHS -// insertColumn(Ab, factor->get_b(), cur_m, n-1); -// -// // check if the model is unit already -// if (!boost::shared_dynamic_cast(factor->get_model())) { -// unit = false; -// const Vector& subsigmas = factor->get_model()->sigmas(); -// subInsert(sigmas, subsigmas, cur_m); -// -// // check for constraint -// if (boost::shared_dynamic_cast(factor->get_model())) -// constrained = true; -// } -// -// // move to next row -// cur_m += factor->numberOfRows(); -// } -// -// // combine the noisemodels -// SharedDiagonal model; -// if (unit) { -// model = noiseModel::Unit::Create(m); -// } else if (constrained) { -// model = noiseModel::Constrained::MixedSigmas(sigmas); -// } else { -// model = noiseModel::Diagonal::Sigmas(sigmas); -// } -// return make_pair(Ab, model); -//} - -///* ************************************************************************* */ -//GaussianConditional::shared_ptr -//GaussianFactorGraph::eliminateOneMatrixJoin(Index key) { -// // find and remove all factors connected to key -// vector factors = findAndRemoveFactors(key); -// -// // Collect all dimensions as well as the set of separator keys -// set separator; -// Dimensions dimensions; -// BOOST_FOREACH(const sharedFactor& factor, factors) { -// Dimensions factor_dim = factor->dimensions(); -// dimensions.insert(factor_dim.begin(), factor_dim.end()); -// BOOST_FOREACH(Index k, factor->keys()) -// if (!k == key) -// separator.insert(k); -// } -// -// // add the keys to the rendering -// Ordering render; render += key; -// BOOST_FOREACH(Index k, separator) -// if (k != key) render += k; -// -// // combine the factors to get a noisemodel and a combined matrix -// Matrix Ab; SharedDiagonal model; -// -// boost::tie(Ab, model) = combineFactorsAndCreateMatrix(factors,render,dimensions); -// -// // eliminate that joint factor -// GaussianFactor::shared_ptr factor; -// GaussianConditional::shared_ptr conditional; -// render.pop_front(); -// boost::tie(conditional, factor) = -// GaussianFactor::eliminateMatrix(Ab, model, key, render, dimensions); -// -// // add new factor on separator back into the graph -// if (!factor->empty()) push_back(factor); -// -// // return the conditional Gaussian -// return conditional; -//} -// -///* ************************************************************************* */ -//GaussianBayesNet -//GaussianFactorGraph::eliminateFrontals(const Ordering& frontals) -//{ -// // find the factors that contain at least one of the frontal variables -// Dimensions dimensions = this->dimensions(); -// -// // collect separator -// Ordering separator; -// set frontal_set(frontals.begin(), frontals.end()); -// BOOST_FOREACH(Index key, this->keys()) { -// if (frontal_set.find(key) == frontal_set.end()) -// separator.push_back(key); -// } -// -// Matrix Ab; SharedDiagonal model; -// Ordering ord = frontals; -// ord.insert(ord.end(), separator.begin(), separator.end()); -// boost::tie(Ab, model) = combineFactorsAndCreateMatrix(*this, ord, dimensions); -// -// // eliminate that joint factor -// GaussianFactor::shared_ptr factor; -// GaussianBayesNet bn; -// boost::tie(bn, factor) = -// GaussianFactor::eliminateMatrix(Ab, model, frontals, separator, dimensions); -// -// // add new factor on separator back into the graph -// *this = GaussianFactorGraph(); -// if (!factor->empty()) push_back(factor); -// -// return bn; -//} - - -///* ************************************************************************* */ -//VectorValues GaussianFactorGraph::optimize(const Ordering& ordering, bool old) -//{ -// // eliminate all nodes in the given ordering -> chordal Bayes net -// GaussianBayesNet chordalBayesNet = eliminate(ordering, old); -// -// // calculate new values structure (using backsubstitution) -// VectorValues delta = ::optimize(chordalBayesNet); -// return delta; -//} -// -///* ************************************************************************* */ -//VectorValues GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering) -//{ -// GaussianJunctionTree junctionTree(*this, ordering); -// -// // calculate new values structure (using backsubstitution) -// VectorValues delta = junctionTree.optimize(); -// return delta; -//} - -///* ************************************************************************* */ -//boost::shared_ptr -//GaussianFactorGraph::eliminate_(const Ordering& ordering) -//{ -// boost::shared_ptr chordalBayesNet(new GaussianBayesNet); // empty -// BOOST_FOREACH(Index key, ordering) { -// GaussianConditional::shared_ptr cg = eliminateOne(key); -// chordalBayesNet->push_back(cg); -// } -// return chordalBayesNet; -//} -// -///* ************************************************************************* */ -//boost::shared_ptr -//GaussianFactorGraph::optimize_(const Ordering& ordering) { -// return boost::shared_ptr(new VectorValues(optimize(ordering))); -//} - /* ************************************************************************* */ void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg){ for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){ @@ -350,18 +156,6 @@ GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg return fg; } -// -///* ************************************************************************* */ -//Dimensions GaussianFactorGraph::dimensions() const { -// Dimensions result; -// BOOST_FOREACH(const sharedFactor& factor,factors_) { -// Dimensions vs = factor->dimensions(); -// Index key; size_t dim; -// FOREACH_PAIR(key,dim,vs) result.insert(make_pair(key,dim)); -// } -// return result; -//} - /* ************************************************************************* */ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const GaussianVariableIndex<>& variableIndex) const { @@ -380,178 +174,4 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const Gaussian return result; } -///* ************************************************************************* */ -//Errors GaussianFactorGraph::rhs() const { -// Errors e; -// BOOST_FOREACH(const sharedFactor& factor,factors_) -// e.push_back(ediv(factor->get_b(),factor->get_sigmas())); -// return e; -//} -// -///* ************************************************************************* */ -//Vector GaussianFactorGraph::rhsVector() const { -// Errors e = rhs(); -// return concatVectors(e); -//} -// -///* ************************************************************************* */ -//pair GaussianFactorGraph::matrix(const Ordering& ordering) const { -// -// // get all factors -// GaussianFactorSet found; -// BOOST_FOREACH(const sharedFactor& factor,factors_) -// found.push_back(factor); -// -// // combine them -// GaussianFactor lf(found); -// -// // Return Matrix and Vector -// return lf.matrix(ordering); -//} - -///* ************************************************************************* */ -//VectorValues GaussianFactorGraph::assembleValues(const Vector& vs, const Ordering& ordering) const { -// Dimensions dims = dimensions(); -// VectorValues config; -// Vector::const_iterator itSrc = vs.begin(); -// Vector::iterator itDst; -// BOOST_FOREACH(Index key, ordering){ -// int dim = dims.find(key)->second; -// Vector v(dim); -// for (itDst=v.begin(); itDst!=v.end(); itDst++, itSrc++) -// *itDst = *itSrc; -// config.insert(key, v); -// } -// if (itSrc != vs.end()) -// throw runtime_error("assembleValues: input vector and ordering are not compatible with the graph"); -// return config; -//} -// -///* ************************************************************************* */ -//pair GaussianFactorGraph::columnIndices_(const Ordering& ordering) const { -// -// // get the dimensions for all variables -// Dimensions variableSet = dimensions(); -// -// // Find the starting index and dimensions for all variables given the order -// size_t j = 1; -// Dimensions result; -// BOOST_FOREACH(Index key, ordering) { -// // associate key with first column index -// result.insert(make_pair(key,j)); -// // find dimension for this key -// Dimensions::const_iterator it = variableSet.find(key); -// if (it==variableSet.end()) // key not found, now what ? -// throw invalid_argument("ColumnIndices: this ordering contains keys not in the graph"); -// // advance column index to next block by adding dim(key) -// j += it->second; -// } -// -// return make_pair(result, j-1); -//} -// -///* ************************************************************************* */ -//Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const { -// return columnIndices_(ordering).first; -//} -// -///* ************************************************************************* */ -//pair GaussianFactorGraph::sizeOfA() const { -// size_t m = 0, n = 0; -// Dimensions variableSet = dimensions(); -// BOOST_FOREACH(const Dimensions::value_type value, variableSet) -// n += value.second; -// BOOST_FOREACH(const sharedFactor& factor,factors_) -// m += factor->numberOfRows(); -// return make_pair(m, n); -//} - -///* ************************************************************************* */ -//Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const { -// -// // get the starting column indices for all variables -// Dimensions indices = columnIndices(ordering); -// -// return sparse(indices); -//} -// -///* ************************************************************************* */ -//Matrix GaussianFactorGraph::sparse(const Dimensions& indices) const { -// -// // return values -// list I,J; -// list S; -// -// // Collect the I,J,S lists for all factors -// int row_index = 0; -// BOOST_FOREACH(const sharedFactor& factor,factors_) { -// -// // get sparse lists for the factor -// list i1,j1; -// list s1; -// boost::tie(i1,j1,s1) = factor->sparse(indices); -// -// // add row_start to every row index -// transform(i1.begin(), i1.end(), i1.begin(), bind2nd(plus(), row_index)); -// -// // splice lists from factor to the end of the global lists -// I.splice(I.end(), i1); -// J.splice(J.end(), j1); -// S.splice(S.end(), s1); -// -// // advance row start -// row_index += factor->numberOfRows(); -// } -// -// // Convert them to vectors for MATLAB -// // TODO: just create a sparse matrix class already -// size_t nzmax = S.size(); -// Matrix ijs(3,nzmax); -// copy(I.begin(),I.end(),ijs.begin2()); -// copy(J.begin(),J.end(),ijs.begin2()+nzmax); -// copy(S.begin(),S.end(),ijs.begin2()+2*nzmax); -// -// // return the result -// return ijs; -//} - -///* ************************************************************************* */ -//VectorValues GaussianFactorGraph::steepestDescent(const VectorValues& x0, -// bool verbose, double epsilon, size_t maxIterations) const { -// return gtsam::steepestDescent(*this, x0, verbose, epsilon, maxIterations); -//} -// -///* ************************************************************************* */ -//boost::shared_ptr GaussianFactorGraph::steepestDescent_( -// const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const { -// return boost::shared_ptr(new VectorValues( -// gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon, -// maxIterations))); -//} -// -///* ************************************************************************* */ -//VectorValues GaussianFactorGraph::conjugateGradientDescent( -// const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const { -// return gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon, -// maxIterations); -//} -// -///* ************************************************************************* */ -//boost::shared_ptr GaussianFactorGraph::conjugateGradientDescent_( -// const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const { -// return boost::shared_ptr(new VectorValues( -// gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon, -// maxIterations))); -//} - - - -/* ************************************************************************* */ - -//template std::pair combineFactorsAndCreateMatrix >( -// const vector& factors, const Ordering& order, const Dimensions& dimensions); -// -//template std::pair combineFactorsAndCreateMatrix( -// const GaussianFactorGraph& factors, const Ordering& order, const Dimensions& dimensions); - } // namespace gtsam diff --git a/linear/GaussianFactorGraph.h b/linear/GaussianFactorGraph.h index 8db893b90..69ff3aa84 100644 --- a/linear/GaussianFactorGraph.h +++ b/linear/GaussianFactorGraph.h @@ -52,25 +52,25 @@ namespace gtsam { GaussianFactorGraph(const GaussianBayesNet& CBN); /** Add a null factor */ - inline void add(const Vector& b) { + void add(const Vector& b) { push_back(sharedFactor(new GaussianFactor(b))); } /** Add a unary factor */ - inline void add(Index key1, const Matrix& A1, + void add(Index key1, const Matrix& A1, const Vector& b, const SharedDiagonal& model) { push_back(sharedFactor(new GaussianFactor(key1,A1,b,model))); } /** Add a binary factor */ - inline void add(Index key1, const Matrix& A1, + void add(Index key1, const Matrix& A1, Index key2, const Matrix& A2, const Vector& b, const SharedDiagonal& model) { push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,model))); } /** Add a ternary factor */ - inline void add(Index key1, const Matrix& A1, + void add(Index key1, const Matrix& A1, Index key2, const Matrix& A2, Index key3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) { @@ -78,7 +78,7 @@ namespace gtsam { } /** Add an n-ary factor */ - inline void add(const std::vector > &terms, + void add(const std::vector > &terms, const Vector &b, const SharedDiagonal& model) { push_back(sharedFactor(new GaussianFactor(terms,b,model))); } @@ -87,7 +87,8 @@ namespace gtsam { * Return the set of variables involved in the factors (computes a set * union). */ - std::set, boost::fast_pool_allocator > keys() const; + typedef std::set, boost::fast_pool_allocator > Keys; + Keys keys() const; /** Permute the variables in the factors */ void permuteWithInverse(const Permutation& inversePermutation); @@ -128,44 +129,6 @@ namespace gtsam { return exp(-0.5 * error(c)); } -// /** -// * find the separator, i.e. all the nodes that have at least one -// * common factor with the given node. FD: not used AFAIK. -// */ -// std::set find_separator(Index key) const; - -// /** -// * Peforms a supposedly-faster (fewer matrix copy) version of elimination -// * CURRENTLY IN TESTING -// */ -// GaussianConditional::shared_ptr eliminateOneMatrixJoin(Index key); -// -// -// /** -// * Eliminate multiple variables at once, mostly used to eliminate frontal variables -// */ -// GaussianBayesNet eliminateFrontals(const Ordering& frontals); - -// /** -// * optimize a linear factor graph -// * @param ordering fg in order -// * @param enableJoinFactor uses the older joint factor combine process when true, -// * and when false uses the newer single matrix combine -// */ -// VectorValues optimize(const Ordering& ordering, bool enableJoinFactor = true); - -// /** -// * optimize a linear factor graph using a multi-frontal solver -// * @param ordering fg in order -// */ -// VectorValues optimizeMultiFrontals(const Ordering& ordering); - -// /** -// * shared pointer versions for MATLAB -// */ -// boost::shared_ptr eliminate_(const Ordering&); -// boost::shared_ptr optimize_(const Ordering&); - /** * static function that combines two factor graphs * @param const &lfg1 Linear factor graph @@ -181,11 +144,6 @@ namespace gtsam { */ void combine(const GaussianFactorGraph &lfg); -// /** -// * Find all variables and their dimensions -// * @return The set of all variable/dimension pairs -// */ -// Dimensions dimensions() const; /** * Add zero-mean i.i.d. Gaussian prior terms to each variable @@ -193,84 +151,6 @@ namespace gtsam { */ GaussianFactorGraph add_priors(double sigma, const GaussianVariableIndex<>& variableIndex) const; -// /** -// * Return RHS (b./sigmas) as Errors class -// */ -// Errors rhs() const; -// -// /** -// * Return RHS (b./sigmas) as Vector -// */ -// Vector rhsVector() const; -// -// /** -// * Return (dense) matrix associated with factor graph -// * @param ordering of variables needed for matrix column order -// */ -// std::pair matrix (const Ordering& ordering) const; - -// /** -// * split the source vector w.r.t. the given ordering and assemble a vector config -// * @param v: the source vector -// * @param ordeirng: the ordering corresponding to the vector -// */ -// VectorValues assembleValues(const Vector& v, const Ordering& ordering) const; -// -// /** -// * get the 1-based starting column indices for all variables -// * @param ordering of variables needed for matrix column order -// * @return The set of all variable/index pairs -// */ -// std::pair columnIndices_(const Ordering& ordering) const; -// Dimensions columnIndices(const Ordering& ordering) const; -// -// /** -// * return the size of corresponding A matrix -// */ -// std::pair sizeOfA() const; - -// /** -// * Return 3*nzmax matrix where the rows correspond to the vectors i, j, and s -// * to generate an m-by-n sparse matrix, which can be given to MATLAB's sparse function. -// * The standard deviations are baked into A and b -// * @param ordering of variables needed for matrix column order -// */ -// Matrix sparse(const Ordering& ordering) const; -// -// /** -// * Version that takes column indices rather than ordering -// */ -// Matrix sparse(const Dimensions& indices) const; - -// /** -// * Find solution using gradient descent -// * @param x0: VectorValues specifying initial estimate -// * @return solution -// */ -// VectorValues steepestDescent(const VectorValues& x0, bool verbose = false, -// double epsilon = 1e-3, size_t maxIterations = 0) const; -// -// /** -// * shared pointer versions for MATLAB -// */ -// boost::shared_ptr -// steepestDescent_(const VectorValues& x0, bool verbose = false, -// double epsilon = 1e-3, size_t maxIterations = 0) const; -// -// /** -// * Find solution using conjugate gradient descent -// * @param x0: VectorValues specifying initial estimate -// * @return solution -// */ -// VectorValues conjugateGradientDescent(const VectorValues& x0, bool verbose = -// false, double epsilon = 1e-3, size_t maxIterations = 0) const; -// -// /** -// * shared pointer versions for MATLAB -// */ -// boost::shared_ptr conjugateGradientDescent_( -// const VectorValues& x0, bool verbose = false, double epsilon = 1e-3, -// size_t maxIterations = 0) const; };