From 50d9e1ef1c17f0d4e29cebccd1ff1a4496986530 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Apr 2019 16:49:17 -0400 Subject: [PATCH 01/12] numerical expected results rather than regression --- gtsam/linear/tests/testJacobianFactor.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 2ea1b15bd..f6ab4be73 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -322,27 +322,30 @@ TEST(JacobianFactor, matrices) /* ************************************************************************* */ TEST(JacobianFactor, operators ) { - SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); + const double sigma = 0.1; + SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2, sigma); Matrix I = I_2x2; Vector b = Vector2(0.2,-0.1); JacobianFactor lf(1, -I, 2, I, b, sigma0_1); - VectorValues c; - c.insert(1, Vector2(10.,20.)); - c.insert(2, Vector2(30.,60.)); + VectorValues x; + Vector2 x1(10,20), x2(30,60); + x.insert(1, x1); + x.insert(2, x2); // test A*x - Vector expectedE = Vector2(200.,400.); - Vector actualE = lf * c; + Vector expectedE = (x2 - x1)/sigma; + Vector actualE = lf * x; EXPECT(assert_equal(expectedE, actualE)); // test A^e VectorValues expectedX; - expectedX.insert(1, Vector2(-2000.,-4000.)); - expectedX.insert(2, Vector2(2000., 4000.)); + const double alpha = 0.5; + expectedX.insert(1, - alpha * expectedE /sigma); + expectedX.insert(2, alpha * expectedE /sigma); VectorValues actualX = VectorValues::Zero(expectedX); - lf.transposeMultiplyAdd(1.0, actualE, actualX); + lf.transposeMultiplyAdd(alpha, actualE, actualX); EXPECT(assert_equal(expectedX, actualX)); // test gradient at zero From 1a862c24a699487a627ffa790a4e246b7a3066bf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Apr 2019 16:51:39 -0400 Subject: [PATCH 02/12] Reserve memory for cache --- gtsam/linear/SubgraphPreconditioner.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 657846f95..9578d45bd 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -658,6 +658,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) { /* a cache of starting index and dim */ vector > cache; + cache.reserve(3); /* figure out dimension by traversing the keys */ size_t dim = 0; From 60d07287c9db798894420a1816e97d8b36cc3b4a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Apr 2019 16:52:41 -0400 Subject: [PATCH 03/12] made KeyInfoEntry into a struct --- gtsam/linear/IterativeSolver.h | 23 +++++------------------ 1 file changed, 5 insertions(+), 18 deletions(-) diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 92dcbfa07..f0fbfbfd2 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -32,8 +32,8 @@ namespace gtsam { // Forward declarations +struct KeyInfoEntry; class KeyInfo; -class KeyInfoEntry; class GaussianFactorGraph; class Values; class VectorValues; @@ -109,27 +109,14 @@ public: /** * Handy data structure for iterative solvers - * key to (index, dimension, colstart) + * key to (index, dimension, start) */ -class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { - -public: - - typedef boost::tuple Base; - +struct GTSAM_EXPORT KeyInfoEntry { + size_t index, dim, start; KeyInfoEntry() { } KeyInfoEntry(size_t idx, size_t d, Key start) : - Base(idx, d, start) { - } - size_t index() const { - return this->get<0>(); - } - size_t dim() const { - return this->get<1>(); - } - size_t colstart() const { - return this->get<2>(); + index(idx), dim(d), start(start) { } }; From d8147105755d5fda691e736794373dff582be921 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Apr 2019 16:52:52 -0400 Subject: [PATCH 04/12] Added emplace --- gtsam/linear/VectorValues.cpp | 27 ++++++++++++++++---------- gtsam/linear/VectorValues.h | 36 ++++++++++++++++------------------- 2 files changed, 33 insertions(+), 30 deletions(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index ca95313cf..e8304e6e7 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -51,7 +51,7 @@ namespace gtsam { Key key; size_t n; boost::tie(key, n) = v; - values_.insert(make_pair(key, x.segment(j, n))); + values_.emplace(key, x.segment(j, n)); j += n; } } @@ -60,7 +60,7 @@ namespace gtsam { VectorValues::VectorValues(const Vector& x, const Scatter& scatter) { size_t j = 0; for (const SlotEntry& v : scatter) { - values_.insert(make_pair(v.key, x.segment(j, v.dimension))); + values_.emplace(v.key, x.segment(j, v.dimension)); j += v.dimension; } } @@ -70,14 +70,12 @@ namespace gtsam { { VectorValues result; for(const KeyValuePair& v: other) - result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size()))); + result.values_.emplace(v.first, Vector::Zero(v.second.size())); return result; } /* ************************************************************************* */ VectorValues::iterator VectorValues::insert(const std::pair& key_value) { - // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as - // it is inserted into the values_ map. std::pair result = values_.insert(key_value); if(!result.second) throw std::invalid_argument( @@ -86,6 +84,16 @@ namespace gtsam { return result.first; } + /* ************************************************************************* */ + VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) { + std::pair result = values_.emplace(j, value); + 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) { @@ -132,8 +140,7 @@ namespace gtsam { bool VectorValues::equals(const VectorValues& x, double tol) const { if(this->size() != x.size()) return false; - typedef boost::tuple ValuePair; - for(const ValuePair& values: boost::combine(*this, x)) { + for(const auto& values: boost::combine(*this, x)) { if(values.get<0>().first != values.get<1>().first || !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) return false; @@ -240,7 +247,7 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) - result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second)); + result.values_.emplace(j1->first, j1->second + j2->second); return result; } @@ -298,7 +305,7 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) - result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second)); + result.values_.emplace(j1->first, j1->second - j2->second); return result; } @@ -314,7 +321,7 @@ namespace gtsam { { VectorValues result; for(const VectorValues::KeyValuePair& key_v: v) - result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second)); + result.values_.emplace(key_v.first, a * key_v.second); return result; } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 39abe1b56..e0bacfd12 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -50,9 +50,9 @@ namespace gtsam { * Example: * \code VectorValues values; - values.insert(3, Vector3(1.0, 2.0, 3.0)); - values.insert(4, Vector2(4.0, 5.0)); - values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished()); + values.emplace(3, Vector3(1.0, 2.0, 3.0)); + values.emplace(4, Vector2(4.0, 5.0)); + values.emplace(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished()); // Prints [ 3.0 4.0 ] gtsam::print(values[1]); @@ -64,18 +64,7 @@ namespace gtsam { * *

Advanced Interface and Performance Information

* - * For advanced usage, or where speed is important: - * - Allocate space ahead of time using a pre-allocating constructor - * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), - * SameStructure(), resize(), or append(). Do not use - * insert(Key, const Vector&), which always has to re-allocate the - * internal vector. - * - The vector() function permits access to the underlying Vector, for - * doing mathematical or other operations that require all values. - * - operator[]() returns a SubVector view of the underlying Vector, - * without copying any data. - * - * Access is through the variable index j, and returns a SubVector, + * Access is through the variable Key j, and returns a SubVector, * which is a view on the underlying data structure. * * This class is additionally used in gradient descent and dog leg to store the gradient. @@ -183,15 +172,21 @@ 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 insert(Key j, const Vector& value) { - return insert(std::make_pair(j, value)); - } + iterator insert(const std::pair& key_value); + + /** Emplace a vector \c value with key \c j. Throws an invalid_argument exception if the key \c + * 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); /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - iterator insert(const std::pair& key_value); + iterator insert(Key j, const Vector& value) { + return insert(std::make_pair(j, value)); + } /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be * inserted are already used. */ @@ -202,7 +197,8 @@ namespace gtsam { * 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) { - return values_.insert(std::make_pair(j, value)); } + return values_.emplace(j, value); + } /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ void erase(Key var) { From 2cedda703c925287e1b5150861e487cc664c4744 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Apr 2019 16:53:27 -0400 Subject: [PATCH 05/12] Fixed up iterative methods to use struct --- gtsam/linear/IterativeSolver.cpp | 10 +-- gtsam/linear/PCGSolver.cpp | 9 ++- gtsam/linear/SubgraphPreconditioner.cpp | 84 ++++++++++++------------- 3 files changed, 51 insertions(+), 52 deletions(-) diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 411feb7a9..c7d4e5405 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -117,7 +117,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) { for (size_t i = 0; i < n; ++i) { const size_t key = ordering_[i]; const size_t dim = colspec.find(key)->second; - insert(make_pair(key, KeyInfoEntry(i, dim, start))); + this->emplace(key, KeyInfoEntry(i, dim, start)); start += dim; } numCols_ = start; @@ -126,8 +126,8 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) { /****************************************************************************/ vector KeyInfo::colSpec() const { std::vector result(size(), 0); - for ( const KeyInfo::value_type &item: *this ) { - result[item.second.index()] = item.second.dim(); + for ( const auto &item: *this ) { + result[item.second.index] = item.second.dim; } return result; } @@ -135,8 +135,8 @@ vector KeyInfo::colSpec() const { /****************************************************************************/ VectorValues KeyInfo::x0() const { VectorValues result; - for ( const KeyInfo::value_type &item: *this ) { - result.insert(item.first, Vector::Zero(item.second.dim())); + for ( const auto &item: *this ) { + result.emplace(item.first, Vector::Zero(item.second.dim)); } return result; } diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index f9ef756f1..08307c5ab 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -89,7 +89,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { VectorValues vvX = buildVectorValues(x, keyInfo_); // VectorValues form of A'Ax for multiplyHessianAdd - VectorValues vvAtAx; + VectorValues vvAtAx = keyInfo_.x0(); // crucial for performance // vvAtAx += 1.0 * A'Ax for each factor gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); @@ -132,14 +132,14 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, DenseIndex offset = 0; for (size_t i = 0; i < ordering.size(); ++i) { - const Key &key = ordering[i]; + const Key key = ordering[i]; map::const_iterator it = dimensions.find(key); if (it == dimensions.end()) { throw invalid_argument( "buildVectorValues: inconsistent ordering and dimensions"); } const size_t dim = it->second; - result.insert(key, v.segment(offset, dim)); + result.emplace(key, v.segment(offset, dim)); offset += dim; } @@ -150,8 +150,7 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues result; for ( const KeyInfo::value_type &item: keyInfo ) { - result.insert(item.first, - v.segment(item.second.colstart(), item.second.dim())); + result.emplace(item.first, v.segment(item.second.start, item.second.dim)); } return result; } diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 9578d45bd..e011b60d8 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -99,8 +99,8 @@ static vector iidSampler(const vector &weight, const size_t n) { const double value = rand() / denominator; /* binary search the interval containing "value" */ const auto it = std::lower_bound(acc.begin(), acc.end(), value); - const size_t idx = it - acc.begin(); - result.push_back(idx); + const size_t index = it - acc.begin(); + result.push_back(index); } return result; } @@ -129,10 +129,10 @@ static vector UniqueSampler(const vector &weight, const size_t n /* sampling and cache results */ vector samples = iidSampler(localWeights, n-count); - for ( const size_t &id: samples ) { - if ( touched[id] == false ) { - touched[id] = true ; - result.push_back(id); + for ( const size_t &index: samples ) { + if ( touched[index] == false ) { + touched[index] = true ; + result.push_back(index); if ( ++count >= n ) break; } } @@ -143,8 +143,8 @@ static vector UniqueSampler(const vector &weight, const size_t n /****************************************************************************/ Subgraph::Subgraph(const vector &indices) { edges_.reserve(indices.size()); - for ( const size_t &idx: indices ) { - edges_.emplace_back(idx, 1.0); + for ( const size_t &index: indices ) { + edges_.emplace_back(index, 1.0); } } @@ -296,12 +296,12 @@ vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, /****************************************************************/ vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { vector result ; - size_t idx = 0; + size_t index = 0; for (const auto &factor : gfg) { if ( factor->size() == 1 ) { - result.push_back(idx); + result.push_back(index); } - idx++; + index++; } return result; } @@ -309,14 +309,14 @@ vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { /****************************************************************/ vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { vector result ; - size_t idx = 0; + size_t index = 0; for ( const GaussianFactor::shared_ptr &gf: gfg ) { if ( gf->size() == 2 ) { const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; if ( (k1-k0) == 1 || (k0-k1) == 1 ) - result.push_back(idx); + result.push_back(index); } - idx++; + index++; } return result; } @@ -343,13 +343,13 @@ vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { /* traversal */ while ( !q.empty() ) { const size_t head = q.front(); q.pop(); - for ( const size_t id: variableIndex[head] ) { - const GaussianFactor &gf = *gfg[id]; + for ( const size_t index: variableIndex[head] ) { + const GaussianFactor &gf = *gfg[index]; for ( const size_t key: gf.keys() ) { if ( flags.count(key) == 0 ) { q.push(key); flags.insert(key); - result.push_back(id); + result.push_back(index); } } } @@ -363,7 +363,7 @@ vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const vector &weights) const { const VariableIndex variableIndex(gfg); const size_t n = variableIndex.size(); - const vector idx = sort_idx(weights) ; + const vector sortedIndices = sort_idx(weights) ; /* initialize buffer */ vector result; @@ -373,16 +373,16 @@ vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, DSFVector dsf(n); size_t count = 0 ; double sum = 0.0 ; - for (const size_t id: idx) { - const GaussianFactor &gf = *gfg[id]; + for (const size_t index: sortedIndices) { + const GaussianFactor &gf = *gfg[index]; const auto keys = gf.keys(); if ( keys.size() != 2 ) continue; const size_t u = ordering.find(keys[0])->second, v = ordering.find(keys[1])->second; if ( dsf.find(u) != dsf.find(v) ) { dsf.merge(u, v) ; - result.push_back(id) ; - sum += weights[id] ; + result.push_back(index) ; + sum += weights[index] ; if ( ++count == n-1 ) break ; } } @@ -418,8 +418,8 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg } // Downweight the tree edges to zero. - for ( const size_t id: tree ) { - weights[id] = 0.0; + for ( const size_t index: tree ) { + weights[index] = 0.0; } /* decide how many edges to augment */ @@ -614,8 +614,8 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const { /* in place back substitute */ for (const auto &cg : *Rc1_) { - const Vector rhsFrontal = getSubvector( - x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); + const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals()); + const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys); const Vector solFrontal = cg->get_R().transpose().triangularView().solve( rhsFrontal); @@ -625,13 +625,12 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const { throw IndeterminantLinearSystemException(cg->keys().front()); /* assign subvector of sol to the frontal variables */ - setSubvector(solFrontal, keyInfo_, - KeyVector(cg->beginFrontals(), cg->endFrontals()), x); + setSubvector(solFrontal, keyInfo_, frontalKeys, x); /* substract from parent variables */ for (auto it = cg->beginParents(); it != cg->endParents(); it++) { - const KeyInfoEntry &info = keyInfo_.find(*it)->second; - Eigen::Map rhsParent(x.data() + info.colstart(), info.dim(), 1); + const KeyInfoEntry &entry = keyInfo_.find(*it)->second; + Eigen::Map rhsParent(x.data() + entry.start, entry.dim, 1); rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal; } } @@ -664,16 +663,16 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, size_t dim = 0; for (const Key &key : keys) { const KeyInfoEntry &entry = keyInfo.find(key)->second; - cache.emplace_back(entry.colstart(), entry.dim()); - dim += entry.dim(); + cache.emplace_back(entry.start, entry.dim); + dim += entry.dim; } /* use the cache to fill the result */ - Vector result = Vector::Zero(dim); - size_t idx = 0; + Vector result(dim); + size_t index = 0; for (const auto &p : cache) { - result.segment(idx, p.second) = src.segment(p.first, p.second); - idx += p.second; + result.segment(index, p.second) = src.segment(p.first, p.second); + index += p.second; } return result; @@ -681,11 +680,12 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, /*****************************************************************************/ void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) { - size_t idx = 0; + size_t index = 0; for ( const Key &key: keys ) { const KeyInfoEntry &entry = keyInfo.find(key)->second; - dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ; - idx += entry.dim(); + const size_t keyDim = entry.dim; + dst.segment(entry.start, keyDim) = src.segment(index, keyDim) ; + index += keyDim; } } @@ -696,9 +696,9 @@ GaussianFactorGraph::shared_ptr buildFactorSubgraph( auto result = boost::make_shared(); result->reserve(subgraph.size()); for ( const SubgraphEdge &e: subgraph ) { - const size_t idx = e.index(); - if ( clone ) result->push_back(gfg[idx]->clone()); - else result->push_back(gfg[idx]); + const size_t index = e.index(); + if ( clone ) result->push_back(gfg[index]->clone()); + else result->push_back(gfg[index]); } return result; } From 9ec714063a4a889f3f5ecc9a15d7327fc04a8ad5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Apr 2019 16:53:59 -0400 Subject: [PATCH 06/12] many performance tweaks --- gtsam/linear/JacobianFactor.cpp | 50 +++++++++++++++++++-------------- 1 file changed, 29 insertions(+), 21 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2373e7df0..d617ecc15 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -430,10 +430,9 @@ Vector JacobianFactor::unweighted_error(const VectorValues& c) const { /* ************************************************************************* */ Vector JacobianFactor::error_vector(const VectorValues& c) const { - if (model_) - return model_->whiten(unweighted_error(c)); - else - return unweighted_error(c); + Vector e = unweighted_error(c); + if (model_) model_->whitenInPlace(e); + return e; } /* ************************************************************************* */ @@ -474,10 +473,10 @@ VectorValues JacobianFactor::hessianDiagonal() const { for (size_t k = 0; k < nj; ++k) { Vector column_k = Ab_(pos).col(k); if (model_) - column_k = model_->whiten(column_k); + model_->whitenInPlace(column_k); dj(k) = dot(column_k, column_k); } - d.insert(j, dj); + d.emplace(j, dj); } return d; } @@ -496,7 +495,7 @@ map JacobianFactor::hessianBlockDiagonal() const { Matrix Aj = Ab_(pos); if (model_) Aj = model_->Whiten(Aj); - blocks.insert(make_pair(j, Aj.transpose() * Aj)); + blocks.emplace(j, Aj.transpose() * Aj); } return blocks; } @@ -541,29 +540,38 @@ void JacobianFactor::updateHessian(const KeyVector& infoKeys, /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { - Vector Ax = Vector::Zero(Ab_.rows()); + Vector Ax(Ab_.rows()); + Ax.setZero(); if (empty()) return Ax; // Just iterate over all A matrices and multiply in correct config part - for (size_t pos = 0; pos < size(); ++pos) - Ax += Ab_(pos) * x[keys_[pos]]; + for (size_t pos = 0; pos < size(); ++pos) { + // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html + Ax.noalias() += Ab_(pos) * x[keys_[pos]]; + } - return model_ ? model_->whiten(Ax) : Ax; + if (model_) model_->whitenInPlace(Ax); + return Ax; } /* ************************************************************************* */ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, - VectorValues& x) const { - Vector E = alpha * (model_ ? model_->whiten(e) : e); + VectorValues& x) const { + Vector E(e.size()); + E.noalias() = alpha * e; + if (model_) model_->whitenInPlace(E); // Just iterate over all A matrices and insert Ai^e into VectorValues for (size_t pos = 0; pos < size(); ++pos) { - Key j = keys_[pos]; - // Create the value as a zero vector if it does not exist. - pair xi = x.tryInsert(j, Vector()); - if (xi.second) - xi.first->second = Vector::Zero(getDim(begin() + pos)); - xi.first->second += Ab_(pos).transpose()*E; + const Key j = keys_[pos]; + // To avoid another malloc if key exists, we explicitly check + auto it = x.find(j); + if (it != x.end()) { + // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html + it->second.noalias() += Ab_(pos).transpose() * E; + } else { + x.emplace(j, Ab_(pos).transpose() * E); + } } } @@ -625,8 +633,8 @@ VectorValues JacobianFactor::gradientAtZero() const { Vector b = getb(); // Gradient is really -A'*b / sigma^2 // transposeMultiplyAdd will divide by sigma once, so we need one more - Vector b_sigma = model_ ? model_->whiten(b) : b; - this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2 + if (model_) model_->whitenInPlace(b); + this->transposeMultiplyAdd(-1.0, b, g); // g -= A'*b/sigma^2 return g; } From 35d8ebca22ddd89e671078e48396fe9bd59fc632 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Apr 2019 16:54:20 -0400 Subject: [PATCH 07/12] use emplace where possible --- gtsam/linear/GaussianConditional.cpp | 4 ++-- gtsam/linear/GaussianFactorGraph.cpp | 4 ++-- gtsam/linear/HessianFactor.cpp | 6 +++--- gtsam/linear/linearAlgorithms-inst.h | 6 +++--- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 93217c027..ed4a6e091 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -134,7 +134,7 @@ namespace gtsam { VectorValues result; DenseIndex vectorPosition = 0; for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { - result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal))); + result.emplace(*frontal, solution.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } @@ -162,7 +162,7 @@ namespace gtsam { VectorValues result; DenseIndex vectorPosition = 0; for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { - result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); + result.emplace(*frontal, soln.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index a4df04cf9..9281c7e7a 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -61,7 +61,7 @@ namespace gtsam { for (GaussianFactor::const_iterator it = gf->begin(); it != gf->end(); it++) { map::iterator it2 = spec.find(*it); if ( it2 == spec.end() ) { - spec.insert(make_pair(*it, gf->getDim(it))); + spec.emplace(*it, gf->getDim(it)); } } } @@ -246,7 +246,7 @@ namespace gtsam { if (blocks.count(j)) blocks[j] += Bj; else - blocks.insert(make_pair(j,Bj)); + blocks.emplace(j,Bj); } } return blocks; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index cded6d2ee..da7d7bd7b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -305,7 +305,7 @@ Matrix HessianFactor::information() const { VectorValues HessianFactor::hessianDiagonal() const { VectorValues d; for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { - d.insert(keys_[j], info_.diagonal(j)); + d.emplace(keys_[j], info_.diagonal(j)); } return d; } @@ -436,7 +436,7 @@ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; size_t n = size(); for (size_t j = 0; j < n; ++j) - g.insert(keys_[j], -info_.aboveDiagonalBlock(j, n)); + g.emplace(keys_[j], -info_.aboveDiagonalBlock(j, n)); return g; } @@ -513,7 +513,7 @@ VectorValues HessianFactor::solve() { std::size_t offset = 0; for (DenseIndex j = 0; j < (DenseIndex)n; ++j) { const DenseIndex dim = info_.getDim(j); - delta.insert(keys_[j], solution.segment(offset, dim)); + delta.emplace(keys_[j], solution.segment(offset, dim)); offset += dim; } diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index ac8c2d7c7..8b83ce0c3 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -56,7 +56,7 @@ namespace gtsam myData.parentData = parentData; // Take any ancestor results we'll need for(Key parent: clique->conditional_->parents()) - myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent))); + myData.cliqueResults.emplace(parent, myData.parentData->cliqueResults.at(parent)); // Solve and store in our results { @@ -99,8 +99,8 @@ namespace gtsam DenseIndex vectorPosition = 0; for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { VectorValues::const_iterator r = - collectedResult.insert(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); - myData.cliqueResults.insert(make_pair(r->first, r)); + collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); + myData.cliqueResults.emplace(r->first, r); vectorPosition += c.getDim(frontal); } } From d8570ce09b380a899fee3a5ed0c74186f4c3c7c1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 10 Apr 2019 22:25:33 -0400 Subject: [PATCH 08/12] Fixed issue with << operator (surfaced in debug mode) --- gtsam/slam/InitializePose3.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 2f247811d..a1baab5fa 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -82,7 +82,11 @@ Values InitializePose3::normalizeRelaxedRotations( for(const auto& it: relaxedRot3) { Key key = it.first; if (key != kAnchorKey) { - Matrix3 R; R << it.second; + Matrix3 R; + R << Eigen::Map(it.second.data()); // Recover M from vectorized + + // ClosestTo finds rotation matrix closest to H in Frobenius sense + // Rot3 initRot = Rot3::ClosestTo(M.transpose()); Matrix U, V; Vector s; svd(R.transpose(), U, s, V); From 35d9f65d9c72e682bd1482f96751b937f7dcd206 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 10 Apr 2019 22:26:34 -0400 Subject: [PATCH 09/12] Cut out middle-man, added noalias for better performance --- gtsam/linear/GaussianConditional.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index ed4a6e091..445941fa8 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -172,13 +172,13 @@ namespace gtsam { /* ************************************************************************* */ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals())); - frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); + frontalVec = get_R().triangularView().solve(frontalVec); // Check for indeterminant solution if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front()); for (const_iterator it = beginParents(); it!= endParents(); it++) - gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec; + gy[*it].noalias() += -1.0 * getA(it).transpose() * frontalVec; // Scale by sigmas if (model_) From 8acba8eabe34cec5f2b36652c75f979bfb73ed6c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 10 Apr 2019 22:30:39 -0400 Subject: [PATCH 10/12] ignore debug directory --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index ad0e08aa1..03ce578e7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ /build* +/debug* .idea *.pyc *.DS_Store From 7b6eaa43339e0dbc8246b99d93e5b4f3ec138325 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Apr 2019 11:00:52 -0400 Subject: [PATCH 11/12] removed unused typedefs --- gtsam/slam/tests/testSmartProjectionCameraFactor.cpp | 1 - gtsam_unstable/examples/SmartProjectionFactorExample.cpp | 2 -- gtsam_unstable/partition/GenericGraph.cpp | 1 - tests/testExpressionFactor.cpp | 1 - 4 files changed, 5 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index aaffbf0e6..ddb4d8adb 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -46,7 +46,6 @@ static double rankTol = 1.0; template PinholeCamera perturbCameraPoseAndCalibration( const PinholeCamera& camera) { - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 94a70470a..e290cef7e 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -43,8 +43,6 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv){ - - typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp index 67d767799..92f0266d0 100644 --- a/gtsam_unstable/partition/GenericGraph.cpp +++ b/gtsam_unstable/partition/GenericGraph.cpp @@ -353,7 +353,6 @@ namespace gtsam { namespace partition { void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, const std::vector& dictionary, GenericGraph3D& reducedGraph) { - typedef size_t CameraKey; typedef size_t LandmarkKey; // get a mapping from each landmark to its connected cameras vector > cameraToLandmarks(dictionary.size()); diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 769b458e4..161e6976b 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -243,7 +243,6 @@ TEST(ExpressionFactor, Shallow) { // traceExecution of shallow tree typedef internal::UnaryExpression Unary; - typedef internal::BinaryExpression Binary; size_t size = expression.traceSize(); internal::ExecutionTraceStorage traceStorage[size]; internal::ExecutionTrace trace; From 948367603b2968acc17b22cc6a3a0096b3755519 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Apr 2019 22:06:18 -0400 Subject: [PATCH 12/12] Fixed missing transpose --- gtsam/linear/GaussianConditional.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 445941fa8..85569de14 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -172,7 +172,7 @@ namespace gtsam { /* ************************************************************************* */ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals())); - frontalVec = get_R().triangularView().solve(frontalVec); + frontalVec = get_R().transpose().triangularView().solve(frontalVec); // Check for indeterminant solution if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());