diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp new file mode 100644 index 000000000..341f03779 --- /dev/null +++ b/gtsam/linear/GaussianFactor.cpp @@ -0,0 +1,35 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianFactor.cpp + * @brief A factor with a quadratic error function - a Gaussian + * @brief GaussianFactor + * @author Fan Jiang + */ + +// \callgraph + +#pragma once + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ + VectorValues GaussianFactor::hessianDiagonal() const { + VectorValues d; + hessianDiagonalAdd(d); + return d; + } + +} \ No newline at end of file diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index bd9e7d232..9b4c5f940 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -100,7 +100,7 @@ namespace gtsam { virtual Matrix information() const = 0; /// Return the diagonal of the Hessian for this factor - virtual VectorValues hessianDiagonal() const = 0; + VectorValues hessianDiagonal() const; /// Add the current diagonal to a VectorValues instance virtual void hessianDiagonalAdd(VectorValues& d) const = 0; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 02948fc04..7bf65353b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -301,15 +301,6 @@ Matrix HessianFactor::information() const { return informationView(); } -/* ************************************************************************* */ -VectorValues HessianFactor::hessianDiagonal() const { - VectorValues d; - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { - d.emplace(keys_[j], info_.diagonal(j)); - } - return d; -} - /* ************************************************************************* */ void HessianFactor::hessianDiagonalAdd(VectorValues &d) const { for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 0ee1d359b..64b764087 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -293,12 +293,12 @@ namespace gtsam { */ Matrix information() const override; - /// Return the diagonal of the Hessian for this factor - VectorValues hessianDiagonal() const override; - /// Add the current diagonal to a VectorValues instance void hessianDiagonalAdd(VectorValues& d) const override; + /// Using the base method + using Base::hessianDiagonal; + /// Raw memory access version of hessianDiagonal void hessianDiagonal(double* d) const override; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 8dfe46c49..09a9a6103 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -541,13 +541,6 @@ Matrix JacobianFactor::information() const { } } -/* ************************************************************************* */ -VectorValues JacobianFactor::hessianDiagonal() const { - VectorValues d; - hessianDiagonalAdd(d); - return d; -} - /* ************************************************************************* */ void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const { for (size_t pos = 0; pos < size(); ++pos) { diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index ac5ccd0d2..49c47c7f0 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -215,8 +215,8 @@ namespace gtsam { */ Matrix information() const override; - /// Return the diagonal of the Hessian for this factor - VectorValues hessianDiagonal() const override; + /// Using the base method + using Base::hessianDiagonal; /// Add the current diagonal to a VectorValues instance void hessianDiagonalAdd(VectorValues& d) const override; diff --git a/gtsam/linear/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h index 2b5cf85ff..0312efe78 100644 --- a/gtsam/linear/RegularJacobianFactor.h +++ b/gtsam/linear/RegularJacobianFactor.h @@ -102,10 +102,8 @@ public: DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; } - /// Expose base class hessianDiagonal - virtual VectorValues hessianDiagonal() const { - return JacobianFactor::hessianDiagonal(); - } + /// Using the base method + using GaussianFactor::hessianDiagonal; /// Raw memory access version of hessianDiagonal void hessianDiagonal(double* d) const { diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 8be1b7bb6..f14df9658 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -141,15 +141,8 @@ public: return augmented.block(0, 0, M, M); } - /// Return the diagonal of the Hessian for this factor - virtual VectorValues hessianDiagonal() const { - // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); - VectorValues d; - - hessianDiagonalAdd(d); - - return d; - } + /// Using the base method + using GaussianFactor::hessianDiagonal; /// Add the diagonal of the Hessian for this factor to existing VectorValues virtual void hessianDiagonalAdd(VectorValues &d) const override {