diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 4b8bd180d..b480996ec 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -42,6 +42,9 @@ // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake) #cmakedefine GTSAM_USE_TBB +// Whether we are using a TBB version higher than 2020 +#cmakedefine TBB_GREATER_EQUAL_2020 + // Whether we are using system-Eigen or our own patched version #cmakedefine GTSAM_USE_SYSTEM_EIGEN 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 a80b4dfd4..9b4c5f940 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -100,7 +100,10 @@ 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; /// Raw memory access version of hessianDiagonal virtual void hessianDiagonal(double* d) const = 0; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 8dc3600c6..69890c32d 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -255,8 +255,7 @@ namespace gtsam { VectorValues d; for (const sharedFactor& factor : *this) { if(factor){ - VectorValues di = factor->hessianDiagonal(); - d.addInPlace_(di); + factor->hessianDiagonalAdd(d); } } return d; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index cc82e2fa0..7bf65353b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -302,12 +302,14 @@ Matrix HessianFactor::information() const { } /* ************************************************************************* */ -VectorValues HessianFactor::hessianDiagonal() const { - VectorValues d; +void HessianFactor::hessianDiagonalAdd(VectorValues &d) const { for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { - d.emplace(keys_[j], info_.diagonal(j)); + auto result = d.emplace(keys_[j], info_.diagonal(j)); + if(!result.second) { + // if emplace fails, it returns an iterator to the existing element, which we add to: + result.first->second += info_.diagonal(j); + } } - return d; } /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index d1c362a54..64b764087 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -293,8 +293,11 @@ 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 839ffe69d..09a9a6103 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -542,21 +542,31 @@ Matrix JacobianFactor::information() const { } /* ************************************************************************* */ -VectorValues JacobianFactor::hessianDiagonal() const { - VectorValues d; +void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const { for (size_t pos = 0; pos < size(); ++pos) { Key j = keys_[pos]; size_t nj = Ab_(pos).cols(); - Vector dj(nj); + auto result = d.emplace(j, nj); + + Vector& dj = result.first->second; + for (size_t k = 0; k < nj; ++k) { - Vector column_k = Ab_(pos).col(k); - if (model_) - model_->whitenInPlace(column_k); - dj(k) = dot(column_k, column_k); + Eigen::Ref column_k = Ab_(pos).col(k); + if (model_) { + Vector column_k_copy = column_k; + model_->whitenInPlace(column_k_copy); + if(!result.second) + dj(k) += dot(column_k_copy, column_k_copy); + else + dj(k) = dot(column_k_copy, column_k_copy); + } else { + if (!result.second) + dj(k) += dot(column_k, column_k); + else + dj(k) = dot(column_k, column_k); + } } - d.emplace(j, dj); } - return d; } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 9b9d02726..49c47c7f0 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -215,8 +215,11 @@ 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; /// Raw memory access version of hessianDiagonal void hessianDiagonal(double* 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/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index e74d3776d..0a25617e4 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -96,20 +96,6 @@ namespace gtsam { return result.first; } - /* ************************************************************************* */ - VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) { -#ifdef TBB_GREATER_EQUAL_2020 - std::pair result = values_.emplace(j, value); -#else - std::pair result = values_.insert(std::make_pair(j, value)); -#endif - if(!result.second) - throw std::invalid_argument( - "Requested to emplace variable '" + DefaultKeyFormatter(j) - + "' already in this VectorValues."); - return result.first; - } - /* ************************************************************************* */ void VectorValues::update(const VectorValues& values) { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index eed212eda..9e60ff2aa 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -179,7 +179,14 @@ namespace gtsam { * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - iterator emplace(Key j, const Vector& value); + template + inline std::pair emplace(Key j, Args&&... args) { +#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020) + return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), std::forward_as_tuple(args...)); +#else + return values_.insert(std::make_pair(j, Vector(std::forward(args)...))); +#endif + } /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c * j is already used. @@ -197,7 +204,7 @@ namespace gtsam { * and an iterator to the existing value is returned, along with 'false'. If the value did not * exist, it is inserted and an iterator pointing to the new element, along with 'true', is * returned. */ - std::pair tryInsert(Key j, const Vector& value) { + inline std::pair tryInsert(Key j, const Vector& value) { #ifdef TBB_GREATER_EQUAL_2020 return values_.emplace(j, value); #else diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 1afaf79d1..811e07121 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -98,8 +98,13 @@ namespace gtsam // Insert solution into a VectorValues DenseIndex vectorPosition = 0; for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { - VectorValues::const_iterator r = - collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); + auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); + if(!result.second) + throw std::runtime_error( + "Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal) + + "' that exists."); + + VectorValues::const_iterator r = result.first; myData.cliqueResults.emplace(r->first, r); vectorPosition += c.getDim(frontal); } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 07b749811..f14df9658 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -141,11 +141,12 @@ 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; + /// 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 { + // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); for (size_t k = 0; k < size(); ++k) { // for each camera Key j = keys_[k]; @@ -153,7 +154,7 @@ public: // D x 3 = (D x ZDim) * (ZDim x 3) const MatrixZD& Fj = FBlocks_[k]; Eigen::Matrix FtE = Fj.transpose() - * E_.block(ZDim * k, 0); + * E_.block(ZDim * k, 0); Eigen::Matrix dj; for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian @@ -163,9 +164,12 @@ public: // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); } - d.insert(j, dj); + + auto result = d.emplace(j, dj); + if(!result.second) { + result.first->second += dj; + } } - return d; } /**