diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 543822ce0..8c8e2cb2b 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -130,6 +130,9 @@ namespace gtsam { /// A'*b for Jacobian, eta for Hessian virtual VectorValues gradientAtZero() const = 0; + /// A'*b for Jacobian, eta for Hessian (raw memory version) + virtual void gradientAtZero(double* d) const = 0; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 50442ac6b..ae5730b68 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -257,7 +257,7 @@ namespace gtsam { * @param [output] g A VectorValues to store the gradient, which must be preallocated, * see allocateVectorValues * @return The gradient as a VectorValues */ - VectorValues gradientAtZero() const; + virtual VectorValues gradientAtZero() const; /** Optimize along the gradient direction, with a closed-form computation to perform the line * search. The gradient is computed about \f$ \delta x=0 \f$. diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 18fa44e8b..8d76c7651 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -600,6 +600,23 @@ VectorValues HessianFactor::gradientAtZero() const { return g; } +/* ************************************************************************* */ +// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n +void HessianFactor::gradientAtZero(double* d) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + DVector dj = -info_(pos,size()).knownOffDiagonal(); + DMap(d + 9 * j) += dj; + } +} + /* ************************************************************************* */ std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index faf2ccd33..62d84925c 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -387,6 +387,8 @@ namespace gtsam { /// eta for Hessian VectorValues gradientAtZero() const; + virtual void gradientAtZero(double* d) const; + /** * Densely partially eliminate with Cholesky factorization. JacobianFactors are * left-multiplied with their transpose to form the Hessian using the conversion constructor diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index c080f75cb..bb9d5afc0 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -573,6 +573,11 @@ VectorValues JacobianFactor::gradientAtZero() const { return g; } +/* ************************************************************************* */ +void JacobianFactor::gradientAtZero(double* d) const { + throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); +} + /* ************************************************************************* */ pair JacobianFactor::jacobian() const { pair result = jacobianUnweighted(); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 5a567c2c7..b90012822 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -286,6 +286,9 @@ namespace gtsam { /// A'*b for Jacobian VectorValues gradientAtZero() const; + /* ************************************************************************* */ + virtual void gradientAtZero(double* d) const; + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam_unstable/slam/ImplicitSchurFactor.h index d7d1bc21c..8237b8c07 100644 --- a/gtsam_unstable/slam/ImplicitSchurFactor.h +++ b/gtsam_unstable/slam/ImplicitSchurFactor.h @@ -426,6 +426,28 @@ public: return g; } + /** + * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS + */ + void gradientAtZero(double* d) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // calculate Q*b + e1.resize(size()); + e2.resize(size()); + for (size_t k = 0; k < size(); k++) + e1[k] = b_.segment < 2 > (2 * k); + projectError(e1, e2); + + for (size_t k = 0; k < size(); ++k) { // for each camera in the factor + Key j = keys_[k]; + DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k]; + } + } + }; // ImplicitSchurFactor