From f73429133accc5940dced185f58d7dcb95c2d4cc Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 12:44:57 -0400 Subject: [PATCH 01/14] Switched to in-place update of the diagonal Hessian --- gtsam/linear/GaussianFactor.h | 3 +++ gtsam/linear/GaussianFactorGraph.cpp | 3 +-- gtsam/linear/HessianFactor.cpp | 11 +++++++++++ gtsam/linear/HessianFactor.h | 3 +++ gtsam/linear/JacobianFactor.cpp | 13 +++++++++++-- gtsam/linear/JacobianFactor.h | 3 +++ 6 files changed, 32 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index a80b4dfd4..bd9e7d232 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -102,6 +102,9 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const = 0; + /// 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..06d2ece7b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -310,6 +310,17 @@ VectorValues HessianFactor::hessianDiagonal() const { return d; } +/* ************************************************************************* */ +void HessianFactor::hessianDiagonalAdd(VectorValues &d) const { + for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + if(d.exists(keys_[j])) { + d.at(keys_[j]) += info_.diagonal(j); + } else { + d.emplace(keys_[j], info_.diagonal(j)); + } + } +} + /* ************************************************************************* */ // Raw memory access version should be called in Regular Factors only currently void HessianFactor::hessianDiagonal(double* d) const { diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index d1c362a54..0ee1d359b 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -296,6 +296,9 @@ namespace gtsam { /// 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; + /// 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..2e243c033 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -544,6 +544,12 @@ 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) { Key j = keys_[pos]; size_t nj = Ab_(pos).cols(); @@ -554,9 +560,12 @@ VectorValues JacobianFactor::hessianDiagonal() const { model_->whitenInPlace(column_k); dj(k) = dot(column_k, column_k); } - d.emplace(j, dj); + if(d.exists(j)) { + d.at(j) += dj; + } else { + d.emplace(j, dj); + } } - return d; } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 9b9d02726..ac5ccd0d2 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -218,6 +218,9 @@ namespace gtsam { /// 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; + /// Raw memory access version of hessianDiagonal void hessianDiagonal(double* d) const override; From 50ffeb7dcd539f410196e6549bbc385c62bb042a Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 15:38:04 -0400 Subject: [PATCH 02/14] Use find --- gtsam/linear/HessianFactor.cpp | 5 +++-- gtsam/linear/JacobianFactor.cpp | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 06d2ece7b..8a9adf6f6 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -313,8 +313,9 @@ VectorValues HessianFactor::hessianDiagonal() const { /* ************************************************************************* */ void HessianFactor::hessianDiagonalAdd(VectorValues &d) const { for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { - if(d.exists(keys_[j])) { - d.at(keys_[j]) += info_.diagonal(j); + auto item = d.find(keys_[j]); + if(item != d.end()) { + item->second += info_.diagonal(j); } else { d.emplace(keys_[j], info_.diagonal(j)); } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2e243c033..3f6eba5f3 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -560,8 +560,9 @@ void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const { model_->whitenInPlace(column_k); dj(k) = dot(column_k, column_k); } - if(d.exists(j)) { - d.at(j) += dj; + auto item = d.find(j); + if(item != d.end()) { + item->second += dj; } else { d.emplace(j, dj); } From 15fda68c1ae9ce5a5543630a9ab433f55263c90f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 16:17:28 -0400 Subject: [PATCH 03/14] Use faster version of in-place add --- gtsam/linear/JacobianFactor.cpp | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 3f6eba5f3..d0e702f06 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -552,18 +552,27 @@ VectorValues JacobianFactor::hessianDiagonal() const { 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); - 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); - } auto item = d.find(j); + if(item != d.end()) { - item->second += dj; + size_t nj = Ab_(pos).cols(); + Vector& dj = item->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); + } } else { + size_t nj = Ab_(pos).cols(); + Vector dj(nj); + 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); + } + d.emplace(j, dj); } } From b669e3aa3fe29f09d5f2db253045a210b8c15f23 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 16:18:18 -0400 Subject: [PATCH 04/14] fix checks --- gtsam/slam/RegularImplicitSchurFactor.h | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 07b749811..656e75723 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -146,6 +146,14 @@ public: // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); VectorValues d; + hessianDiagonalAdd(d); + + return d; + } + + /// Return the diagonal of the Hessian for this factor + 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 +161,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 +171,14 @@ 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 item = d.find(j); + if(item != d.end()) { + item->second += dj; + } else { + d.emplace(j, dj); + } } - return d; } /** From 1fe876aba67a8e6d43cda3367e6afea31aaf6b1e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 16:40:41 -0400 Subject: [PATCH 05/14] Fix build --- gtsam/slam/RegularImplicitSchurFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 656e75723..342dc5c65 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -147,7 +147,7 @@ public: VectorValues d; hessianDiagonalAdd(d); - + return d; } From 5bf8dc4174a214761714586de6b24efb842a5a4e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 17:36:20 -0400 Subject: [PATCH 06/14] Fixed emplace to align with std --- gtsam/linear/VectorValues.cpp | 8 ++------ gtsam/linear/VectorValues.h | 2 +- gtsam/linear/linearAlgorithms-inst.h | 9 +++++++-- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index e74d3776d..eb92228e5 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -97,17 +97,13 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) { + std::pair 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; + return result; } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index eed212eda..e11e98cce 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -179,7 +179,7 @@ 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); + std::pair emplace(Key j, const Vector& value); /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c * j is already used. diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 1afaf79d1..57d26f5dc 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::invalid_argument( + "Requested to emplace variable '" + DefaultKeyFormatter(*frontal) + + "' already in this VectorValues."); + + VectorValues::const_iterator r = result.first; myData.cliqueResults.emplace(r->first, r); vectorPosition += c.getDim(frontal); } From 23617fd430a59f6fbf3ffeeca9b5203b10f291f7 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 19:44:44 -0400 Subject: [PATCH 07/14] Move to header --- gtsam/linear/VectorValues.cpp | 10 ---------- gtsam/linear/VectorValues.h | 10 ++++++++-- gtsam/linear/linearAlgorithms-inst.h | 2 +- 3 files changed, 9 insertions(+), 13 deletions(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index eb92228e5..0a25617e4 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -96,16 +96,6 @@ namespace gtsam { return result.first; } - /* ************************************************************************* */ - std::pair 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 - return result; - } - /* ************************************************************************* */ void VectorValues::update(const VectorValues& values) { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index e11e98cce..5db4462e1 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -179,7 +179,13 @@ namespace gtsam { * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - std::pair emplace(Key j, const Vector& value); + std::pair emplace(Key j, const Vector& value) { +#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020) + return values_.emplace(j, value); +#else + return values_.insert(std::make_pair(j, value)); +#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 +203,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 57d26f5dc..e339347f1 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -103,7 +103,7 @@ namespace gtsam throw std::invalid_argument( "Requested to emplace variable '" + DefaultKeyFormatter(*frontal) + "' already in this VectorValues."); - + VectorValues::const_iterator r = result.first; myData.cliqueResults.emplace(r->first, r); vectorPosition += c.getDim(frontal); From 0675b82c1f5173f76b1cdf2e7457fac68b38b7b7 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 20:01:51 -0400 Subject: [PATCH 08/14] Fixed TBB detection and make emplace great again --- gtsam/config.h.in | 3 +++ gtsam/linear/VectorValues.h | 7 ++++--- 2 files changed, 7 insertions(+), 3 deletions(-) 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/VectorValues.h b/gtsam/linear/VectorValues.h index 5db4462e1..5de34c194 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -179,11 +179,12 @@ namespace gtsam { * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - std::pair 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(j, value); + return values_.emplace(j, std::forward(args)...); #else - return values_.insert(std::make_pair(j, value)); + return values_.insert(std::make_pair(j, Vector(std::forward(args)...))); #endif } From 151809c89c8a924de032f6335a1683c4beb98f61 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 21:33:58 -0400 Subject: [PATCH 09/14] replaced all find&insert to emplace --- gtsam/linear/HessianFactor.cpp | 8 +++---- gtsam/linear/JacobianFactor.cpp | 29 +++++++------------------ gtsam/slam/RegularImplicitSchurFactor.h | 8 +++---- 3 files changed, 14 insertions(+), 31 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 8a9adf6f6..6a980eac2 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -313,11 +313,9 @@ VectorValues HessianFactor::hessianDiagonal() const { /* ************************************************************************* */ void HessianFactor::hessianDiagonalAdd(VectorValues &d) const { for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { - auto item = d.find(keys_[j]); - if(item != d.end()) { - item->second += info_.diagonal(j); - } else { - d.emplace(keys_[j], info_.diagonal(j)); + auto result = d.emplace(keys_[j], info_.diagonal(j)); + if(!result.second) { + result.first->second += info_.diagonal(j); } } } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index d0e702f06..af2aa5f45 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -552,28 +552,15 @@ VectorValues JacobianFactor::hessianDiagonal() const { void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const { for (size_t pos = 0; pos < size(); ++pos) { Key j = keys_[pos]; - auto item = d.find(j); + size_t nj = Ab_(pos).cols(); + auto result = d.emplace(j, nj); - if(item != d.end()) { - size_t nj = Ab_(pos).cols(); - Vector& dj = item->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); - } - } else { - size_t nj = Ab_(pos).cols(); - Vector dj(nj); - 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); - } - - d.emplace(j, dj); + 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); } } } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 342dc5c65..4443d6a6d 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -172,11 +172,9 @@ public: dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); } - auto item = d.find(j); - if(item != d.end()) { - item->second += dj; - } else { - d.emplace(j, dj); + auto result = d.emplace(j, dj); + if(!result.second) { + result.first->second += dj; } } } From 9186e656a5235df501758d4c037ff07fa897d12f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 22:00:11 -0400 Subject: [PATCH 10/14] Zero initialize the allocated vector --- gtsam/linear/JacobianFactor.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index af2aa5f45..fcd517edd 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -556,11 +556,15 @@ void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const { 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); + if(!result.second) + dj(k) += dot(column_k, column_k); + else + dj(k) = dot(column_k, column_k); } } } From e8111a129dc2968c8d5001df64153eedc317d894 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 23:06:00 -0400 Subject: [PATCH 11/14] Address comments --- gtsam/linear/HessianFactor.cpp | 1 + gtsam/linear/linearAlgorithms-inst.h | 6 +++--- gtsam/slam/RegularImplicitSchurFactor.h | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 6a980eac2..02948fc04 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -315,6 +315,7 @@ void HessianFactor::hessianDiagonalAdd(VectorValues &d) const { for (DenseIndex j = 0; j < (DenseIndex)size(); ++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); } } diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index e339347f1..811e07121 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -100,9 +100,9 @@ namespace gtsam for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); if(!result.second) - throw std::invalid_argument( - "Requested to emplace variable '" + DefaultKeyFormatter(*frontal) - + "' already in this VectorValues."); + 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); diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 4443d6a6d..8be1b7bb6 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -151,7 +151,7 @@ public: return d; } - /// Return the diagonal of the Hessian for this factor + /// 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 From f2a7864fb6465e7ba27fc06e9e7c9f2f005d7239 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 23:52:38 -0400 Subject: [PATCH 12/14] Further optimization --- gtsam/linear/JacobianFactor.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index fcd517edd..8dfe46c49 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -558,13 +558,20 @@ void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const { 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); - if(!result.second) - dj(k) += dot(column_k, column_k); - else - 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); + } } } } From 4d5d1dc1de80021c0406a9ddb434de39f7a4f2e9 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 3 Jun 2020 13:29:19 -0400 Subject: [PATCH 13/14] Trying to fix old GCC --- gtsam/linear/VectorValues.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 5de34c194..9e60ff2aa 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -182,7 +182,7 @@ namespace gtsam { template inline std::pair emplace(Key j, Args&&... args) { #if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020) - return values_.emplace(j, std::forward(args)...); + 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 From 65036b2019a20656fd0382a9b0a0668ca0db6731 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 3 Jun 2020 20:07:14 -0400 Subject: [PATCH 14/14] Devirtualize hessianDiagonal --- gtsam/linear/GaussianFactor.cpp | 35 +++++++++++++++++++++++++ gtsam/linear/GaussianFactor.h | 2 +- gtsam/linear/HessianFactor.cpp | 9 ------- gtsam/linear/HessianFactor.h | 6 ++--- gtsam/linear/JacobianFactor.cpp | 7 ----- gtsam/linear/JacobianFactor.h | 4 +-- gtsam/linear/RegularJacobianFactor.h | 6 ++--- gtsam/slam/RegularImplicitSchurFactor.h | 11 ++------ 8 files changed, 45 insertions(+), 35 deletions(-) create mode 100644 gtsam/linear/GaussianFactor.cpp 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 {