diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index bd06c5f8d..8ca12e1f2 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -432,13 +432,6 @@ namespace gtsam { return model_ ? model_->whiten(Ax) : Ax; } - /* ************************************************************************* */ - void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, - VectorValues& y) const { - Vector Ax = (*this)*x; - transposeMultiplyAdd(alpha,Ax,y); - } - /* ************************************************************************* */ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const @@ -451,10 +444,17 @@ namespace gtsam { pair xi = x.tryInsert(keys_[pos], Vector()); if(xi.second) xi.first->second = Vector::Zero(getDim(begin() + pos)); - gtsam::transposeMultiplyAdd(1.0, Ab_(pos), E, xi.first->second); + gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second); } } + /* ************************************************************************* */ + void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + Vector Ax = (*this)*x; + transposeMultiplyAdd(alpha,Ax,y); + } + /* ************************************************************************* */ VectorValues JacobianFactor::gradientAtZero() const { VectorValues g; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d6e937769..640b3db6f 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -259,13 +259,13 @@ namespace gtsam { /** Return A*x */ Vector operator*(const VectorValues& x) const; - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - /** x += A'*e. If x is initially missing any values, they are created and assumed to start as * zero vectors. */ void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; + /** y += alpha * A'*A*x */ + void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; + /// A'*b for Jacobian VectorValues gradientAtZero() const;