diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 5721a4760..056bda67e 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index ae76d958e..4a0e4b355 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/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2b884e1bd..b4d7016aa 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -536,98 +536,4 @@ namespace gtsam { model_ = noiseModel::Diagonal::Sigmas(sigmas); } - /* ************************************************************************* */ - Errors operator*(const FactorGraph& fg, const VectorValues& x) { - Errors e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { - e.push_back((*Ai)*x); - } - return e; - } - - /* ************************************************************************* */ - void multiplyInPlace(const FactorGraph& fg, const VectorValues& x, Errors& e) { - multiplyInPlace(fg,x,e.begin()); - } - - /* ************************************************************************* */ - void multiplyInPlace(const FactorGraph& 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 FactorGraph& 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 FactorGraph& 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 FactorGraph& 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 FactorGraph& 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 FactorGraph& 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 FactorGraph& 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; - } - } - } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 30e8e19bb..d45873c3b 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -318,45 +318,5 @@ namespace gtsam { } }; // JacobianFactor - - /** return A*x */ - Errors operator*(const FactorGraph& fg, const VectorValues& x); - - /* In-place version e <- A*x that overwrites e. */ - void multiplyInPlace(const FactorGraph& fg, const VectorValues& x, Errors& e); - - /* In-place version e <- A*x that takes an iterator. */ - void multiplyInPlace(const FactorGraph& fg, const VectorValues& x, const Errors::iterator& e); - - /** x += alpha*A'*e */ - void transposeMultiplyAdd(const FactorGraph& 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 FactorGraph& 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 FactorGraph& fg, VectorValues& g); - - // matrix-vector operations - void residual(const FactorGraph& fg, const VectorValues &x, VectorValues &r); - void multiply(const FactorGraph& fg, const VectorValues &x, VectorValues &r); - void transposeMultiply(const FactorGraph& fg, const VectorValues &r, VectorValues &x); - } // gtsam diff --git a/gtsam/linear/JacobianFactorGraph.cpp b/gtsam/linear/JacobianFactorGraph.cpp new file mode 100644 index 000000000..4a77c17c4 --- /dev/null +++ b/gtsam/linear/JacobianFactorGraph.cpp @@ -0,0 +1,108 @@ +/* + * JacobianFactorGraph.cpp + * + * Created on: Jun 6, 2012 + * Author: ydjian + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +Errors operator*(const FactorGraph& fg, const VectorValues& x) { + Errors e; + BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { + e.push_back((*Ai)*x); + } + return e; +} + +/* ************************************************************************* */ +void multiplyInPlace(const FactorGraph& fg, const VectorValues& x, Errors& e) { + multiplyInPlace(fg,x,e.begin()); +} + +/* ************************************************************************* */ +void multiplyInPlace(const FactorGraph& 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 FactorGraph& 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 FactorGraph& 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 FactorGraph& 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 FactorGraph& 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 FactorGraph& 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 FactorGraph& 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; + } +} +} + + diff --git a/gtsam/linear/JacobianFactorGraph.h b/gtsam/linear/JacobianFactorGraph.h new file mode 100644 index 000000000..e91dee311 --- /dev/null +++ b/gtsam/linear/JacobianFactorGraph.h @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * JacobianFactorGraph.cpp + * + * Created on: Jun 6, 2012 + * Author: ydjian + */ +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + /** + * Linear Algebra Operations for the JacobianFactorGraph + */ + + /** return A*x */ + Errors operator*(const FactorGraph& fg, const VectorValues& x); + + /** In-place version e <- A*x that overwrites e. */ + void multiplyInPlace(const FactorGraph& fg, const VectorValues& x, Errors& e); + + /** In-place version e <- A*x that takes an iterator. */ + void multiplyInPlace(const FactorGraph& fg, const VectorValues& x, const Errors::iterator& e); + + /** x += alpha*A'*e */ + void transposeMultiplyAdd(const FactorGraph& 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 FactorGraph& 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 FactorGraph& fg, VectorValues& g); + + /* matrix-vector operations */ + void residual(const FactorGraph& fg, const VectorValues &x, VectorValues &r); + void multiply(const FactorGraph& fg, const VectorValues &x, VectorValues &r); + void transposeMultiply(const FactorGraph& fg, const VectorValues &r, VectorValues &x); + +} diff --git a/gtsam/linear/SimpleSPCGSolver.cpp b/gtsam/linear/SimpleSPCGSolver.cpp index 86cf3d1ca..8f59b9058 100644 --- a/gtsam/linear/SimpleSPCGSolver.cpp +++ b/gtsam/linear/SimpleSPCGSolver.cpp @@ -13,7 +13,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index d785657a3..e2ab825b3 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -26,6 +26,7 @@ using namespace boost::assign; #include #include #include +#include #include #include diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 58709328d..0f76af4f6 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index cf2842af9..c74dbe276 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include