diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 0ddc4a7b3..5182bb914 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -16,7 +16,7 @@ */ #include -#include +#include #include #include @@ -242,12 +242,12 @@ double determinant(const GaussianBayesNet& bayesNet) { /* ************************************************************************* */ VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) { - return gradient(FactorGraph(bayesNet), x0); + return gradient(GaussianFactorGraph(bayesNet), x0); } /* ************************************************************************* */ void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) { - gradientAtZero(FactorGraph(bayesNet), g); + gradientAtZero(GaussianFactorGraph(bayesNet), g); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index f9ea3fdc8..04cb4a25a 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -18,7 +18,7 @@ */ #include -#include +#include namespace gtsam { diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 144f2c0d9..14d558e62 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -521,4 +521,151 @@ break; } // \EliminatePreferCholesky + /* ************************************************************************* */ + Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) { + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + e.push_back((*Ai)*x); + } + return e; + } + + /* ************************************************************************* */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e) { + multiplyInPlace(fg,x,e.begin()); + } + + /* ************************************************************************* */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { + Errors::iterator ei = e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + *ei = (*Ai)*x; + ei++; + } + } + + /* ************************************************************************* */ + // x += alpha*A'*e + void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + Ai->transposeMultiplyAdd(alpha,*(ei++),x); + } + } + + /* ************************************************************************* */ + VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0) { + // It is crucial for performance to make a zero-valued clone of x + VectorValues g = VectorValues::Zero(x0); + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + e.push_back(Ai->error_vector(x0)); + } + transposeMultiplyAdd(fg, 1.0, e, g); + return g; + } + + /* ************************************************************************* */ + void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g) { + // Zero-out the gradient + g.setZero(); + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + e.push_back(-Ai->getb()); + } + transposeMultiplyAdd(fg, 1.0, e, g); + } + + /* ************************************************************************* */ + void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { + Index i = 0 ; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + r[i] = Ai->getb(); + i++; + } + VectorValues Ax = VectorValues::SameStructure(r); + multiply(fg,x,Ax); + axpy(-1.0,Ax,r); + } + + /* ************************************************************************* */ + void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { + r.vector() = Vector::Zero(r.dim()); + Index i = 0; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + SubVector &y = r[i]; + for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + y += Ai->getA(j) * x[*j]; + } + ++i; + } + } + + /* ************************************************************************* */ + void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) { + x.vector() = Vector::Zero(x.dim()); + Index i = 0; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + x[*j] += Ai->getA(j).transpose() * r[i]; + } + ++i; + } + } + + /* ************************************************************************* */ + boost::shared_ptr gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) { + boost::shared_ptr e(new Errors); + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + e->push_back(Ai->error_vector(x)); + } + return e; + } + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index e7563b0c5..6c06e34c0 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -31,24 +31,6 @@ namespace gtsam { - /** return A*x-b - * \todo Make this a member function - affects SubgraphPreconditioner */ - template - Errors gaussianErrors(const FactorGraph& fg, const VectorValues& x) { - return *gaussianErrors_(fg, x); - } - - /** shared pointer version - * \todo Make this a member function - affects SubgraphPreconditioner */ - template - boost::shared_ptr gaussianErrors_(const FactorGraph& fg, const VectorValues& x) { - boost::shared_ptr e(new Errors); - BOOST_FOREACH(const typename boost::shared_ptr& factor, fg) { - e->push_back(factor->error_vector(x)); - } - return e; - } - /** * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. * Factor == GaussianFactor @@ -321,5 +303,53 @@ namespace gtsam { */ GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< GaussianFactor>& factors, size_t nrFrontals = 1); + + /** return A*x */ + Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x); + + /** In-place version e <- A*x that overwrites e. */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e); + + /** In-place version e <- A*x that takes an iterator. */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e); + + /** x += alpha*A'*e */ + void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x); + + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ + VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0); + + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, + * centered around zero. + * The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ + void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g); + + /* matrix-vector operations */ + void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); + void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); + void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x); + + /** shared pointer version + * \todo Make this a member function - affects SubgraphPreconditioner */ + boost::shared_ptr gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x); + + /** return A*x-b + * \todo Make this a member function - affects SubgraphPreconditioner */ + inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) { + return *gaussianErrors_(fg, x); } } // namespace gtsam diff --git a/gtsam/linear/JacobianFactorGraph.cpp b/gtsam/linear/JacobianFactorGraph.cpp deleted file mode 100644 index 70c57e8df..000000000 --- a/gtsam/linear/JacobianFactorGraph.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/** - * @file JacobianFactorGraph.h - * @date Jun 6, 2012 - * @author Yong Dian Jian - */ - -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x) { - Errors e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { - e.push_back((*Ai)*x); - } - return e; -} - -/* ************************************************************************* */ -void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e) { - multiplyInPlace(fg,x,e.begin()); -} - -/* ************************************************************************* */ -void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { - Errors::iterator ei = e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { - *ei = (*Ai)*x; - ei++; - } -} - - -/* ************************************************************************* */ -// x += alpha*A'*e -void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { - Ai->transposeMultiplyAdd(alpha,*(ei++),x); - } -} - -/* ************************************************************************* */ -VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0) { - // It is crucial for performance to make a zero-valued clone of x - VectorValues g = VectorValues::Zero(x0); - Errors e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - e.push_back(factor->error_vector(x0)); - } - transposeMultiplyAdd(fg, 1.0, e, g); - return g; -} - -/* ************************************************************************* */ -void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g) { - // Zero-out the gradient - g.setZero(); - Errors e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - e.push_back(-factor->getb()); - } - transposeMultiplyAdd(fg, 1.0, e, g); -} - -/* ************************************************************************* */ -void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - Index i = 0 ; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - r[i] = factor->getb(); - i++; - } - VectorValues Ax = VectorValues::SameStructure(r); - multiply(fg,x,Ax); - axpy(-1.0,Ax,r); -} - -/* ************************************************************************* */ -void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - r.vector() = Vector::Zero(r.dim()); - Index i = 0; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - SubVector &y = r[i]; - for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { - y += factor->getA(j) * x[*j]; - } - ++i; - } -} - -/* ************************************************************************* */ -void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x) { - x.vector() = Vector::Zero(x.dim()); - Index i = 0; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { - x[*j] += factor->getA(j).transpose() * r[i]; - } - ++i; - } -} - -/* ************************************************************************* */ -JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg) { - JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph()); - jfg->reserve(gfg.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { - JacobianFactor::shared_ptr castedFactor(boost::dynamic_pointer_cast(factor)); - if(castedFactor) jfg->push_back(castedFactor); - else throw std::invalid_argument("dynamicCastFactors(), dynamic_cast failed, meaning an invalid cast was requested."); - } - return jfg; -} - -/* ************************************************************************* */ -JacobianFactorGraph::shared_ptr convertToJacobianFactorGraph(const GaussianFactorGraph &gfg) { - JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph()); - jfg->reserve(gfg.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr & factor, gfg) { - if( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(factor) ) { - jfg->push_back(jf); - } - else { - jfg->push_back(boost::make_shared(*factor)); - } - } - return jfg; -} - -} // namespace - - diff --git a/gtsam/linear/JacobianFactorGraph.h b/gtsam/linear/JacobianFactorGraph.h deleted file mode 100644 index 1164652c3..000000000 --- a/gtsam/linear/JacobianFactorGraph.h +++ /dev/null @@ -1,75 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file JacobianFactorGraph.cpp - * @date Jun 6, 2012 - * @brief Linear Algebra Operations for a JacobianFactorGraph - * @author Yong Dian Jian - */ -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace gtsam { - - typedef FactorGraph JacobianFactorGraph; - - /** return A*x */ - Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x); - - /** In-place version e <- A*x that overwrites e. */ - void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e); - - /** In-place version e <- A*x that takes an iterator. */ - void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e); - - /** x += alpha*A'*e */ - void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ A^T(Ax-b) \f$. - * @param fg The Jacobian factor graph $(A,b)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ - VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, - * centered around zero. - * The gradient is \f$ A^T(Ax-b) \f$. - * @param fg The Jacobian factor graph $(A,b)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ - void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g); - - /* matrix-vector operations */ - void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r); - void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r); - void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x); - - /** dynamic_cast the gaussian factors down to jacobian factors, may throw exception if it contains non-Jacobian Factor */ - JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg); - - /** convert the gaussian factors down to jacobian factors, may duplicate factors if it contains Hessian Factor */ - JacobianFactorGraph::shared_ptr convertToJacobianFactorGraph(const GaussianFactorGraph &gfg); -} diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index b33adbc38..72aa25554 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index b109368af..db7c023ce 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include namespace gtsam { @@ -35,7 +36,7 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; typedef boost::shared_ptr sharedBayesNet; - typedef boost::shared_ptr > sharedFG; + typedef boost::shared_ptr sharedFG; typedef boost::shared_ptr sharedValues; typedef boost::shared_ptr sharedErrors; diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 14a0cc174..c34f7e22e 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include #include #include @@ -31,12 +31,11 @@ namespace gtsam { SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters) : parameters_(parameters) { - JacobianFactorGraph::shared_ptr jfg = dynamicCastFactors(gfg); - initialize(*jfg); + initialize(gfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &jfg, const Parameters ¶meters) +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters ¶meters) : parameters_(parameters) { initialize(*jfg); @@ -47,13 +46,12 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFac : parameters_(parameters) { GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(Ab1)->eliminate(&EliminateQR); - JacobianFactorGraph::shared_ptr Ab2Jacobian = dynamicCastFactors(Ab2); - initialize(Rc1, Ab2Jacobian); + initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, - const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters) { GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); @@ -64,13 +62,12 @@ SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters) : parameters_(parameters) { - JacobianFactorGraph::shared_ptr Ab2Jacobians = dynamicCastFactors(Ab2); - initialize(Rc1, Ab2Jacobians); + initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters) + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters) { initialize(Rc1, Ab2); } @@ -80,10 +77,10 @@ VectorValues SubgraphSolver::optimize() { return pc_->x(ybar); } -void SubgraphSolver::initialize(const JacobianFactorGraph &jfg) +void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { - JacobianFactorGraph::shared_ptr Ab1 = boost::make_shared(), - Ab2 = boost::make_shared(); + GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(), + Ab2 = boost::make_shared(); boost::tie(Ab1, Ab2) = splitGraph(jfg) ; if (parameters_.verbosity()) @@ -91,28 +88,33 @@ void SubgraphSolver::initialize(const JacobianFactorGraph &jfg) GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); - JacobianFactorGraph::shared_ptr Ab2Jacobians = convertToJacobianFactorGraph(*Ab2); - pc_ = boost::make_shared(Ab2Jacobians, Rc1, xbar); + pc_ = boost::make_shared(Ab2, Rc1, xbar); } -void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2) +void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2) { VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); pc_ = boost::make_shared(Ab2, Rc1, xbar); } -boost::tuple -SubgraphSolver::splitGraph(const JacobianFactorGraph &jfg) { +boost::tuple +SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { const VariableIndex index(jfg); const size_t n = index.size(), m = jfg.size(); DisjointSet D(n) ; - JacobianFactorGraph::shared_ptr At(new JacobianFactorGraph()); - JacobianFactorGraph::shared_ptr Ac( new JacobianFactorGraph()); + GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); + GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); size_t t = 0; - BOOST_FOREACH ( const JacobianFactor::shared_ptr &jf, jfg ) { + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { + + JacobianFactor::shared_ptr jf; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(gf)) + jf = Ai_J; + else + jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) if ( jf->keys().size() > 2 ) { throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index e4d99933b..441fdaeef 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -13,7 +13,7 @@ #include #include -#include +#include #include #include @@ -62,26 +62,26 @@ protected: public: /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters); - SubgraphSolver(const JacobianFactorGraph::shared_ptr &A, const Parameters ¶meters); + SubgraphSolver(const GaussianFactorGraph::shared_ptr &A, const Parameters ¶meters); /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */ SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters); - SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); + SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); /* The same as above, but the A1 is solved before */ SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters); - SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); + SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); virtual ~SubgraphSolver() {} virtual VectorValues optimize () ; protected: - void initialize(const JacobianFactorGraph &jfg); - void initialize(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2); + void initialize(const GaussianFactorGraph &jfg); + void initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2); - boost::tuple - splitGraph(const JacobianFactorGraph &gfg) ; + boost::tuple + splitGraph(const GaussianFactorGraph &gfg) ; public: diff --git a/gtsam/linear/iterative.cpp b/gtsam/linear/iterative.cpp index d0a7fd551..07d4de865 100644 --- a/gtsam/linear/iterative.cpp +++ b/gtsam/linear/iterative.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include @@ -61,15 +61,15 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues steepestDescent(const FactorGraph& fg, + VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients, VectorValues, Errors>( + return conjugateGradients( fg, x, parameters, true); } - VectorValues conjugateGradientDescent(const FactorGraph& fg, + VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients, VectorValues, Errors>( + return conjugateGradients( fg, x, parameters); } diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index 384dfe814..0eb04bf49 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -130,7 +130,7 @@ namespace gtsam { * Method of steepest gradients, Gaussian Factor Graph version */ VectorValues steepestDescent( - const FactorGraph& fg, + const GaussianFactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters); @@ -138,7 +138,7 @@ namespace gtsam { * Method of conjugate gradients (CG), Gaussian Factor Graph version */ VectorValues conjugateGradientDescent( - const FactorGraph& fg, + const GaussianFactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters); diff --git a/gtsam/linear/tests/testJacobianFactorGraph.cpp b/gtsam/linear/tests/testJacobianFactorGraph.cpp index e756d83e7..07dda4652 100644 --- a/gtsam/linear/tests/testJacobianFactorGraph.cpp +++ b/gtsam/linear/tests/testJacobianFactorGraph.cpp @@ -11,7 +11,7 @@ /** * @file testJacobianFactorGraph.cpp - * @brief Unit tests for JacobianFactorGraph + * @brief Unit tests for GaussianFactorGraph * @author Yong Dian Jian **/ diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index b09133c4d..081910e8c 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -27,7 +27,7 @@ using namespace boost::assign; #include #include #include -#include +#include #include #include diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index f0b3852ef..da14d8982 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include #include diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp index f81ab5528..6df106acf 100644 --- a/tests/smallExample.cpp +++ b/tests/smallExample.cpp @@ -138,9 +138,9 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { + FactorGraph createGaussianFactorGraph(const Ordering& ordering) { // Create empty graph - JacobianFactorGraph fg; + FactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); @@ -273,7 +273,7 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createSimpleConstraintGraph() { + GaussianFactorGraph createSimpleConstraintGraph() { // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 Matrix Ax = eye(2); @@ -293,7 +293,7 @@ namespace example { constraintModel)); // construct the graph - JacobianFactorGraph fg; + GaussianFactorGraph fg; fg.push_back(f1); fg.push_back(f2); @@ -310,7 +310,7 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createSingleConstraintGraph() { + GaussianFactorGraph createSingleConstraintGraph() { // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 Matrix Ax = eye(2); @@ -335,7 +335,7 @@ namespace example { constraintModel)); // construct the graph - JacobianFactorGraph fg; + GaussianFactorGraph fg; fg.push_back(f1); fg.push_back(f2); @@ -351,7 +351,7 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createMultiConstraintGraph() { + GaussianFactorGraph createMultiConstraintGraph() { // unary factor 1 Matrix A = eye(2); Vector b = Vector_(2, -2.0, 2.0); @@ -396,7 +396,7 @@ namespace example { constraintModel)); // construct the graph - JacobianFactorGraph fg; + GaussianFactorGraph fg; fg.push_back(lf1); fg.push_back(lc1); fg.push_back(lc2); @@ -421,7 +421,7 @@ namespace example { } /* ************************************************************************* */ - boost::tuple planarGraph(size_t N) { + boost::tuple planarGraph(size_t N) { // create empty graph NonlinearFactorGraph nlfg; @@ -460,7 +460,7 @@ namespace example { // linearize around zero boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); - JacobianFactorGraph jfg; + GaussianFactorGraph jfg; BOOST_FOREACH(GaussianFactorGraph::sharedFactor factor, *gfg) jfg.push_back(boost::dynamic_pointer_cast(factor)); @@ -477,9 +477,9 @@ namespace example { } /* ************************************************************************* */ - pair splitOffPlanarTree(size_t N, - const JacobianFactorGraph& original) { - JacobianFactorGraph T, C; + pair splitOffPlanarTree(size_t N, + const GaussianFactorGraph& original) { + GaussianFactorGraph T, C; // Add the x11 constraint to the tree T.push_back(boost::dynamic_pointer_cast(original[0])); diff --git a/tests/smallExample.h b/tests/smallExample.h index 54c3a14c5..9c43563a3 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -23,7 +23,6 @@ #include #include -#include #include namespace gtsam { @@ -66,7 +65,7 @@ namespace gtsam { * create a linear factor graph * The non-linear graph above evaluated at NoisyValues */ - JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering); + FactorGraph createGaussianFactorGraph(const Ordering& ordering); /** * create small Chordal Bayes Net x <- y @@ -100,21 +99,21 @@ namespace gtsam { * Creates a simple constrained graph with one linear factor and * one binary equality constraint that sets x = y */ - JacobianFactorGraph createSimpleConstraintGraph(); + GaussianFactorGraph createSimpleConstraintGraph(); VectorValues createSimpleConstraintValues(); /** * Creates a simple constrained graph with one linear factor and * one binary constraint. */ - JacobianFactorGraph createSingleConstraintGraph(); + GaussianFactorGraph createSingleConstraintGraph(); VectorValues createSingleConstraintValues(); /** * Creates a constrained graph with a linear factor and two * binary constraints that share a node */ - JacobianFactorGraph createMultiConstraintGraph(); + GaussianFactorGraph createMultiConstraintGraph(); VectorValues createMultiConstraintValues(); /* ******************************************************* */ @@ -130,7 +129,7 @@ namespace gtsam { * -x11-x21-x31 * with x11 clamped at (1,1), and others related by 2D odometry. */ - boost::tuple planarGraph(size_t N); + boost::tuple planarGraph(size_t N); /* * Create canonical ordering for planar graph that also works for tree @@ -147,8 +146,8 @@ namespace gtsam { * | * -x11-x21-x31 */ - std::pair splitOffPlanarTree( - size_t N, const JacobianFactorGraph& original); + std::pair splitOffPlanarTree( + size_t N, const GaussianFactorGraph& original); } // example } // gtsam diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index 3fe92c50d..a41c8d9a7 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -72,7 +72,7 @@ TEST( GaussianFactor, getDim ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // get a factor Ordering ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); + FactorGraph fg = example::createGaussianFactorGraph(ordering); GaussianFactor::shared_ptr factor = fg[0]; // get the size of a variable @@ -89,7 +89,7 @@ TEST( GaussianFactor, error ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // create a small linear factor graph Ordering ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); + FactorGraph fg = example::createGaussianFactorGraph(ordering); // get the first factor from the factor graph GaussianFactor::shared_ptr lf = fg[0]; @@ -210,7 +210,7 @@ TEST( GaussianFactor, size ) // create a linear factor graph const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); Ordering ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); + FactorGraph fg = example::createGaussianFactorGraph(ordering); // get some factors from the graph boost::shared_ptr factor1 = fg[0]; diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index c6ec69d4e..ca8b9b1ea 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 67fa287d3..c4b90d882 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 88eb222bd..09047005e 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -40,7 +40,7 @@ TEST( Iterative, steepestDescent ) // Create factor graph Ordering ord; ord += L(1), X(1), X(2); - JacobianFactorGraph fg = createGaussianFactorGraph(ord); + FactorGraph fg = createGaussianFactorGraph(ord); // eliminate and solve VectorValues expected = *GaussianSequentialSolver(fg).optimize(); diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index c41b95881..39c5e2b70 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include #include @@ -54,9 +54,9 @@ TEST( SubgraphPreconditioner, planarOrdering ) { /* ************************************************************************* */ /** unnormalized error */ -static double error(const JacobianFactorGraph& fg, const VectorValues& x) { +static double error(const GaussianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg) total_error += factor->error(x); return total_error; } @@ -65,7 +65,7 @@ static double error(const JacobianFactorGraph& fg, const VectorValues& x) { TEST( SubgraphPreconditioner, planarGraph ) { // Check planar graph construction - JacobianFactorGraph A; + GaussianFactorGraph A; VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); LONGS_EQUAL(13,A.size()); @@ -82,12 +82,12 @@ TEST( SubgraphPreconditioner, planarGraph ) TEST( SubgraphPreconditioner, splitOffPlanarTree ) { // Build a planar graph - JacobianFactorGraph A; + GaussianFactorGraph A; VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); // Get the spanning tree and constraints, and check their sizes - JacobianFactorGraph T, C; + GaussianFactorGraph T, C; boost::tie(T, C) = splitOffPlanarTree(3, A); LONGS_EQUAL(9,T.size()); LONGS_EQUAL(4,C.size()); @@ -103,16 +103,16 @@ TEST( SubgraphPreconditioner, splitOffPlanarTree ) TEST( SubgraphPreconditioner, system ) { // Build a planar graph - JacobianFactorGraph Ab; + GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new JacobianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new JacobianFactorGraph(Ab2_)); + SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); // Eliminate the spanning tree to build a prior SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 @@ -179,16 +179,16 @@ TEST( SubgraphPreconditioner, system ) TEST( SubgraphPreconditioner, conjugateGradients ) { // Build a planar graph - JacobianFactorGraph Ab; + GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new JacobianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new JacobianFactorGraph(Ab2_)); + SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); // Eliminate the spanning tree to build a prior Ordering ordering = planarOrdering(N); diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 84f54a6fa..700f35261 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -38,9 +38,9 @@ using namespace example; /* ************************************************************************* */ /** unnormalized error */ -static double error(const JacobianFactorGraph& fg, const VectorValues& x) { +static double error(const GaussianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg) total_error += factor->error(x); return total_error; } @@ -50,7 +50,7 @@ static double error(const JacobianFactorGraph& fg, const VectorValues& x) { TEST( SubgraphSolver, constructor1 ) { // Build a planar graph - JacobianFactorGraph Ab; + GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b @@ -67,13 +67,13 @@ TEST( SubgraphSolver, constructor1 ) TEST( SubgraphSolver, constructor2 ) { // Build a planar graph - JacobianFactorGraph Ab; + GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); // The second constructor takes two factor graphs, @@ -88,13 +88,13 @@ TEST( SubgraphSolver, constructor2 ) TEST( SubgraphSolver, constructor3 ) { // Build a planar graph - JacobianFactorGraph Ab; + GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT