Merged in feature/pcg_performance (pull request #413)

Feature/pcg performance

.gitignore

@@ -1,4 +1,5 @@
 /build*
+/debug*
 .idea
 *.pyc
 *.DS_Store

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);
     }
 
@@ -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().transpose().triangularView<Eigen::Lower>().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_)

gtsam/linear/GaussianFactorGraph.cpp

@@ -61,7 +61,7 @@ namespace gtsam {
       for (GaussianFactor::const_iterator it = gf->begin(); it != gf->end(); it++) {
         map<Key,size_t>::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;

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;
   }
 

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<size_t> KeyInfo::colSpec() const {
   std::vector<size_t> result(size(), 0);
-  for ( const KeyInfo::value_type &item: *this ) {
+  for ( const auto &item: *this ) {
-    result[item.second.index()] = item.second.dim();
+    result[item.second.index] = item.second.dim;
   }
   return result;
 }
@@ -135,8 +135,8 @@ vector<size_t> KeyInfo::colSpec() const {
 /****************************************************************************/
 VectorValues KeyInfo::x0() const {
   VectorValues result;
-  for ( const KeyInfo::value_type &item: *this ) {
+  for ( const auto &item: *this ) {
-    result.insert(item.first, Vector::Zero(item.second.dim()));
+    result.emplace(item.first, Vector::Zero(item.second.dim));
   }
   return result;
 }

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<Key, size_t, Key> {
+struct GTSAM_EXPORT KeyInfoEntry {
-
+  size_t index, dim, start;
-public:
-
-  typedef boost::tuple<Key, size_t, Key> Base;
-
   KeyInfoEntry() {
   }
   KeyInfoEntry(size_t idx, size_t d, Key start) :
-      Base(idx, d, start) {
+      index(idx), dim(d), start(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>();
   }
 };
 

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_)
+  Vector e = unweighted_error(c);
-    return model_->whiten(unweighted_error(c));
+  if (model_) model_->whitenInPlace(e);
-  else
+  return e;
-    return unweighted_error(c);
 }
 
 /* ************************************************************************* */
@@ -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<Key, Matrix> 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)
+  for (size_t pos = 0; pos < size(); ++pos) {
-    Ax += Ab_(pos) * x[keys_[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);
+  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];
+    const Key j = keys_[pos];
-    // Create the value as a zero vector if it does not exist.
+    // To avoid another malloc if key exists, we explicitly check
-    pair<VectorValues::iterator, bool> xi = x.tryInsert(j, Vector());
+    auto it = x.find(j);
-    if (xi.second)
+    if (it != x.end()) {
-      xi.first->second = Vector::Zero(getDim(begin() + pos));
+      // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html
-    xi.first->second += Ab_(pos).transpose()*E;
+      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;
+  if (model_) model_->whitenInPlace(b);
-  this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2
+  this->transposeMultiplyAdd(-1.0, b, g); // g -= A'*b/sigma^2
   return g;
 }
 

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<Key, size_t>::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,
+    result.emplace(item.first, v.segment(item.second.start, item.second.dim));
-        v.segment(item.second.colstart(), item.second.dim()));
   }
   return result;
 }

gtsam/linear/SubgraphPreconditioner.cpp

@@ -99,8 +99,8 @@ static vector<size_t> iidSampler(const vector<double> &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();
+    const size_t index = it - acc.begin();
-    result.push_back(idx);
+    result.push_back(index);
   }
   return result;
 }
@@ -129,10 +129,10 @@ static vector<size_t> UniqueSampler(const vector<double> &weight, const size_t n
 
     /* sampling and cache results */
     vector<size_t> samples = iidSampler(localWeights, n-count);
-    for ( const size_t &id: samples ) {
+    for ( const size_t &index: samples ) {
-      if ( touched[id] == false ) {
+      if ( touched[index] == false ) {
-        touched[id] = true ;
+        touched[index] = true ;
-        result.push_back(id);
+        result.push_back(index);
         if ( ++count >= n ) break;
       }
     }
@@ -143,8 +143,8 @@ static vector<size_t> UniqueSampler(const vector<double> &weight, const size_t n
 /****************************************************************************/
 Subgraph::Subgraph(const vector<size_t> &indices) {
   edges_.reserve(indices.size());
-  for ( const size_t &idx: indices ) {
+  for ( const size_t &index: indices ) {
-    edges_.emplace_back(idx, 1.0);
+    edges_.emplace_back(index, 1.0);
   }
 }
 
@@ -296,12 +296,12 @@ vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
 /****************************************************************/
 vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
   vector<size_t> 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<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
 /****************************************************************/
 vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
   vector<size_t> 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<size_t> 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] ) {
+    for ( const size_t index: variableIndex[head] ) {
-      const GaussianFactor &gf = *gfg[id];
+      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<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
                                         const vector<double> &weights) const {
   const VariableIndex variableIndex(gfg);
   const size_t n = variableIndex.size();
-  const vector<size_t> idx = sort_idx(weights) ;
+  const vector<size_t> sortedIndices = sort_idx(weights) ;
 
   /* initialize buffer */
   vector<size_t> result;
@@ -373,16 +373,16 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
   DSFVector dsf(n);
 
   size_t count = 0 ; double sum = 0.0 ;
-  for (const size_t id: idx) {
+  for (const size_t index: sortedIndices) {
-    const GaussianFactor &gf = *gfg[id];
+    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) ;
+      result.push_back(index) ;
-      sum += weights[id] ;
+      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 ) {
+  for ( const size_t index: tree ) {
-    weights[id] = 0.0;
+    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(
+    const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
-        x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
+    const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys);
     const Vector solFrontal =
         cg->get_R().transpose().triangularView<Eigen::Lower>().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_,
+    setSubvector(solFrontal, keyInfo_, frontalKeys, x);
-                 KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
 
     /* substract from parent variables */
     for (auto it = cg->beginParents(); it != cg->endParents(); it++) {
-      const KeyInfoEntry &info = keyInfo_.find(*it)->second;
+      const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
-      Eigen::Map<Vector> rhsParent(x.data() + info.colstart(), info.dim(), 1);
+      Eigen::Map<Vector> rhsParent(x.data() + entry.start, entry.dim, 1);
       rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
     }
   }
@@ -658,21 +657,22 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
                     const KeyVector &keys) {
   /* a cache of starting index and dim */
   vector<std::pair<size_t, size_t> > cache;
+  cache.reserve(3);
 
   /* figure out dimension by traversing the keys */
   size_t dim = 0;
   for (const Key &key : keys) {
     const KeyInfoEntry &entry = keyInfo.find(key)->second;
-    cache.emplace_back(entry.colstart(), entry.dim());
+    cache.emplace_back(entry.start, entry.dim);
-    dim += entry.dim();
+    dim += entry.dim;
   }
 
   /* use the cache to fill the result */
-  Vector result = Vector::Zero(dim);
+  Vector result(dim);
-  size_t idx = 0;
+  size_t index = 0;
   for (const auto &p : cache) {
-    result.segment(idx, p.second) = src.segment(p.first, p.second);
+    result.segment(index, p.second) = src.segment(p.first, p.second);
-    idx += p.second;
+    index += p.second;
   }
 
   return result;
@@ -680,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()) ;
+    const size_t keyDim = entry.dim;
-    idx += entry.dim();
+    dst.segment(entry.start, keyDim) = src.segment(index, keyDim) ;
+    index += keyDim;
   }
 }
 
@@ -695,9 +696,9 @@ GaussianFactorGraph::shared_ptr buildFactorSubgraph(
   auto result = boost::make_shared<GaussianFactorGraph>();
   result->reserve(subgraph.size());
   for ( const SubgraphEdge &e: subgraph ) {
-    const size_t idx = e.index();
+    const size_t index = e.index();
-    if ( clone ) result->push_back(gfg[idx]->clone());
+    if ( clone ) result->push_back(gfg[index]->clone());
-    else result->push_back(gfg[idx]);
+    else result->push_back(gfg[index]);
   }
   return result;
 }

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, Vector>& 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<iterator, bool> 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<iterator, bool> 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<value_type, value_type> ValuePair;
+    for(const auto& values: boost::combine(*this, x)) {
-    for(const ValuePair& 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;
   }
 

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.emplace(3, Vector3(1.0, 2.0, 3.0));
-     values.insert(4, Vector2(4.0, 5.0));
+     values.emplace(4, Vector2(4.0, 5.0));
-     values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished());
+     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 {
    *
    * <h2>Advanced Interface and Performance Information</h2>
    *
-   * For advanced usage, or where speed is important:
+   * Access is through the variable Key j, and returns a SubVector,
-   *  - 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,
    * 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) {
+    iterator insert(const std::pair<Key, Vector>& key_value);
-      return insert(std::make_pair(j, 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, Vector>& 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<iterator, bool> 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) {

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)));
+                collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
-              myData.cliqueResults.insert(make_pair(r->first, r));
+              myData.cliqueResults.emplace(r->first, r);
               vectorPosition += c.getDim(frontal);
             }
           }

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;
+  VectorValues x;
-  c.insert(1, Vector2(10.,20.));
+  Vector2 x1(10,20), x2(30,60);
-  c.insert(2, Vector2(30.,60.));
+  x.insert(1, x1);
+  x.insert(2, x2);
 
   // test A*x
-  Vector expectedE = Vector2(200.,400.);
+  Vector expectedE = (x2 - x1)/sigma;
-  Vector actualE = lf * c;
+  Vector actualE = lf * x;
   EXPECT(assert_equal(expectedE, actualE));
 
   // test A^e
   VectorValues expectedX;
-  expectedX.insert(1, Vector2(-2000.,-4000.));
+  const double alpha = 0.5;
-  expectedX.insert(2, Vector2(2000., 4000.));
+  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

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<const Matrix3>(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);

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

@@ -46,7 +46,6 @@ static double rankTol = 1.0;
 template<class CALIBRATION>
 PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
     const PinholeCamera<CALIBRATION>& 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();

gtsam_unstable/examples/SmartProjectionFactorExample.cpp

@@ -43,8 +43,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv){
-
-  typedef PinholePose<Cal3_S2> Camera;
   typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
 
   Values initial_estimate;

gtsam_unstable/partition/GenericGraph.cpp

@@ -353,7 +353,6 @@ namespace gtsam { namespace partition {
   void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys,  const std::vector<size_t>& landmarkKeys,
       const std::vector<int>& dictionary,  GenericGraph3D& reducedGraph) {
 
-    typedef size_t CameraKey;
     typedef size_t LandmarkKey;
     // get a mapping from each landmark to its connected cameras
     vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());

tests/testExpressionFactor.cpp

@@ -243,7 +243,6 @@ TEST(ExpressionFactor, Shallow) {
 
   // traceExecution of shallow tree
   typedef internal::UnaryExpression<Point2, Point3> Unary;
-  typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
   size_t size = expression.traceSize();
   internal::ExecutionTraceStorage traceStorage[size];
   internal::ExecutionTrace<Point2> trace;