diff --git a/gtsam/linear/JacobianFactorGraph.cpp b/gtsam/linear/JacobianFactorGraph.cpp index b02375a79..632a55e02 100644 --- a/gtsam/linear/JacobianFactorGraph.cpp +++ b/gtsam/linear/JacobianFactorGraph.cpp @@ -10,7 +10,7 @@ namespace gtsam { /* ************************************************************************* */ -Errors operator*(const FactorGraph& fg, const VectorValues& x) { +Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x) { Errors e; BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { e.push_back((*Ai)*x); @@ -19,12 +19,12 @@ Errors operator*(const FactorGraph& fg, const VectorValues& x) { } /* ************************************************************************* */ -void multiplyInPlace(const FactorGraph& fg, const VectorValues& x, Errors& e) { +void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e) { multiplyInPlace(fg,x,e.begin()); } /* ************************************************************************* */ -void multiplyInPlace(const FactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { +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; @@ -35,7 +35,7 @@ void multiplyInPlace(const FactorGraph& fg, const VectorValues& /* ************************************************************************* */ // x += alpha*A'*e -void transposeMultiplyAdd(const FactorGraph& fg, double alpha, const Errors& e, VectorValues& x) { +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) { @@ -44,7 +44,7 @@ void transposeMultiplyAdd(const FactorGraph& fg, double alpha, c } /* ************************************************************************* */ -VectorValues gradient(const FactorGraph& fg, const VectorValues& x0) { +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; @@ -56,7 +56,7 @@ VectorValues gradient(const FactorGraph& fg, const VectorValues& } /* ************************************************************************* */ -void gradientAtZero(const FactorGraph& fg, VectorValues& g) { +void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g) { // Zero-out the gradient g.setZero(); Errors e; @@ -67,7 +67,7 @@ void gradientAtZero(const FactorGraph& fg, VectorValues& g) { } /* ************************************************************************* */ -void residual(const FactorGraph& fg, const VectorValues &x, VectorValues &r) { +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(); @@ -79,7 +79,7 @@ void residual(const FactorGraph& fg, const VectorValues &x, Vect } /* ************************************************************************* */ -void multiply(const FactorGraph& fg, const VectorValues &x, VectorValues &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) { @@ -92,7 +92,7 @@ void multiply(const FactorGraph& fg, const VectorValues &x, Vect } /* ************************************************************************* */ -void transposeMultiply(const FactorGraph& fg, const VectorValues &r, VectorValues &x) { +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) { @@ -102,6 +102,6 @@ void transposeMultiply(const FactorGraph& fg, const VectorValues ++i; } } -} +} // namespace diff --git a/gtsam/linear/JacobianFactorGraph.h b/gtsam/linear/JacobianFactorGraph.h index 8d240c00e..598c69677 100644 --- a/gtsam/linear/JacobianFactorGraph.h +++ b/gtsam/linear/JacobianFactorGraph.h @@ -24,17 +24,19 @@ namespace gtsam { + typedef FactorGraph JacobianFactorGraph; + /** return A*x */ - Errors operator*(const FactorGraph& fg, const VectorValues& x); + Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x); /** In-place version e <- A*x that overwrites e. */ - void multiplyInPlace(const FactorGraph& fg, const VectorValues& x, Errors& e); + void multiplyInPlace(const JacobianFactorGraph& 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); + void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e); /** x += alpha*A'*e */ - void transposeMultiplyAdd(const FactorGraph& fg, double alpha, const Errors& e, VectorValues& x); + void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x); /** * Compute the gradient of the energy function, @@ -45,7 +47,7 @@ namespace gtsam { * @param x0 The center about which to compute the gradient * @return The gradient as a VectorValues */ - VectorValues gradient(const FactorGraph& fg, const VectorValues& x0); + VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0); /** * Compute the gradient of the energy function, @@ -56,11 +58,11 @@ namespace gtsam { * @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); + void gradientAtZero(const JacobianFactorGraph& 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); + 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); }