Merge branch 'refs/heads/isam2-chain-optimization' into trunk

gtsam.h

@@ -724,7 +724,6 @@ class Permutation {
   // FIXME: Cannot currently wrap std::vector
   //static gtsam::Permutation PullToFront(const vector<size_t>& toFront, size_t size, bool filterDuplicates);
   //static gtsam::Permutation PushToBack(const vector<size_t>& toBack, size_t size, bool filterDuplicates = false);
-    gtsam::Permutation* partialPermutation(const gtsam::Permutation& selector, const gtsam::Permutation& partialPermutation) const;
 };
 
 class IndexFactor {
@@ -772,7 +771,6 @@ class IndexConditional {
   gtsam::IndexFactor* toFactor() const;
 
   //Advanced interface
-  bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
   void permuteWithInverse(const gtsam::Permutation& inversePermutation);
 };
 
@@ -795,7 +793,6 @@ virtual class BayesNet {
   void push_front(This& conditional);
   void pop_front();
   void permuteWithInverse(const gtsam::Permutation& inversePermutation);
-  bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
 };
 
 #include <gtsam/inference/BayesTree.h>
@@ -840,7 +837,6 @@ virtual class BayesTreeClique {
 //  derived_ptr parent() const { return parent_.lock(); }
 
   void permuteWithInverse(const gtsam::Permutation& inversePermutation);
-  bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
 
   // FIXME: need wrapped versions graphs, BayesNet
 //  BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
@@ -868,7 +864,6 @@ virtual class SymbolicBayesNet  : gtsam::SymbolicBayesNetBase {
   //Advanced Interface
   void pop_front();
   void permuteWithInverse(const gtsam::Permutation& inversePermutation);
-  bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
 };
 
 typedef gtsam::BayesTreeClique<gtsam::IndexConditional> SymbolicBayesTreeClique;
@@ -1050,12 +1045,11 @@ class VectorValues {
   //Standard Interface
   size_t size() const;
   size_t dim(size_t j) const;
-  size_t dim() const;
   bool exists(size_t j) const;
   void print(string s) const;
   bool equals(const gtsam::VectorValues& expected, double tol) const;
   void insert(size_t j, Vector value);
-  Vector vector() const;
+  Vector asVector() const;
   Vector at(size_t j) const;
 
   //Advanced Interface
@@ -1821,7 +1815,6 @@ virtual class ISAM2Clique {
     void print(string s);
 
     void permuteWithInverse(const gtsam::Permutation& inversePermutation);
-    bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
 };
 
 class ISAM2Result {

gtsam/inference/BayesNet-inl.h

@@ -117,18 +117,6 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
-  template<class CONDITIONAL>
-  bool BayesNet<CONDITIONAL>::permuteSeparatorWithInverse(
-      const Permutation& inversePermutation) {
-    bool separatorChanged = false;
-    BOOST_FOREACH(sharedConditional conditional, conditionals_) {
-      if (conditional->permuteSeparatorWithInverse(inversePermutation))
-        separatorChanged = true;
-    }
-    return separatorChanged;
-  }
-
   /* ************************************************************************* */
   template<class CONDITIONAL>
   void BayesNet<CONDITIONAL>::push_back(const BayesNet<CONDITIONAL>& bn) {

gtsam/inference/BayesNet.h

@@ -220,13 +220,6 @@ public:
   /** Permute the variables in the BayesNet */
   void permuteWithInverse(const Permutation& inversePermutation);
 
-  /**
-   * Permute the variables when only separator variables need to be permuted.
-   * Returns true if any reordered variables appeared in the separator and
-   * false if not.
-   */
-  bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
-
   iterator begin()          {return conditionals_.begin();}    ///<TODO: comment
   iterator end()            {return conditionals_.end();}      ///<TODO: comment
   reverse_iterator rbegin() {return conditionals_.rbegin();}  ///<TODO: comment

gtsam/inference/BayesTree.h

@@ -92,7 +92,7 @@ namespace gtsam {
     };
 
     /** Map from indices to Clique */
-    typedef std::deque<sharedClique> Nodes;
+    typedef std::vector<sharedClique> Nodes;
 
   protected:
 

gtsam/inference/BayesTreeCliqueBase-inl.h

@@ -134,22 +134,19 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class DERIVED, class CONDITIONAL>
-  bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::permuteSeparatorWithInverse(
+  bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::reduceSeparatorWithInverse(
-      const Permutation& inversePermutation) {
+    const internal::Reduction& inverseReduction)
-    bool changed = conditional_->permuteSeparatorWithInverse(
+  {
-        inversePermutation);
+    bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction);
 #ifndef NDEBUG
     if(!changed) {
-      BOOST_FOREACH(Index& separatorKey, conditional_->parents()) {assert(separatorKey == inversePermutation[separatorKey]);}
       BOOST_FOREACH(const derived_ptr& child, children_) {
-        assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
+        assert(child->reduceSeparatorWithInverse(inverseReduction) == false); }
-      }
     }
 #endif
-    if (changed) {
+    if(changed) {
       BOOST_FOREACH(const derived_ptr& child, children_) {
-        (void) child->permuteSeparatorWithInverse(inversePermutation);
+        (void) child->reduceSeparatorWithInverse(inverseReduction); }
-      }
     }
     assertInvariants();
     return changed;

gtsam/inference/BayesTreeCliqueBase.h

@@ -183,7 +183,7 @@ namespace gtsam {
      * traversing the whole tree.  Returns whether any separator variables in
      * this subtree were reordered.
      */
-    bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
+    bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
 
     /** return the conditional P(S|Root) on the separator given the root */
     BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;

gtsam/inference/IndexConditional.cpp

@@ -44,16 +44,16 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
+  bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
-  #ifndef NDEBUG
+#ifndef NDEBUG
-    BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
+    BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); }
-  #endif
+#endif
     bool parentChanged = false;
     BOOST_FOREACH(KeyType& parent, parents()) {
-      KeyType newParent = inversePermutation[parent];
+      internal::Reduction::const_iterator it = inverseReduction.find(parent);
-      if(parent != newParent) {
+      if(it != inverseReduction.end()) {
         parentChanged = true;
-        parent = newParent;
+        parent = it->second;
       }
     }
     assertInvariants();

gtsam/inference/IndexConditional.h

@@ -112,7 +112,13 @@ namespace gtsam {
      * Returns true if any reordered variables appeared in the separator and
      * false if not.
      */
-    bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
+    //bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
+
+    /** Permute the variables when only separator variables need to be permuted.
+     * Returns true if any reordered variables appeared in the separator and
+     * false if not.
+     */
+    bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
 
     /**
      * Permutes the Conditional, but for efficiency requires the permutation

gtsam/inference/Permutation.cpp

@@ -123,18 +123,6 @@ Permutation::shared_ptr Permutation::permute(const Permutation& permutation) con
   return result;
 }
 
-/* ************************************************************************* */
-Permutation::shared_ptr Permutation::partialPermutation(
-    const Permutation& selector, const Permutation& partialPermutation) const {
-  assert(selector.size() == partialPermutation.size());
-  Permutation::shared_ptr result(new Permutation(*this));
-
-  for(Index subsetPos=0; subsetPos<selector.size(); ++subsetPos)
-    (*result)[selector[subsetPos]] = (*this)[selector[partialPermutation[subsetPos]]];
-
-  return result;
-}
-
 /* ************************************************************************* */
 Permutation::shared_ptr Permutation::inverse() const {
   Permutation::shared_ptr result(new Permutation(this->size()));
@@ -161,6 +149,16 @@ namespace internal {
     return result;
   }
 
+  /* ************************************************************************* */
+  Reduction Reduction::CreateFromPartialPermutation(const Permutation& selector, const Permutation& p) {
+    if(selector.size() != p.size())
+      throw invalid_argument("internal::Reduction::CreateFromPartialPermutation called with selector and permutation of different sizes");
+    Reduction result;
+    for(size_t dstSlot = 0; dstSlot < p.size(); ++dstSlot)
+      result.insert(make_pair(selector[dstSlot], selector[p[dstSlot]]));
+    return result;
+  }
+
   /* ************************************************************************* */
   void Reduction::applyInverse(std::vector<Index>& js) const {
     BOOST_FOREACH(Index& j, js) {
@@ -183,10 +181,10 @@ namespace internal {
   }
 
   /* ************************************************************************* */
-  Index& Reduction::operator[](const Index& j) {
+  const Index& Reduction::operator[](const Index& j) {
     iterator it = this->find(j);
     if(it == this->end())
-      throw std::out_of_range("Index to Reduction::operator[] not present");
+      return j;
     else
       return it->second;
   }
@@ -195,7 +193,7 @@ namespace internal {
   const Index& Reduction::operator[](const Index& j) const {
     const_iterator it = this->find(j);
     if(it == this->end())
-      throw std::out_of_range("Index to Reduction::operator[] not present");
+      return j;
     else
       return it->second;
   }
@@ -207,6 +205,11 @@ namespace internal {
       cout << "  " << p.first << " : " << p.second << endl;
   }
 
+  /* ************************************************************************* */
+  bool Reduction::equals(const Reduction& other, double tol) const {
+    return (const Base&)(*this) == (const Base&)other;
+  }
+
   /* ************************************************************************* */
   Permutation createReducingPermutation(const std::set<Index>& indices) {
     Permutation p(indices.size());

gtsam/inference/Permutation.h

@@ -167,14 +167,6 @@ public:
   /// @name Advanced Interface
   /// @{
 
-
-  /**
-   * A partial permutation, reorders the variables selected by selector through
-   * partialPermutation.  selector and partialPermutation should have the same
-   * size, this is checked if NDEBUG is not defined.
-   */
-  Permutation::shared_ptr partialPermutation(const Permutation& selector, const Permutation& partialPermutation) const;
-
   iterator begin() { return rangeIndices_.begin(); }  ///< Iterate through the indices
   iterator end() { return rangeIndices_.end(); }      ///< Iterate through the indices
 
@@ -192,12 +184,14 @@ namespace internal {
     typedef gtsam::FastMap<Index,Index> Base;
 
     static Reduction CreateAsInverse(const Permutation& p);
+    static Reduction CreateFromPartialPermutation(const Permutation& selector, const Permutation& p);
     void applyInverse(std::vector<Index>& js) const;
     Permutation inverse() const;
-    Index& operator[](const Index& j);
+    const Index& operator[](const Index& j);
     const Index& operator[](const Index& j) const;
 
     void print(const std::string& s="") const;
+    bool equals(const Reduction& other, double tol = 1e-9) const;
   };
 
   // Reduce the variable indices so that those in the set are mapped to start at zero

gtsam/inference/VariableIndex.cpp

@@ -68,7 +68,7 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
 /* ************************************************************************* */
 void VariableIndex::permuteInPlace(const Permutation& permutation) {
   // Create new index and move references to data into it in permuted order
-  deque<VariableIndex::Factors> newIndex(this->size());
+  vector<VariableIndex::Factors> newIndex(this->size());
   for(Index i = 0; i < newIndex.size(); ++i)
     newIndex[i].swap(this->index_[permutation[i]]);
 
@@ -76,6 +76,20 @@ void VariableIndex::permuteInPlace(const Permutation& permutation) {
   index_.swap(newIndex);
 }
 
+/* ************************************************************************* */
+void VariableIndex::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
+  if(selector.size() != permutation.size())
+    throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
+  // Create new index the size of the permuted entries
+  vector<VariableIndex::Factors> newIndex(selector.size());
+  // Permute the affected entries into the new index
+  for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
+    newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]);
+  // Put the affected entries back in the new order
+  for(size_t slot = 0; slot < selector.size(); ++slot)
+    this->index_[selector[slot]].swap(newIndex[slot]);
+}
+
 /* ************************************************************************* */
 void VariableIndex::removeUnusedAtEnd(size_t nToRemove) {
 #ifndef NDEBUG

gtsam/inference/VariableIndex.h

@@ -48,7 +48,7 @@ public:
   typedef Factors::const_iterator Factor_const_iterator;
 
 protected:
-  std::deque<Factors> index_;
+  std::vector<Factors> index_;
   size_t nFactors_; // Number of factors in the original factor graph.
   size_t nEntries_; // Sum of involved variable counts of each factor.
 
@@ -135,6 +135,9 @@ public:
   /// Permute the variables in the VariableIndex according to the given permutation
   void permuteInPlace(const Permutation& permutation);
 
+  /// Permute the variables in the VariableIndex according to the given partial permutation
+  void permuteInPlace(const Permutation& selector, const Permutation& permutation);
+
   /** Remove unused empty variables at the end of the ordering (in debug mode
    * verifies they are empty).
    * @param nToRemove The number of unused variables at the end to remove

gtsam/inference/tests/testPermutation.cpp

@@ -25,6 +25,7 @@
 using namespace gtsam;
 using namespace std;
 
+/* ************************************************************************* */
 TEST(Permutation, Identity) {
   Permutation expected(5);
   expected[0] = 0;
@@ -38,6 +39,7 @@ TEST(Permutation, Identity) {
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
 TEST(Permutation, PullToFront) {
   Permutation expected(5);
   expected[0] = 4;
@@ -55,6 +57,7 @@ TEST(Permutation, PullToFront) {
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
 TEST(Permutation, PushToBack) {
   Permutation expected(5);
   expected[0] = 1;
@@ -72,6 +75,7 @@ TEST(Permutation, PushToBack) {
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
 TEST(Permutation, compose) {
   Permutation p1(5);
   p1[0] = 1;
@@ -104,6 +108,25 @@ TEST(Permutation, compose) {
   LONGS_EQUAL(p1[p2[4]], actual[4]);
 }
 
+/* ************************************************************************* */
+TEST(Reduction, CreateFromPartialPermutation) {
+  Permutation selector(3);
+  selector[0] = 2;
+  selector[1] = 4;
+  selector[2] = 6;
+  Permutation p(3);
+  p[0] = 2;
+  p[1] = 0;
+  p[2] = 1;
+
+  internal::Reduction expected;
+  expected.insert(make_pair(2,6));
+  expected.insert(make_pair(4,2));
+  expected.insert(make_pair(6,4));
+
+  internal::Reduction actual = internal::Reduction::CreateFromPartialPermutation(selector, p);
+}
+
 /* ************************************************************************* */
 int main() {  TestResult tr;  return TestRegistry::runAllTests(tr); }
 

gtsam/linear/GaussianFactorGraph.cpp

@@ -607,11 +607,11 @@ break;
 
   /* ************************************************************************* */
   void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
-    r.vector() = Vector::Zero(r.dim());
+    r.setZero();
     Index i = 0;
     BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
       JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
-      SubVector &y = r[i];
+      Vector &y = r[i];
       for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
         y += Ai->getA(j) * x[*j];
       }
@@ -621,7 +621,7 @@ break;
 
   /* ************************************************************************* */
   void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
-    x.vector() = Vector::Zero(x.dim());
+    x.setZero();
     Index i = 0;
     BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
       JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);

gtsam/linear/GaussianISAM.h

@@ -30,11 +30,11 @@ namespace gtsam {
 class GaussianISAM : public ISAM<GaussianConditional> {
 
   typedef ISAM<GaussianConditional> Super;
-  std::deque<size_t, boost::fast_pool_allocator<size_t> > dims_;
+  std::vector<size_t> dims_;
 
 public:
 
-  typedef std::deque<size_t, boost::fast_pool_allocator<size_t> > Dims;
+  typedef std::vector<size_t> Dims;
 
   /** Create an empty Bayes Tree */
   GaussianISAM() : Super() {}

gtsam/linear/HessianFactor.cpp

@@ -325,17 +325,11 @@ double HessianFactor::error(const VectorValues& c) const {
   // error 0.5*(f - 2*x'*g + x'*G*x)
   const double f = constantTerm();
   double xtg = 0, xGx = 0;
-  if (c.dim() == this->rows()) {
+  // extract the relevant subset of the VectorValues
-    // If using the full vector values, this will reduce copying
+  // NOTE may not be as efficient
-    xtg = c.vector().dot(linearTerm());
+  const Vector x = c.vector(this->keys());
-    xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() *  c.vector();
+  xtg = x.dot(linearTerm());
-  } else {
+  xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() *  x;
-    // extract the relevant subset of the VectorValues
-    // NOTE may not be as efficient
-    const Vector x = c.vector(this->keys());
-    xtg = x.dot(linearTerm());
-    xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() *  x;
-  }
   return 0.5 * (f - 2.0 * xtg +  xGx);
 }
 

gtsam/linear/VectorValues.cpp

@@ -20,29 +20,19 @@
 #include <gtsam/inference/Permutation.h>
 #include <gtsam/linear/VectorValues.h>
 
+#include <boost/iterator/counting_iterator.hpp>
+
 using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
-VectorValues::VectorValues(const VectorValues& other) {
-  *this = other;
-}
-
-/* ************************************************************************* */
-VectorValues& VectorValues::operator=(const VectorValues& rhs) {
-  if(this != &rhs) {
-    resizeLike(rhs);        // Copy structure
-    values_ = rhs.values_;  // Copy values
-  }
-  return *this;
-}
-
 /* ************************************************************************* */
 VectorValues VectorValues::Zero(const VectorValues& x) {
-  VectorValues cloned(SameStructure(x));
+  VectorValues result;
-  cloned.setZero();
+  result.values_.resize(x.size());
-  return cloned;
+  for(size_t j=0; j<x.size(); ++j)
+    result.values_[j] = Vector::Zero(x.values_[j].size());
+  return result;
 }
 
 /* ************************************************************************* */
@@ -59,69 +49,44 @@ void VectorValues::insert(Index j, const Vector& value) {
   if(exists(j))
     throw invalid_argument("VectorValues: requested variable index to insert already exists.");
 
-  // Get vector of dimensions
-  FastVector<size_t> dimensions(size());
-  for(size_t k=0; k<maps_.size(); ++k)
-    dimensions[k] = maps_[k].rows();
-
   // If this adds variables at the end, insert zero-length entries up to j
   if(j >= size())
-    dimensions.insert(dimensions.end(), j+1-size(), 0);
+    values_.resize(j+1);
 
-  // Set correct dimension for j
+  // Assign value
-  dimensions[j] = value.rows();
+  values_[j] = value;
-
-  // Make a copy to make assignment easier
-  VectorValues original(*this);
-
-  // Resize to accomodate new variable
-  resize(dimensions);
-
-  // Copy original variables
-  for(Index k = 0; k < original.size(); ++k)
-    if(k != j && exists(k))
-      operator[](k) = original[k];
-
-  // Copy new variable
-  operator[](j) = value;
 }
 
 /* ************************************************************************* */
 void VectorValues::print(const std::string& str, const IndexFormatter& formatter) const {
   std::cout << str << ": " << size() << " elements\n";
   for (Index var = 0; var < size(); ++var)
-    std::cout << "  " << formatter(var) << ": \n" << operator[](var) << "\n";
+    std::cout << "  " << formatter(var) << ": \n" << (*this)[var] << "\n";
   std::cout.flush();
 }
 
 /* ************************************************************************* */
 bool VectorValues::equals(const VectorValues& x, double tol) const {
-  return hasSameStructure(x) && equal_with_abs_tol(values_, x.values_, tol);
+  if(this->size() != x.size())
+    return false;
+  for(Index j=0; j < size(); ++j)
+    if(!equal_with_abs_tol(values_[j], x.values_[j], tol))
+      return false;
+  return true;
 }
 
 /* ************************************************************************* */
 void VectorValues::resize(Index nVars, size_t varDim) {
-  maps_.clear();
+  values_.resize(nVars);
-  maps_.reserve(nVars);
+  for(Index j = 0; j < nVars; ++j)
-  values_.resize(nVars * varDim);
+    values_[j] = Vector(varDim);
-  int varStart = 0;
-  for (Index j = 0; j < nVars; ++j) {
-    maps_.push_back(values_.segment(varStart, varDim));
-    varStart += varDim;
-  }
 }
 
 /* ************************************************************************* */
 void VectorValues::resizeLike(const VectorValues& other) {
-  values_.resize(other.dim());
+  values_.resize(other.size());
-  // Create SubVectors referencing our values_ vector
+  for(Index j = 0; j < other.size(); ++j)
-  maps_.clear();
+    values_[j].resize(other.values_[j].size());
-  maps_.reserve(other.size());
-  int varStart = 0;
-  BOOST_FOREACH(const SubVector& value, other) {
-    maps_.push_back(values_.segment(varStart, value.rows()));
-    varStart += value.rows();
-  }
 }
 
 /* ************************************************************************* */
@@ -140,91 +105,133 @@ VectorValues VectorValues::Zero(Index nVars, size_t varDim) {
 
 /* ************************************************************************* */
 void VectorValues::setZero() {
-  values_.setZero();
+  BOOST_FOREACH(Vector& v, *this) {
+    v.setZero();
+  }
+}
+
+/* ************************************************************************* */
+const Vector VectorValues::asVector() const {
+  return internal::extractVectorValuesSlices(*this,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true);
+}
+
+/* ************************************************************************* */
+const Vector VectorValues::vector(const std::vector<Index>& indices) const {
+  return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end());
 }
 
 /* ************************************************************************* */
 bool VectorValues::hasSameStructure(const VectorValues& other) const {
   if(this->size() != other.size())
     return false;
-  for(size_t j=0; j<size(); ++j)
+  for(size_t j = 0; j < size(); ++j)
     // Directly accessing maps instead of using VV::dim in case some values are empty
-    if(this->maps_[j].rows() != other.maps_[j].rows())
+    if(this->values_[j].rows() != other.values_[j].rows())
       return false;
   return true;
 }
 
 /* ************************************************************************* */
-VectorValues VectorValues::operator+(const VectorValues& c) const {
+void VectorValues::permuteInPlace(const Permutation& permutation) {
-  assert(this->hasSameStructure(c));
+  // Allocate new values
-  VectorValues result(SameStructure(c));
+  Values newValues(this->size());
-  result.values_ = this->values_ + c.values_;
+
-  return result;
+  // Swap values from this VectorValues to the permuted VectorValues
+  for(size_t i = 0; i < this->size(); ++i)
+    newValues[i].swap(this->at(permutation[i]));
+
+  // Swap the values into this VectorValues
+  values_.swap(newValues);
 }
 
 /* ************************************************************************* */
-VectorValues VectorValues::operator-(const VectorValues& c) const {
+void VectorValues::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
-  assert(this->hasSameStructure(c));
+  if(selector.size() != permutation.size())
-  VectorValues result(SameStructure(c));
+    throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
-  result.values_ = this->values_ - c.values_;
+  // Create new index the size of the permuted entries
-  return result;
+  Values reorderedEntries(selector.size());
-}
+  // Permute the affected entries into the new index
-
+  for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
-/* ************************************************************************* */
+    reorderedEntries[dstSlot].swap(values_[selector[permutation[dstSlot]]]);
-void VectorValues::operator+=(const VectorValues& c) {
+  // Put the affected entries back in the new order
-  assert(this->hasSameStructure(c));
+  for(size_t slot = 0; slot < selector.size(); ++slot)
-  this->values_ += c.values_;
+    values_[selector[slot]].swap(reorderedEntries[slot]);
-}
-
-/* ************************************************************************* */
-VectorValues VectorValues::permute(const Permutation& permutation) const {
-  // Create result and allocate space
-  VectorValues lhs;
-  lhs.values_.resize(this->dim());
-  lhs.maps_.reserve(this->size());
-
-  // Copy values from this VectorValues to the permuted VectorValues
-  size_t lhsPos = 0;
-  for(size_t i = 0; i < this->size(); ++i) {
-    // Map the next LHS subvector to the next slice of the LHS vector
-    lhs.maps_.push_back(SubVector(lhs.values_, lhsPos, this->at(permutation[i]).size()));
-    // Copy the data from the RHS subvector to the LHS subvector
-    lhs.maps_[i] = this->at(permutation[i]);
-    // Increment lhs position
-    lhsPos += lhs.maps_[i].size();
-  }
-
-  return lhs;
 }
 
 /* ************************************************************************* */
 void VectorValues::swap(VectorValues& other) {
   this->values_.swap(other.values_);
-  this->maps_.swap(other.maps_);
 }
 
 /* ************************************************************************* */
-Vector VectorValues::vector(const std::vector<Index>& indices) const {
+double VectorValues::dot(const VectorValues& v) const {
-  if (indices.empty())
+  double result = 0.0;
-    return Vector();
+  if(this->size() != v.size())
-
+    throw invalid_argument("VectorValues::dot called with different vector sizes");
-  // find dimensions
+  for(Index j = 0; j < this->size(); ++j)
-  size_t d = 0;
+    // Directly accessing maps instead of using VV::dim in case some values are empty
-  BOOST_FOREACH(const Index& idx, indices)
+    if(this->values_[j].size() == v.values_[j].size())
-    d += dim(idx);
+      result += this->values_[j].dot(v.values_[j]);
-
+    else
-  // copy out values
+      throw invalid_argument("VectorValues::dot called with different vector sizes");
-  Vector result(d);
-  size_t curHead = 0;
-  BOOST_FOREACH(const Index& j, indices) {
-    const SubVector& vj = at(j);
-    size_t dj = (size_t) vj.rows();
-    result.segment(curHead, dj) = vj;
-    curHead += dj;
-  }
   return result;
 }
 
+/* ************************************************************************* */
+double VectorValues::norm() const {
+  return std::sqrt(this->squaredNorm());
+}
+
+/* ************************************************************************* */
+double VectorValues::squaredNorm() const {
+  double sumSquares = 0.0;
+  for(Index j = 0; j < this->size(); ++j)
+    // Directly accessing maps instead of using VV::dim in case some values are empty
+    sumSquares += this->values_[j].squaredNorm();
+  return sumSquares;
+}
+
+/* ************************************************************************* */
+VectorValues VectorValues::operator+(const VectorValues& c) const {
+  VectorValues result = SameStructure(*this);
+  if(this->size() != c.size())
+    throw invalid_argument("VectorValues::operator+ called with different vector sizes");
+  for(Index j = 0; j < this->size(); ++j)
+    // Directly accessing maps instead of using VV::dim in case some values are empty
+    if(this->values_[j].size() == c.values_[j].size())
+      result.values_[j] = this->values_[j] + c.values_[j];
+    else
+      throw invalid_argument("VectorValues::operator- called with different vector sizes");
+  return result;
+}
+
+/* ************************************************************************* */
+VectorValues VectorValues::operator-(const VectorValues& c) const {
+  VectorValues result = SameStructure(*this);
+  if(this->size() != c.size())
+    throw invalid_argument("VectorValues::operator- called with different vector sizes");
+  for(Index j = 0; j < this->size(); ++j)
+    // Directly accessing maps instead of using VV::dim in case some values are empty
+    if(this->values_[j].size() == c.values_[j].size())
+      result.values_[j] = this->values_[j] - c.values_[j];
+    else
+      throw invalid_argument("VectorValues::operator- called with different vector sizes");
+  return result;
+}
+
+/* ************************************************************************* */
+void VectorValues::operator+=(const VectorValues& c) {
+  if(this->size() != c.size())
+    throw invalid_argument("VectorValues::operator+= called with different vector sizes");
+  for(Index j = 0; j < this->size(); ++j)
+    // Directly accessing maps instead of using VV::dim in case some values are empty
+    if(this->values_[j].size() == c.values_[j].size())
+      this->values_[j] += c.values_[j];
+    else
+      throw invalid_argument("VectorValues::operator+= called with different vector sizes");
+}
+
 /* ************************************************************************* */
 
 } // \namespace gtsam

gtsam/linear/VectorValues.h

@@ -93,15 +93,14 @@ namespace gtsam {
    */
   class VectorValues {
   protected:
-    Vector values_; ///< The underlying vector storing the values
+    typedef std::vector<Vector> Values; ///< Typedef for the collection of Vectors making up a VectorValues
-    typedef std::vector<SubVector> ValueMaps; ///< Collection of SubVector s
+    Values values_; ///< Collection of Vectors making up this VectorValues
-    ValueMaps maps_; ///< SubVector s referencing each vector variable in values_
 
   public:
-    typedef ValueMaps::iterator iterator; ///< Iterator over vector values
+    typedef Values::iterator iterator; ///< Iterator over vector values
-    typedef ValueMaps::const_iterator const_iterator; ///< Const iterator over vector values
+    typedef Values::const_iterator const_iterator; ///< Const iterator over vector values
-    typedef ValueMaps::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values
+    typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values
-    typedef ValueMaps::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values
+    typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values
     typedef boost::shared_ptr<VectorValues> shared_ptr; ///< shared_ptr to this class
 
     /// @name Standard Constructors
@@ -112,11 +111,8 @@ namespace gtsam {
      */
     VectorValues() {}
 
-    /** Copy constructor */
-    VectorValues(const VectorValues &other);
-
     /** Named constructor to create a VectorValues of the same structure of the
-     * specifed one, but filled with zeros.
+     * specified one, but filled with zeros.
      * @return
      */
     static VectorValues Zero(const VectorValues& model);
@@ -127,55 +123,49 @@ namespace gtsam {
 
     /** Number of variables stored, always 1 more than the highest variable index,
      * even if some variables with lower indices are not present. */
-    Index size() const { return maps_.size(); }
+    Index size() const { return values_.size(); }
 
     /** Return the dimension of variable \c j. */
     size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); }
 
-    /** Return the summed dimensionality of all variables. */
-    size_t dim() const { return values_.rows(); }
-
     /** Return the dimension of each vector in this container */
     std::vector<size_t> dims() const;
 
     /** Check whether a variable with index \c j exists. */
-    bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; }
+    bool exists(Index j) const { return j < size() && values_[j].rows() > 0; }
 
     /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Index). */
-    SubVector& at(Index j) { checkExists(j); return maps_[j]; }
+    Vector& at(Index j) { checkExists(j); return values_[j]; }
 
     /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Index). */
-    const SubVector& at(Index j) const { checkExists(j); return maps_[j]; }
+    const Vector& at(Index j) const { checkExists(j); return values_[j]; }
 
     /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to at(Index). */
-    SubVector& operator[](Index j) { return at(j); }
+    Vector& operator[](Index j) { return at(j); }
 
     /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Index). */
-    const SubVector& operator[](Index j) const { return at(j); }
+    const Vector& operator[](Index j) const { return at(j); }
 
     /** Insert a vector \c value with index \c j.
-     * Causes reallocation. Can be used to insert values in any order, but
+     * Causes reallocation, but can insert values in any order.
-     * throws an invalid_argument exception if the index \c j is already used.
+     * Throws an invalid_argument exception if the index \c j is already used.
      * @param value The vector to be inserted.
      * @param j The index with which the value will be associated.
      */
     void insert(Index j, const Vector& value);
 
-    /** Assignment */
+    iterator begin()                      { return values_.begin(); }  ///< Iterator over variables
-    VectorValues& operator=(const VectorValues& rhs);
+    const_iterator begin() const          { return values_.begin(); }  ///< Iterator over variables
-
+    iterator end()                         { return values_.end(); }    ///< Iterator over variables
-    iterator begin()                      { chk(); return maps_.begin(); }  ///< Iterator over variables
+    const_iterator end() const            { return values_.end(); }    ///< Iterator over variables
-    const_iterator begin() const          { chk(); return maps_.begin(); }  ///< Iterator over variables
+    reverse_iterator rbegin()              { return values_.rbegin(); } ///< Reverse iterator over variables
-    iterator end()                         { chk(); return maps_.end(); }    ///< Iterator over variables
+    const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables
-    const_iterator end() const            { chk(); return maps_.end(); }    ///< Iterator over variables
+    reverse_iterator rend()                { return values_.rend(); }   ///< Reverse iterator over variables
-    reverse_iterator rbegin()              { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables
+    const_reverse_iterator rend() const   { return values_.rend(); }   ///< Reverse iterator over variables
-    const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables
-    reverse_iterator rend()                { chk(); return maps_.rend(); }   ///< Reverse iterator over variables
-    const_reverse_iterator rend() const   { chk(); return maps_.rend(); }   ///< Reverse iterator over variables
 
     /** print required by Testable for unit testing */
     void print(const std::string& str = "VectorValues: ",
-        const IndexFormatter& formatter =DefaultIndexFormatter) const;
+        const IndexFormatter& formatter = DefaultIndexFormatter) const;
 
     /** equals required by Testable for unit testing */
     bool equals(const VectorValues& x, double tol = 1e-9) const;
@@ -187,10 +177,10 @@ namespace gtsam {
 
     /** Construct from a container of variable dimensions (in variable order), without initializing any values. */
     template<class CONTAINER>
-    explicit VectorValues(const CONTAINER& dimensions) { append(dimensions); }
+    explicit VectorValues(const CONTAINER& dimensions) { this->append(dimensions); }
 
     /** Construct to hold nVars vectors of varDim dimension each. */
-    VectorValues(Index nVars, size_t varDim) { resize(nVars, varDim); }
+    VectorValues(Index nVars, size_t varDim) { this->resize(nVars, varDim); }
 
     /** Named constructor to create a VectorValues that matches the structure of
      * the specified VectorValues, but do not initialize the new values. */
@@ -222,17 +212,16 @@ namespace gtsam {
     void resizeLike(const VectorValues& other);
 
     /** Resize the VectorValues to hold \c nVars variables, each of dimension
-     * \c varDim, not preserving any data.  After calling this function, all
+     * \c varDim.  Any individual vectors that do not change size will keep
-     * variables will be uninitialized.
+     * their values, but any new or resized vectors will be uninitialized.
      * @param nVars The number of variables to create
      * @param varDim The dimension of each variable
      */
     void resize(Index nVars, size_t varDim);
 
     /** Resize the VectorValues to contain variables of the dimensions stored
-     * in \c dimensions, not preserving any data.  The new variables are
+     * in \c dimensions.  Any individual vectors that do not change size will keep
-     * uninitialized, but this function is used to pre-allocate space for
+     * their values, but any new or resized vectors will be uninitialized.
-     * performance.  After calling this function all variables will be uninitialized.
      * @param dimensions A container of the dimension of each variable to create.
      */
     template<class CONTAINER>
@@ -251,14 +240,11 @@ namespace gtsam {
     /** Set all entries to zero, does not modify the size. */
     void setZero();
 
-    /** Reference the entire solution vector (const version). */
+    /** Retrieve the entire solution as a single vector */
-    const Vector& vector() const { chk(); return values_; }
+    const Vector asVector() const;
-
-    /** Reference the entire solution vector. */
-    Vector& vector() { chk(); return values_; }
 
     /** Access a vector that is a subset of relevant indices */
-    Vector vector(const std::vector<Index>& indices) const;
+    const Vector vector(const std::vector<Index>& indices) const;
 
     /** Check whether this VectorValues has the same structure, meaning has the
      * same number of variables and that all variables are of the same dimension,
@@ -268,16 +254,34 @@ namespace gtsam {
      */
     bool hasSameStructure(const VectorValues& other) const;
 
+    /**  
+     * Permute the variables in the VariableIndex according to the given partial permutation
+     */
+    void permuteInPlace(const Permutation& selector, const Permutation& permutation);
+
+    /**
+     * Permute the entries of this VectorValues in place
+     */
+    void permuteInPlace(const Permutation& permutation);
+
+    /**
+     * Swap the data in this VectorValues with another.
+     */
+    void swap(VectorValues& other);
+
+    /// @}
+    /// @name Linear algebra operations
+    /// @{
+
     /** Dot product with another VectorValues, interpreting both as vectors of
      * their concatenated values. */
-    double dot(const VectorValues& V) const {
+    double dot(const VectorValues& v) const;
-      return gtsam::dot(this->values_, V.values_);
-    }
 
     /** Vector L2 norm */
-    inline double norm() const {
+    double norm() const;
-      return this->vector().norm();
+
-    }
+    /** Squared vector L2 norm */
+    double squaredNorm() const;
 
     /**
      * + operator does element-wise addition.  Both VectorValues must have the
@@ -297,26 +301,11 @@ namespace gtsam {
      */
     void operator+=(const VectorValues& c);
 
-    /**
-     * Permute the entries of this VectorValues, returns a new VectorValues as
-     * the result.
-     */
-    VectorValues permute(const Permutation& permutation) const;
-
-    /**
-     * Swap the data in this VectorValues with another.
-     */
-    void swap(VectorValues& other);
-
     /// @}
 
   private:
-    // Verifies that the underlying Vector is consistent with the collection of SubVectors
-    void chk() const;
-
     // Throw an exception if j does not exist
-    inline void checkExists(Index j) const {
+    void checkExists(Index j) const {
-      chk();
       if(!exists(j)) {
         const std::string msg =
             (boost::format("VectorValues: requested variable index j=%1% is not in this VectorValues.") % j).str();
@@ -329,73 +318,63 @@ namespace gtsam {
     /**
      * scale a vector by a scalar
      */
-    friend VectorValues operator*(const double a, const VectorValues &V) {
+    friend VectorValues operator*(const double a, const VectorValues &v) {
-      VectorValues result(VectorValues::SameStructure(V));
+      VectorValues result = VectorValues::SameStructure(v);
-      result.values_ = a * V.values_;
+      for(Index j = 0; j < v.size(); ++j)
+        result.values_[j] = a * v.values_[j];
       return result;
     }
 
-    /// TODO: linear algebra interface seems to have been added for SPCG.
-    friend size_t dim(const VectorValues& V) {
-      return V.dim();
-    }
-    /// TODO: linear algebra interface seems to have been added for SPCG.
-    friend double dot(const VectorValues& V1, const VectorValues& V2) {
-      return gtsam::dot(V1.values_, V2.values_);
-    }
     /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void scal(double alpha, VectorValues& x) {
-      gtsam::scal(alpha, x.values_);
+      for(Index j = 0; j < x.size(); ++j)
+        x.values_[j] *= alpha;
     }
     /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void axpy(double alpha, const VectorValues& x, VectorValues& y) {
-      gtsam::axpy(alpha, x.values_, y.values_);
+      if(x.size() != y.size())
+        throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
+      for(Index j = 0; j < x.size(); ++j)
+        if(x.values_[j].size() == y.values_[j].size())
+          y.values_[j] += alpha * x.values_[j];
+        else
+          throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
     }
     /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void sqrt(VectorValues &x) {
-      Vector y = gtsam::esqrt(x.values_);
+      for(Index j = 0; j < x.size(); ++j)
-      x.values_ = y;
+        x.values_[j] = x.values_[j].cwiseSqrt();
     }
 
     /// TODO: linear algebra interface seems to have been added for SPCG.
-    friend void ediv(const VectorValues& numerator,
+    friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) {
-        const VectorValues& denominator, VectorValues &result) {
+      if(numerator.size() != denominator.size() || numerator.size() != result.size())
-      assert(
+        throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
-          numerator.dim() == denominator.dim() && denominator.dim() == result.dim());
+      for(Index j = 0; j < numerator.size(); ++j)
-      const size_t sz = result.dim();
+        if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size())
-      for (size_t i = 0; i < sz; ++i)
+          result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]);
-        result.values_[i] = numerator.values_[i] / denominator.values_[i];
+        else
+          throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
     }
 
     /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void edivInPlace(VectorValues& x, const VectorValues& y) {
-      assert(x.dim() == y.dim());
+      if(x.size() != y.size())
-      const size_t sz = x.dim();
+        throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
-      for (size_t i = 0; i < sz; ++i)
+      for(Index j = 0; j < x.size(); ++j)
-        x.values_[i] /= y.values_[i];
+        if(x.values_[j].size() == y.values_[j].size())
+          x.values_[j].array() /= y.values_[j].array();
+        else
+          throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
     }
 
   private:
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
-    void save(ARCHIVE & ar, const unsigned int version) const {
+    void serialize(ARCHIVE & ar, const unsigned int version) {
-      // The maps_ stores pointers, so we serialize dimensions instead
-      std::vector<size_t> dimensions(size());
-      for(size_t j=0; j<maps_.size(); ++j)
-        dimensions[j] = maps_[j].rows();
-      ar & BOOST_SERIALIZATION_NVP(dimensions);
       ar & BOOST_SERIALIZATION_NVP(values_);
     }
-    template<class ARCHIVE>
-    void load(ARCHIVE & ar, const unsigned int version) {
-      std::vector<size_t> dimensions;
-      ar & BOOST_SERIALIZATION_NVP(dimensions); // Load dimensions
-      resize(dimensions); // Allocate space for everything
-      ar & BOOST_SERIALIZATION_NVP(values_); // Load values
-      chk();
-    }
-    BOOST_SERIALIZATION_SPLIT_MEMBER()
   }; // VectorValues definition
 
   // Implementations of template and inline functions
@@ -403,86 +382,75 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class CONTAINER>
   void VectorValues::resize(const CONTAINER& dimensions) {
-    maps_.clear();
+    values_.clear();
-    values_.resize(0);
     append(dimensions);
   }
 
   /* ************************************************************************* */
   template<class CONTAINER>
   void VectorValues::append(const CONTAINER& dimensions) {
-    chk();
+    size_t i = size();
-    int newDim = std::accumulate(dimensions.begin(), dimensions.end(), 0); // Sum of dimensions
+    values_.resize(size() + dimensions.size());
-    values_.conservativeResize(dim() + newDim);
-    // Relocate existing maps
-    int varStart = 0;
-    for(size_t j = 0; j < maps_.size(); ++j) {
-      new (&maps_[j]) SubVector(values_.segment(varStart, maps_[j].rows()));
-      varStart += maps_[j].rows();
-    }
-    maps_.reserve(maps_.size() + dimensions.size());
     BOOST_FOREACH(size_t dim, dimensions) {
-      maps_.push_back(values_.segment(varStart, dim));
+      values_[i] = Vector(dim);
-      varStart += (int)dim; // varStart is continued from first for loop
+      ++ i;
     }
   }
 
   /* ************************************************************************* */
   template<class CONTAINER>
   VectorValues VectorValues::Zero(const CONTAINER& dimensions) {
-    VectorValues ret(dimensions);
+    VectorValues ret;
-    ret.setZero();
+    ret.values_.resize(dimensions.size());
-    return ret;
+    size_t i = 0;
-  }
+    BOOST_FOREACH(size_t dim, dimensions) {
-
+      ret.values_[i] = Vector::Zero(dim);
-  /* ************************************************************************* */
+      ++ i;
-  inline void VectorValues::chk() const {
-#ifndef NDEBUG
-    // Check that the first SubVector points to the beginning of the Vector
-    if(maps_.size() > 0) {
-      assert(values_.data() == maps_[0].data());
-      // Check that the end of the last SubVector points to the end of the Vector
-      assert(values_.rows() == maps_.back().data() + maps_.back().rows() - maps_.front().data());
     }
-#endif
+    return ret;
   }
 
   namespace internal {
-  /* ************************************************************************* */
+    /* ************************************************************************* */
-  // Helper function, extracts vectors with variable indices
+    // Helper function, extracts vectors with variable indices
-  // in the first and last iterators, and concatenates them in that order into the
+    // in the first and last iterators, and concatenates them in that order into the
-  // output.
+    // output.
-  template<class VALUES, typename ITERATOR>
+    template<typename ITERATOR>
-  Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) {
+    const Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) {
-    // Find total dimensionality
+      // Find total dimensionality
-    int dim = 0;
+      int dim = 0;
-    for(ITERATOR j = first; j != last; ++j)
+      for(ITERATOR j = first; j != last; ++j)
-      dim += values[*j].rows();
+        // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent)
+        if(!allowNonexistant || values.exists(*j))
+          dim += values.dim(*j);
 
-    // Copy vectors
+      // Copy vectors
-    Vector ret(dim);
+      Vector ret(dim);
-    int varStart = 0;
+      int varStart = 0;
-    for(ITERATOR j = first; j != last; ++j) {
+      for(ITERATOR j = first; j != last; ++j) {
-      ret.segment(varStart, values[*j].rows()) = values[*j];
+        // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent)
-      varStart += values[*j].rows();
+        if(!allowNonexistant || values.exists(*j)) {
+          ret.segment(varStart, values.dim(*j)) = values[*j];
+          varStart += values.dim(*j);
+        }
+      }
+      return ret;
     }
-    return ret;
-  }
 
-  /* ************************************************************************* */
+    /* ************************************************************************* */
-  // Helper function, writes to the variables in values
+    // Helper function, writes to the variables in values
-  // with indices iterated over by first and last, interpreting vector as the
+    // with indices iterated over by first and last, interpreting vector as the
-  // concatenated vectors to write.
+    // concatenated vectors to write.
-  template<class VECTOR, class VALUES, typename ITERATOR>
+    template<class VECTOR, typename ITERATOR>
-  void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) {
+    void writeVectorValuesSlices(const VECTOR& vector, VectorValues& values, ITERATOR first, ITERATOR last) {
-    // Copy vectors
+      // Copy vectors
-    int varStart = 0;
+      int varStart = 0;
-    for(ITERATOR j = first; j != last; ++j) {
+      for(ITERATOR j = first; j != last; ++j) {
-      values[*j] = vector.segment(varStart, values[*j].rows());
+        values[*j] = vector.segment(varStart, values[*j].rows());
-      varStart += values[*j].rows();
+        varStart += values[*j].rows();
+      }
+      assert(varStart == vector.rows());
     }
-    assert(varStart == vector.rows());
-  }
   }
 
 } // \namespace gtsam

gtsam/linear/tests/testVectorValues.cpp

@@ -39,7 +39,6 @@ TEST(VectorValues, insert) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -59,7 +58,7 @@ TEST(VectorValues, insert) {
   EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
   EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
   EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
-  EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector()));
+  EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector()));
 
   // Check exceptions
   CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
@@ -80,7 +79,6 @@ TEST(VectorValues, dimsConstructor) {
 
   // Check dimensions
   LONGS_EQUAL(3, actual.size());
-  LONGS_EQUAL(5, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -89,7 +87,7 @@ TEST(VectorValues, dimsConstructor) {
   EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
   EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
   EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
-  EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.vector()));
+  EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
 }
 
 /* ************************************************************************* */
@@ -106,7 +104,6 @@ TEST(VectorValues, copyConstructor) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -126,7 +123,7 @@ TEST(VectorValues, copyConstructor) {
   EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
   EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
   EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
-  EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector()));
+  EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector()));
 
   // Check exceptions
   CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
@@ -149,7 +146,6 @@ TEST(VectorValues, assignment) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -169,7 +165,7 @@ TEST(VectorValues, assignment) {
   EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
   EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
   EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
-  EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector()));
+  EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector()));
 
   // Check exceptions
   CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
@@ -188,7 +184,6 @@ TEST(VectorValues, SameStructure) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -220,7 +215,6 @@ TEST(VectorValues, Zero_fromModel) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -256,7 +250,6 @@ TEST(VectorValues, Zero_fromDims) {
 
   // Check dimensions
   LONGS_EQUAL(3, actual.size());
-  LONGS_EQUAL(5, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -273,7 +266,6 @@ TEST(VectorValues, Zero_fromUniform) {
 
   // Check dimensions
   LONGS_EQUAL(3, actual.size());
-  LONGS_EQUAL(6, actual.dim());
   LONGS_EQUAL(2, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -298,7 +290,6 @@ TEST(VectorValues, resizeLike) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -328,7 +319,6 @@ TEST(VectorValues, resize_fromUniform) {
 
   // Check dimensions
   LONGS_EQUAL(3, actual.size());
-  LONGS_EQUAL(6, actual.dim());
   LONGS_EQUAL(2, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -337,7 +327,7 @@ TEST(VectorValues, resize_fromUniform) {
   EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actual[0]));
   EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
   EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
-  EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.vector()));
+  EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
 }
 
 /* ************************************************************************* */
@@ -355,7 +345,6 @@ TEST(VectorValues, resize_fromDims) {
 
   // Check dimensions
   LONGS_EQUAL(3, actual.size());
-  LONGS_EQUAL(5, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -364,7 +353,7 @@ TEST(VectorValues, resize_fromDims) {
   EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
   EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
   EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
-  EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.vector()));
+  EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
 }
 
 /* ************************************************************************* */
@@ -383,7 +372,6 @@ TEST(VectorValues, append) {
 
   // Check dimensions
   LONGS_EQUAL(5, actual.size());
-  LONGS_EQUAL(13, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -443,7 +431,8 @@ TEST(VectorValues, permute) {
   permutation[2] = 3;
   permutation[3] = 1;
 
-  VectorValues actual = original.permute(permutation);
+  VectorValues actual = original;
+  actual.permuteInPlace(permutation);
 
   EXPECT(assert_equal(expected, actual));
 }

gtsam/nonlinear/DoglegOptimizerImpl.cpp

@@ -28,13 +28,12 @@ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint(
   // Get magnitude of each update and find out which segment Delta falls in
   assert(Delta >= 0.0);
   double DeltaSq = Delta*Delta;
-  double x_u_norm_sq = dx_u.vector().squaredNorm();
+  double x_u_norm_sq = dx_u.squaredNorm();
-  double x_n_norm_sq = dx_n.vector().squaredNorm();
+  double x_n_norm_sq = dx_n.squaredNorm();
   if(verbose) cout << "Steepest descent magnitude " << std::sqrt(x_u_norm_sq) << ", Newton's method magnitude " << std::sqrt(x_n_norm_sq) << endl;
   if(DeltaSq < x_u_norm_sq) {
     // Trust region is smaller than steepest descent update
-    VectorValues x_d = VectorValues::SameStructure(dx_u);
+    VectorValues x_d = std::sqrt(DeltaSq / x_u_norm_sq) * dx_u;
-    x_d.vector() = dx_u.vector() * std::sqrt(DeltaSq / x_u_norm_sq);
     if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl;
     return x_d;
   } else if(DeltaSq < x_n_norm_sq) {
@@ -79,8 +78,7 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double Delta, const VectorValues&
 
   // Compute blended point
   if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl;
-  VectorValues blend = VectorValues::SameStructure(x_u);
+  VectorValues blend = (1. - tau) * x_u;  axpy(tau, x_n, blend);
-  blend.vector() = (1. - tau) * x_u.vector() + tau * x_n.vector();
   return blend;
 }
 

gtsam/nonlinear/DoglegOptimizerImpl.h

@@ -177,7 +177,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
     result.dx_d = ComputeDoglegPoint(Delta, dx_u, dx_n, verbose);
     gttoc(Dog_leg_point);
 
-    if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << std::endl;
+    if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.norm() << std::endl;
 
     gttic(retract);
     // Compute expmapped solution
@@ -208,7 +208,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
 
     if(rho >= 0.75) {
       // M agrees very well with f, so try to increase lambda
-      const double dx_d_norm = result.dx_d.vector().norm();
+      const double dx_d_norm = result.dx_d.norm();
       const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta
 
       if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY)

gtsam/nonlinear/ISAM2-impl.cpp

@@ -39,15 +39,15 @@ void ISAM2::Impl::AddVariables(
   // Add the new keys onto the ordering, add zeros to the delta for the new variables
   std::vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
   if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
-  const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
-  const size_t originalDim = delta.dim();
   const size_t originalnVars = delta.size();
   delta.append(dims);
-  delta.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
   deltaNewton.append(dims);
-  deltaNewton.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
   RgProd.append(dims);
-  RgProd.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
+  for(Index j = originalnVars; j < delta.size(); ++j) {
+    delta[j].setZero();
+    deltaNewton[j].setZero();
+    RgProd[j].setZero();
+  }
   {
     Index nextVar = originalnVars;
     BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
@@ -55,10 +55,9 @@ void ISAM2::Impl::AddVariables(
       if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl;
       ++ nextVar;
     }
-    assert(ordering.nVars() == delta.size());
     assert(ordering.size() == delta.size());
   }
-  replacedKeys.resize(ordering.nVars(), false);
+  replacedKeys.resize(ordering.size(), false);
 }
 
 /* ************************************************************************* */
@@ -115,9 +114,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Cli
      }
 
      // Reorder and remove from ordering and solution
-     ordering.permuteWithInverse(unusedToEndInverse);
+     ordering.permuteInPlace(unusedToEnd);
      BOOST_REVERSE_FOREACH(Key key, unusedKeys) {
-       ordering.pop_back(key);
+       Ordering::value_type removed = ordering.pop_back();
+       assert(removed.first == key);
        theta.erase(key);
      }
 
@@ -188,14 +188,14 @@ void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double thres
 }
 
 /* ************************************************************************* */
-void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const VectorValues& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) {
+void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const VectorValues& delta, const Ordering& ordering, const ISAM2Clique::shared_ptr& clique) {
 
   // Check the current clique for relinearization
   bool relinearize = false;
   BOOST_FOREACH(Index var, clique->conditional()->keys()) {
 
     // Lookup the key associated with this index
-    Key key = decoder.at(var);
+    Key key = ordering.key(var);
 
     // Find the threshold for this variable type
     const Vector& threshold = thresholds.find(Symbol(key).chr())->second;
@@ -214,7 +214,7 @@ void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<c
   // If this node was relinearized, also check its children
   if(relinearize) {
     BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
-      CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, decoder, child);
+      CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, ordering, child);
     }
   }
 }
@@ -229,8 +229,7 @@ FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::share
     if(relinearizeThreshold.type() == typeid(double)) {
       CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
     } else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) {
-      Ordering::InvertedMap decoder = ordering.invert();
+      CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, ordering, root);
-      CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, decoder, root);
     }
   }
 
@@ -267,8 +266,8 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const
   invalidateIfDebug = boost::none;
 #endif
 
-  assert(values.size() == ordering.nVars());
+  assert(values.size() == ordering.size());
-  assert(delta.size() == ordering.nVars());
+  assert(delta.size() == ordering.size());
   Values::iterator key_value;
   Ordering::const_iterator key_index;
   for(key_value = values.begin(), key_index = ordering.begin();
@@ -353,11 +352,9 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
   Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse());
   if(debug) affectedColamd->print("affectedColamd: ");
   if(debug) affectedColamdInverse->print("affectedColamdInverse: ");
-  result.fullReordering =
+  result.reorderingSelector = affectedKeysSelector;
-      *Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamd);
+  result.reorderingPermutation = *affectedColamd;
-  result.fullReorderingInverse =
+  result.reorderingInverse = internal::Reduction::CreateFromPartialPermutation(affectedKeysSelector, *affectedColamdInverse);
-      *Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamdInverse);
-  if(debug) result.fullReordering.print("partialReordering: ");
   gttoc(ccolamd_permutations);
 
   gttic(permute_affected_variable_index);

gtsam/nonlinear/ISAM2-impl.h

@@ -26,8 +26,9 @@ struct ISAM2::Impl {
 
   struct PartialSolveResult {
     ISAM2::sharedClique bayesTree;
-    Permutation fullReordering;
+    Permutation reorderingSelector;
-    Permutation fullReorderingInverse;
+    Permutation reorderingPermutation;
+    internal::Reduction reorderingInverse;
   };
 
   struct ReorderingMode {

gtsam/nonlinear/ISAM2-inl.h

@@ -29,7 +29,7 @@ namespace gtsam {
 template<class VALUE>
 VALUE ISAM2::calculateEstimate(Key key) const {
   const Index index = getOrdering()[key];
-  const SubVector delta = getDelta()[index];
+  const Vector& delta = getDelta()[index];
   return theta_.at<VALUE>(key).retract(delta);
 }
 
@@ -158,7 +158,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
     for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
       if(!valuesChanged) {
         const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]);
-        const SubVector& newValue(delta[*it]);
+        const Vector& newValue(delta[*it]);
         if((oldValue - newValue).lpNorm<Eigen::Infinity>() >= threshold) {
           valuesChanged = true;
           break;

gtsam/nonlinear/ISAM2.cpp

@@ -348,12 +348,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
     variableIndex_.permuteInPlace(*colamd);
     gttoc(permute_global_variable_index);
     gttic(permute_delta);
-    delta_ = delta_.permute(*colamd);
+    delta_.permuteInPlace(*colamd);
-    deltaNewton_ = deltaNewton_.permute(*colamd);
+    deltaNewton_.permuteInPlace(*colamd);
-    RgProd_ = RgProd_.permute(*colamd);
+    RgProd_.permuteInPlace(*colamd);
     gttoc(permute_delta);
     gttic(permute_ordering);
-    ordering_.permuteWithInverse(*colamdInverse);
+    ordering_.permuteInPlace(*colamd);
     gttoc(permute_ordering);
     gttoc(reorder);
 
@@ -412,7 +412,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
     // Reeliminated keys for detailed results
     if(params_.enableDetailedResults) {
       BOOST_FOREACH(Index index, affectedAndNewKeys) {
-        result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true;
+        result.detail->variableStatus[ordering_.key(index)].isReeliminated = true;
       }
     }
 
@@ -450,7 +450,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
 
     gttic(PartialSolve);
     Impl::ReorderingMode reorderingMode;
-    reorderingMode.nFullSystemVars = ordering_.nVars();
+    reorderingMode.nFullSystemVars = ordering_.size();
     reorderingMode.algorithm = Impl::ReorderingMode::COLAMD;
     reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST;
     if(constrainKeys) {
@@ -481,22 +481,22 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
     // re-eliminate.  The reordered variables are also mentioned in the
     // orphans and the leftover cached factors.
     gttic(permute_global_variable_index);
-    variableIndex_.permuteInPlace(partialSolveResult.fullReordering);
+    variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
     gttoc(permute_global_variable_index);
     gttic(permute_delta);
-    delta_ = delta_.permute(partialSolveResult.fullReordering);
+    delta_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
-    deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering);
+    deltaNewton_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
-    RgProd_ = RgProd_.permute(partialSolveResult.fullReordering);
+    RgProd_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
     gttoc(permute_delta);
     gttic(permute_ordering);
-    ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
+    ordering_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
     gttoc(permute_ordering);
     if(params_.cacheLinearizedFactors) {
       gttic(permute_cached_linear);
       //linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
       FastList<size_t> permuteLinearIndices = getAffectedFactors(affectedAndNewKeys);
       BOOST_FOREACH(size_t idx, permuteLinearIndices) {
-        linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse);
+        linearFactors_[idx]->reduceWithInverse(partialSolveResult.reorderingInverse);
       }
       gttoc(permute_cached_linear);
     }
@@ -514,7 +514,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
     gttic(orphans);
     gttic(permute);
     BOOST_FOREACH(sharedClique orphan, orphans) {
-      (void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse);
+      (void)orphan->reduceSeparatorWithInverse(partialSolveResult.reorderingInverse);
     }
     gttoc(permute);
     gttic(insert);
@@ -538,7 +538,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
   // Root clique variables for detailed results
   if(params_.enableDetailedResults) {
     BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) {
-      result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true;
+      result.detail->variableStatus[ordering_.key(index)].inRootClique = true;
     }
   }
 
@@ -628,7 +628,6 @@ ISAM2Result ISAM2::update(
   Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_);
   // New keys for detailed results
   if(params_.enableDetailedResults) {
-    inverseOrdering_ = ordering_.invert();
     BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } }
   gttoc(add_new_variables);
 
@@ -649,7 +648,7 @@ ISAM2Result ISAM2::update(
   // Observed keys for detailed results
   if(params_.enableDetailedResults) {
     BOOST_FOREACH(Index index, markedKeys) {
-      result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true;
+      result.detail->variableStatus[ordering_.key(index)].isObserved = true;
     }
   }
   // NOTE: we use assign instead of the iterator constructor here because this
@@ -666,7 +665,7 @@ ISAM2Result ISAM2::update(
   FastSet<Index> relinKeys;
   if (relinearizeThisStep) {
     gttic(gather_relinearize_keys);
-    vector<bool> markedRelinMask(ordering_.nVars(), false);
+    vector<bool> markedRelinMask(ordering_.size(), false);
     // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
     if(params_.enablePartialRelinearizationCheck)
       relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold);
@@ -677,8 +676,8 @@ ISAM2Result ISAM2::update(
     // Above relin threshold keys for detailed results
     if(params_.enableDetailedResults) {
       BOOST_FOREACH(Index index, relinKeys) {
-        result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold = true;
+        result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold = true;
-        result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } }
+        result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } }
 
     // Add the variables being relinearized to the marked keys
     BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; }
@@ -696,9 +695,9 @@ ISAM2Result ISAM2::update(
         FastSet<Index> involvedRelinKeys;
         Impl::FindAll(this->root(), involvedRelinKeys, markedRelinMask);
         BOOST_FOREACH(Index index, involvedRelinKeys) {
-          if(!result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold) {
+          if(!result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold) {
-            result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearizeInvolved = true;
+            result.detail->variableStatus[ordering_.key(index)].isRelinearizeInvolved = true;
-            result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } }
+            result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } }
       }
     }
     gttoc(fluid_find_all);
@@ -818,7 +817,7 @@ Values ISAM2::calculateEstimate() const {
   const VectorValues& delta(getDelta());
   gttoc(getDelta);
   gttic(Expmap);
-  vector<bool> mask(ordering_.nVars(), true);
+  vector<bool> mask(ordering_.size(), true);
   Impl::ExpmapMasked(ret, delta, ordering_, mask);
   gttoc(Expmap);
   return ret;
@@ -905,13 +904,13 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
 
   gttic(Compute_minimizing_step_size);
   // Compute minimizing step size
-  double RgNormSq = isam.RgProd_.vector().squaredNorm();
+  double RgNormSq = isam.RgProd_.asVector().squaredNorm();
   double step = -gradientSqNorm / RgNormSq;
   gttoc(Compute_minimizing_step_size);
 
   gttic(Compute_point);
   // Compute steepest descent point
-  grad.vector() *= step;
+  scal(step, grad);
   gttoc(Compute_point);
 }
 

gtsam/nonlinear/ISAM2.h

@@ -405,9 +405,15 @@ public:
     Base::permuteWithInverse(inversePermutation);
   }
 
-  bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
+  //bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
-    bool changed = Base::permuteSeparatorWithInverse(inversePermutation);
+  //  bool changed = Base::permuteSeparatorWithInverse(inversePermutation);
-    if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
+  //  if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
+  //  return changed;
+  //}
+
+  bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
+    bool changed = Base::reduceSeparatorWithInverse(inverseReduction);
+    if(changed) if(cachedFactor_) cachedFactor_->reduceWithInverse(inverseReduction);
     return changed;
   }
 
@@ -496,9 +502,6 @@ protected:
   /** The current Dogleg Delta (trust region radius) */
   mutable boost::optional<double> doglegDelta_;
 
-  /** The inverse ordering, only used for creating ISAM2Result::DetailedResults */
-  boost::optional<Ordering::InvertedMap> inverseOrdering_;
-
 public:
 
   typedef ISAM2 This; ///< This class

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -110,7 +110,7 @@ void LevenbergMarquardtOptimizer::iterate() {
       // Solve Damped Gaussian Factor Graph
       const VectorValues delta = solveGaussianFactorGraph(dampedSystem, params_);
 
-      if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.vector().norm() << endl;
+      if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl;
       if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta");
 
       // update values

gtsam/nonlinear/LinearContainerFactor.cpp

@@ -13,14 +13,8 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void LinearContainerFactor::rekeyFactor(const Ordering& ordering) {
-  Ordering::InvertedMap invOrdering = ordering.invert(); // TODO: inefficient - make more selective ordering invert
-  rekeyFactor(invOrdering);
-}
-
-/* ************************************************************************* */
-void LinearContainerFactor::rekeyFactor(const Ordering::InvertedMap& invOrdering) {
   BOOST_FOREACH(Index& idx, factor_->keys()) {
-    Key fullKey = invOrdering.find(idx)->second;
+    Key fullKey = ordering.key(idx);
     idx = fullKey;
     keys_.push_back(fullKey);
   }
@@ -86,34 +80,6 @@ LinearContainerFactor::LinearContainerFactor(
   initializeLinearizationPoint(linearizationPoint);
 }
 
-/* ************************************************************************* */
-LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor,
-    const Ordering::InvertedMap& inverted_ordering,
-    const Values& linearizationPoint)
-: factor_(factor.clone()) {
-  rekeyFactor(inverted_ordering);
-  initializeLinearizationPoint(linearizationPoint);
-}
-
-/* ************************************************************************* */
-LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor,
-    const Ordering::InvertedMap& inverted_ordering,
-    const Values& linearizationPoint)
-: factor_(factor.clone()) {
-  rekeyFactor(inverted_ordering);
-  initializeLinearizationPoint(linearizationPoint);
-}
-
-/* ************************************************************************* */
-LinearContainerFactor::LinearContainerFactor(
-    const GaussianFactor::shared_ptr& factor,
-    const Ordering::InvertedMap& ordering,
-    const Values& linearizationPoint)
-: factor_(factor->clone()) {
-  rekeyFactor(ordering);
-  initializeLinearizationPoint(linearizationPoint);
-}
-
 /* ************************************************************************* */
 void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
   Base::print(s+"LinearContainerFactor", keyFormatter);
@@ -198,6 +164,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(
 
   // Determine delta between linearization points using new ordering
   VectorValues delta = linearizationPoint_->localCoordinates(subsetC, localOrdering);
+  Vector deltaVector = delta.asVector();
 
   // clone and reorder linear factor to final ordering
   GaussianFactor::shared_ptr linFactor = order(localOrdering);
@@ -208,15 +175,14 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(
     HessianFactor::shared_ptr hesFactor = boost::shared_dynamic_cast<HessianFactor>(linFactor);
     size_t dim = hesFactor->linearTerm().size();
     Eigen::Block<HessianFactor::Block> Gview = hesFactor->info().block(0, 0, dim, dim);
-    Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * delta.vector();
+    Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * deltaVector;
-    hesFactor->constantTerm() += delta.vector().dot(G_delta) + delta.vector().dot(hesFactor->linearTerm());
+    hesFactor->constantTerm() += deltaVector.dot(G_delta) + deltaVector.dot(hesFactor->linearTerm());
     hesFactor->linearTerm() += G_delta;
   }
 
   // reset ordering
-  Ordering::InvertedMap invLocalOrdering = localOrdering.invert();
   BOOST_FOREACH(Index& idx, linFactor->keys())
-    idx = ordering[invLocalOrdering[idx] ];
+    idx = ordering[localOrdering.key(idx)];
   return linFactor;
 }
 
@@ -258,18 +224,11 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negate() const {
 NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
     const GaussianFactorGraph& linear_graph,  const Ordering& ordering,
     const Values& linearizationPoint) {
-  return convertLinearGraph(linear_graph, ordering.invert());
-}
-
-/* ************************************************************************* */
-NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
-    const GaussianFactorGraph& linear_graph,  const InvertedOrdering& invOrdering,
-    const Values& linearizationPoint) {
   NonlinearFactorGraph result;
   BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph)
     if (f)
       result.push_back(NonlinearFactorGraph::sharedFactor(
-          new LinearContainerFactor(f, invOrdering, linearizationPoint)));
+          new LinearContainerFactor(f, ordering, linearizationPoint)));
   return result;
 }
 

gtsam/nonlinear/LinearContainerFactor.h

@@ -45,21 +45,6 @@ public:
   LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
       const Values& linearizationPoint = Values());
 
-  /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
-  LinearContainerFactor(const JacobianFactor& factor,
-      const Ordering::InvertedMap& inverted_ordering,
-      const Values& linearizationPoint = Values());
-
-  /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
-  LinearContainerFactor(const HessianFactor& factor,
-      const Ordering::InvertedMap& inverted_ordering,
-      const Values& linearizationPoint = Values());
-
-  /** Constructor from shared_ptr with inverted ordering*/
-  LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
-      const Ordering::InvertedMap& ordering,
-      const Values& linearizationPoint = Values());
-
   // Access
 
   const GaussianFactor::shared_ptr& factor() const { return factor_; }
@@ -130,19 +115,13 @@ public:
 
   /**
    * Utility function for converting linear graphs to nonlinear graphs
-   * consisting of LinearContainerFactors.  Two versions are available, using
+   * consisting of LinearContainerFactors.
-   * either the ordering the linear graph was linearized around, or the inverse ordering.
-   * If the inverse ordering is present, it will be faster.
    */
   static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph,
       const Ordering& ordering, const Values& linearizationPoint = Values());
 
-  static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph,
-      const InvertedOrdering& invOrdering, const Values& linearizationPoint = Values());
-
 protected:
   void rekeyFactor(const Ordering& ordering);
-  void rekeyFactor(const Ordering::InvertedMap& invOrdering);
   void initializeLinearizationPoint(const Values& linearizationPoint);
 
 }; // \class LinearContainerFactor

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp

@@ -44,7 +44,7 @@ NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradient
 }
 NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State &current, const double alpha, const Gradient &g) const {
   Gradient step = g;
-  step.vector() *= alpha;
+  scal(alpha, step);
   return current.retract(step, ordering_);
 }
 

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -223,7 +223,7 @@ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
       variableIndex));
 
   // Permute the Ordering with the COLAMD ordering
-  ordering->permuteWithInverse(*colamdPerm->inverse());
+  ordering->permuteInPlace(*colamdPerm);
   return ordering;
 }
 
@@ -254,7 +254,7 @@ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMDConstrained(const Value
       variableIndex, index_constraints));
 
   // Permute the Ordering with the COLAMD ordering
-  ordering->permuteWithInverse(*colamdPerm->inverse());
+  ordering->permuteInPlace(*colamdPerm);
   return ordering;
 }
 

gtsam/nonlinear/NonlinearISAM.cpp

@@ -55,10 +55,8 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
 
     // Augment ordering
     // TODO: allow for ordering constraints within the new variables
-    // FIXME: should just loop over new values
+    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialValues)
-    BOOST_FOREACH(const NonlinearFactorGraph::sharedFactor& factor, newFactors)
+      ordering_.insert(key_value.key, ordering_.size());
-      BOOST_FOREACH(Key key, factor->keys())
-        ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
 
     boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_, ordering_);
 

gtsam/nonlinear/Ordering.cpp

@@ -26,17 +26,53 @@ using namespace std;
 namespace gtsam {
 
 /* ************************************************************************* */
-Ordering::Ordering(const std::list<Key> & L):nVars_(0) {
+Ordering::Ordering(const std::list<Key> & L) {
   int i = 0;
   BOOST_FOREACH( Key s, L )
     insert(s, i++) ;
 }
 
 /* ************************************************************************* */
-void Ordering::permuteWithInverse(const Permutation& inversePermutation) {
+Ordering::Ordering(const Ordering& other) : order_(other.order_), orderingIndex_(other.size()) {
-  gttic(Ordering_permuteWithInverse);
+  for(iterator item = order_.begin(); item != order_.end(); ++item)
-  BOOST_FOREACH(Ordering::value_type& key_order, *this) {
+    orderingIndex_[item->second] = item;
-    key_order.second = inversePermutation[key_order.second];
+}
+
+/* ************************************************************************* */
+Ordering& Ordering::operator=(const Ordering& rhs) {
+  order_ = rhs.order_;
+  orderingIndex_.resize(rhs.size());
+  for(iterator item = order_.begin(); item != order_.end(); ++item)
+    orderingIndex_[item->second] = item;
+  return *this;
+}
+
+/* ************************************************************************* */
+void Ordering::permuteInPlace(const Permutation& permutation) {
+  gttic(Ordering_permuteInPlace);
+  // Allocate new index and permute in map iterators
+  OrderingIndex newIndex(permutation.size());
+  for(size_t j = 0; j < newIndex.size(); ++j) {
+    newIndex[j] = orderingIndex_[permutation[j]]; // Assign the iterator
+    newIndex[j]->second = j; // Change the stored index to its permuted value
+  }
+  // Swap the new index into this Ordering class
+  orderingIndex_.swap(newIndex);
+}
+
+/* ************************************************************************* */
+void Ordering::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
+  if(selector.size() != permutation.size())
+    throw invalid_argument("Ordering::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
+  // Create new index the size of the permuted entries
+  OrderingIndex newIndex(selector.size());
+  // Permute the affected entries into the new index
+  for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
+    newIndex[dstSlot] = orderingIndex_[selector[permutation[dstSlot]]];
+  // Put the affected entries back in the new order and fix the indices
+  for(size_t slot = 0; slot < selector.size(); ++slot) {
+    orderingIndex_[selector[slot]] = newIndex[slot];
+    orderingIndex_[selector[slot]]->second = selector[slot];
   }
 }
 
@@ -44,16 +80,15 @@ void Ordering::permuteWithInverse(const Permutation& inversePermutation) {
 void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const {
   cout << str;
   // Print ordering in index order
-  Ordering::InvertedMap inverted = this->invert();
   // Print the ordering with varsPerLine ordering entries printed on each line,
   // for compactness.
   static const size_t varsPerLine = 10;
   bool endedOnNewline = false;
-  BOOST_FOREACH(const Ordering::InvertedMap::value_type& index_key, inverted) {
+  BOOST_FOREACH(const Map::iterator& index_key, orderingIndex_) {
-    if(index_key.first % varsPerLine != 0)
+    if(index_key->second % varsPerLine != 0)
       cout << ", ";
-    cout << index_key.first << ":" << keyFormatter(index_key.second);
+    cout << index_key->second<< ":" << keyFormatter(index_key->first);
-    if(index_key.first % varsPerLine == varsPerLine - 1) {
+    if(index_key->second % varsPerLine == varsPerLine - 1) {
       cout << "\n";
       endedOnNewline = true;
     } else {
@@ -72,40 +107,11 @@ bool Ordering::equals(const Ordering& rhs, double tol) const {
 
 /* ************************************************************************* */
 Ordering::value_type Ordering::pop_back() {
-  // FIXME: is there a way of doing this without searching over the entire structure?
+  iterator lastItem = orderingIndex_.back(); // Get the map iterator to the highest-index entry
-  for (iterator it=begin(); it!=end(); ++it) {
+  value_type removed = *lastItem; // Save the key-index pair to return
-    if (it->second == nVars_ - 1) {
+  order_.erase(lastItem); // Erase the entry from the map
-      value_type result = *it;
+  orderingIndex_.pop_back(); // Erase the entry from the index
-      order_.erase(it);
+  return removed; // Return the removed item
-      --nVars_;
-      return result;
-    }
-  }
-  return value_type();
-}
-
-/* ************************************************************************* */
-Index Ordering::pop_back(Key key) {
-  Map::iterator item = order_.find(key);
-  if(item == order_.end()) {
-    throw invalid_argument("Attempting to remove a key from an ordering that does not contain that key");
-  } else {
-    if(item->second != nVars_ - 1) {
-      throw invalid_argument("Attempting to remove a key from an ordering in which that key is not last");
-    } else {
-      order_.erase(item);
-      -- nVars_;
-      return nVars_;
-    }
-  }
-}
-
-/* ************************************************************************* */
-Ordering::InvertedMap Ordering::invert() const {
-  InvertedMap result;
-  BOOST_FOREACH(const value_type& p, *this)
-    result.insert(make_pair(p.second, p.first));
-  return result;
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/Ordering.h

@@ -24,7 +24,7 @@
 #include <boost/foreach.hpp>
 #include <boost/assign/list_inserter.hpp>
 #include <boost/pool/pool_alloc.hpp>
-#include <set>
+#include <vector>
 
 namespace gtsam {
 
@@ -35,8 +35,9 @@ namespace gtsam {
 class Ordering {
 protected:
   typedef FastMap<Key, Index> Map;
+  typedef std::vector<Map::iterator> OrderingIndex;
   Map order_;
-  Index nVars_;
+  OrderingIndex orderingIndex_;
 
 public:
 
@@ -51,25 +52,33 @@ public:
   /// @{
 
   /// Default constructor for empty ordering
-  Ordering() : nVars_(0) {}
+  Ordering() {}
+
+  /// Copy constructor
+  Ordering(const Ordering& other);
 
   /// Construct from list, assigns order indices sequentially to list items.
-  Ordering(const std::list<Key> & L) ;
+  Ordering(const std::list<Key> & L);
+
+  /// Assignment operator
+  Ordering& operator=(const Ordering& rhs);
 
   /// @}
   /// @name Standard Interface
   /// @{
 
-  /** One greater than the maximum ordering index, i.e. including missing indices in the count.  See also size(). */
-  Index nVars() const { return nVars_; }
-
   /** The actual number of variables in this ordering, i.e. not including missing indices in the count.  See also nVars(). */
-  Index size() const { return order_.size(); }
+  Index size() const { return orderingIndex_.size(); }
 
   const_iterator begin() const { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
   const_iterator end() const { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
 
   Index at(Key key) const { return operator[](key); } ///< Synonym for operator[](Key) const
+  Key key(Index index) const {
+    if(index >= orderingIndex_.size())
+      throw std::out_of_range("Attempting to access out-of-range index in Ordering");
+    else
+      return orderingIndex_[index]->first; }
 
   /** Assigns the ordering index of the requested \c key into \c index if the symbol
    * is present in the ordering, otherwise does not modify \c index.  The
@@ -85,8 +94,7 @@ public:
       index = i->second;
       return true;
     } else
-      return false;
+      return false; }
-  }
 
   /// Access the index for the requested key, throws std::out_of_range if the
   /// key is not present in the ordering (note that this differs from the
@@ -122,23 +130,25 @@ public:
    */
   const_iterator find(Key key) const { return order_.find(key); }
 
-  /**
+  /** Insert a key-index pair, but will fail if the key is already present */
-   * Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(),
-   * i.e., returns a pair of iterator and success (false if already present)
-   */
-  std::pair<iterator,bool> tryInsert(const value_type& key_order) {
-    std::pair<iterator,bool> it_ok(order_.insert(key_order));
-    if(it_ok.second == true && key_order.second+1 > nVars_)
-      nVars_ = key_order.second+1;
-    return it_ok;
-  }
-
-  /** Try insert, but will fail if the key is already present */
   iterator insert(const value_type& key_order) {
-    std::pair<iterator,bool> it_ok(tryInsert(key_order));
+    std::pair<iterator,bool> it_ok(order_.insert(key_order));
-    if(!it_ok.second)  throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key"));
+    if(it_ok.second) {
-    else               return it_ok.first;
+      if(key_order.second >= orderingIndex_.size())
-  }
+        orderingIndex_.resize(key_order.second + 1);
+      orderingIndex_[key_order.second] = it_ok.first;
+      return it_ok.first;
+    } else
+      throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key")); }
+
+  /// Test if the key exists in the ordering.
+  bool exists(Key key) const { return order_.count(key) > 0; }
+
+  /** Insert a key-index pair, but will fail if the key is already present */
+  iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); }
+
+  /// Adds a new key to the ordering with an index of one greater than the current highest index.
+  Index push_back(Key key) { return insert(std::make_pair(key, orderingIndex_.size()))->second; }
 
   /// @}
   /// @name Advanced Interface
@@ -154,18 +164,6 @@ public:
    */
   iterator end() { return order_.end(); }
 
-  /// Test if the key exists in the ordering.
-  bool exists(Key key) const { return order_.count(key) > 0; }
-
-  ///TODO: comment
-  std::pair<iterator,bool> tryInsert(Key key, Index order) { return tryInsert(std::make_pair(key,order)); }
-
-  ///TODO: comment
-  iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); }
-
-  /// Adds a new key to the ordering with an index of one greater than the current highest index.
-  Index push_back(Key key) { return insert(std::make_pair(key, nVars_))->second; }
-
   /** Remove the last (last-ordered, not highest-sorting key) symbol/index pair
    * from the ordering (this version is \f$ O(n) \f$, use it when you do not
    * know the last-ordered key).
@@ -177,16 +175,6 @@ public:
    */
   value_type pop_back();
 
-  /** Remove the last-ordered symbol from the ordering (this version is
-   * \f$ O(\log n) \f$, use it if you already know the last-ordered key).
-   *
-   * Throws std::invalid_argument if the requested key is not actually the
-   * last-ordered.
-   *
-   * @return The index of the symbol that was removed.
-   */
-  Index pop_back(Key key);
-
   /**
    * += operator allows statements like 'ordering += x0,x1,x2,x3;', which are
    * very useful for unit tests.  This functionality is courtesy of
@@ -201,17 +189,13 @@ public:
    * internally, permuting an initial key-sorted ordering into a fill-reducing
    * ordering.
    */
-  void permuteWithInverse(const Permutation& inversePermutation);
+  void permuteInPlace(const Permutation& permutation);
+
+  void permuteInPlace(const Permutation& selector, const Permutation& permutation);
 
   /// Synonym for operator[](Key)
   Index& at(Key key) { return operator[](key); }
 
-  /**
-   * Create an inverse mapping from Index->Key, useful for decoding linear systems
-   * @return inverse mapping structure
-   */
-  InvertedMap invert() const;
-
   /// @}
   /// @name Testable
   /// @{
@@ -228,16 +212,26 @@ private:
 
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class ARCHIVE>
+  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
+  void save(ARCHIVE & ar, const unsigned int version) const
+  {
     ar & BOOST_SERIALIZATION_NVP(order_);
-    ar & BOOST_SERIALIZATION_NVP(nVars_);
+    size_t size_ = orderingIndex_.size(); // Save only the size but not the iterators
-  }
+    ar & BOOST_SERIALIZATION_NVP(size_);
+  }
+  template<class ARCHIVE>
+  void load(ARCHIVE & ar, const unsigned int version)
+  {
+    ar & BOOST_SERIALIZATION_NVP(order_);
+    size_t size_;
+    ar & BOOST_SERIALIZATION_NVP(size_);
+    orderingIndex_.resize(size_);
+    for(iterator item = order_.begin(); item != order_.end(); ++item)
+      orderingIndex_[item->second] = item; // Assign the iterators
+  }
+  BOOST_SERIALIZATION_SPLIT_MEMBER()
 }; // \class Ordering
 
-// typedef for use with matlab
-typedef Ordering::InvertedMap InvertedOrdering;
-
 /**
  * @class Unordered
  * @brief a set of unordered indices
@@ -263,14 +257,15 @@ public:
 
 // Create an index formatter that looks up the Key in an inverse ordering, then
 // formats the key using the provided key formatter, used in saveGraph.
-struct OrderingIndexFormatter {
+class OrderingIndexFormatter {
-  Ordering::InvertedMap inverseOrdering;
+private:
-  const KeyFormatter& keyFormatter;
+  Ordering ordering_;
+  KeyFormatter keyFormatter_;
+public:
   OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) :
-      inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {}
+      ordering_(ordering), keyFormatter_(keyFormatter) {}
   std::string operator()(Index index) {
-    return keyFormatter(inverseOrdering.at(index));
+    return keyFormatter_(ordering_.key(index)); }
-  }
 };
 
 } // \namespace gtsam

gtsam/nonlinear/Values.cpp

@@ -81,7 +81,7 @@ namespace gtsam {
     Values result;
 
     for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
-      const SubVector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value
+      const Vector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value
       Key key = key_value->key;  // Non-const duplicate to deal with non-const insert argument
       Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract
       result.values_.insert(key, retractedValue); // Add retracted result directly to result values

gtsam/nonlinear/tests/testOrdering.cpp

@@ -23,26 +23,31 @@ using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-TEST( testOrdering, simple_modifications ) {
+TEST( Ordering, simple_modifications ) {
   Ordering ordering;
 
   // create an ordering
   Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4);
   ordering += x1, x2, x3, x4;
 
+  EXPECT_LONGS_EQUAL(0, ordering[x1]);
+  EXPECT_LONGS_EQUAL(1, ordering[x2]);
+  EXPECT_LONGS_EQUAL(2, ordering[x3]);
+  EXPECT_LONGS_EQUAL(3, ordering[x4]);
+  EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0));
+  EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1));
+  EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2));
+  EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3));
+
   // pop the last two elements
   Ordering::value_type x4p = ordering.pop_back();
   EXPECT_LONGS_EQUAL(3, ordering.size());
   EXPECT(assert_equal(x4, x4p.first));
 
-  Index x3p = ordering.pop_back(x3);
+  Index x3p = ordering.pop_back().second;
   EXPECT_LONGS_EQUAL(2, ordering.size());
   EXPECT_LONGS_EQUAL(2, (int)x3p);
 
-  // try to pop an element that doesn't exist and isn't last
-  CHECK_EXCEPTION(ordering.pop_back(x4), std::invalid_argument);
-  CHECK_EXCEPTION(ordering.pop_back(x1), std::invalid_argument);
-
   // reassemble back make the ordering 1, 2, 4, 3
   EXPECT_LONGS_EQUAL(2, ordering.push_back(x4));
   EXPECT_LONGS_EQUAL(3, ordering.push_back(x3));
@@ -50,6 +55,9 @@ TEST( testOrdering, simple_modifications ) {
   EXPECT_LONGS_EQUAL(2, ordering[x4]);
   EXPECT_LONGS_EQUAL(3, ordering[x3]);
 
+  EXPECT_LONGS_EQUAL(Key(x4), ordering.key(2));
+  EXPECT_LONGS_EQUAL(Key(x3), ordering.key(3));
+
   // verify
   Ordering expectedFinal;
   expectedFinal += x1, x2, x4, x3;
@@ -57,7 +65,36 @@ TEST( testOrdering, simple_modifications ) {
 }
 
 /* ************************************************************************* */
-TEST( testOrdering, invert ) {
+TEST(Ordering, permute) {
+  Ordering ordering;
+  ordering += 2, 4, 6, 8;
+
+  Ordering expected;
+  expected += 2, 8, 6, 4;
+
+  Permutation permutation(4);
+  permutation[0] = 0;
+  permutation[1] = 3;
+  permutation[2] = 2;
+  permutation[3] = 1;
+
+  Ordering actual = ordering;
+  actual.permuteInPlace(permutation);
+
+  EXPECT(assert_equal(expected, actual));
+
+  EXPECT_LONGS_EQUAL(0, actual[2]);
+  EXPECT_LONGS_EQUAL(1, actual[8]);
+  EXPECT_LONGS_EQUAL(2, actual[6]);
+  EXPECT_LONGS_EQUAL(3, actual[4]);
+  EXPECT_LONGS_EQUAL(2, actual.key(0));
+  EXPECT_LONGS_EQUAL(8, actual.key(1));
+  EXPECT_LONGS_EQUAL(6, actual.key(2));
+  EXPECT_LONGS_EQUAL(4, actual.key(3));
+}
+
+/* ************************************************************************* */
+TEST( Ordering, invert ) {
   // creates a map with the opposite mapping: Index->Key
   Ordering ordering;
 
@@ -65,14 +102,10 @@ TEST( testOrdering, invert ) {
   Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4);
   ordering += x1, x2, x3, x4;
 
-  Ordering::InvertedMap actual = ordering.invert();
+  EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0));
-  Ordering::InvertedMap expected;
+  EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1));
-  expected.insert(make_pair(0, x1));
+  EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2));
-  expected.insert(make_pair(1, x2));
+  EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3));
-  expected.insert(make_pair(2, x3));
-  expected.insert(make_pair(3, x4));
-
-  EXPECT(assert_container_equality(expected, actual));
 }
 
 /* ************************************************************************* */

tests/testDoglegOptimizer.cpp

@@ -32,6 +32,7 @@
 #pragma GCC diagnostic pop
 #include <boost/assign/list_of.hpp> // for 'list_of()'
 #include <functional>
+#include <boost/iterator/counting_iterator.hpp>
 
 using namespace std;
 using namespace gtsam;
@@ -45,7 +46,8 @@ double computeError(const GaussianBayesNet& gbn, const LieVector& values) {
 
   // Convert Vector to VectorValues
   VectorValues vv = *allocateVectorValues(gbn);
-  vv.vector() = values;
+  internal::writeVectorValuesSlices(values, vv,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));
 
   // Convert to factor graph
   GaussianFactorGraph gfg(gbn);
@@ -57,7 +59,8 @@ double computeErrorBt(const BayesTree<GaussianConditional>& gbt, const LieVector
 
   // Convert Vector to VectorValues
   VectorValues vv = *allocateVectorValues(gbt);
-  vv.vector() = values;
+  internal::writeVectorValuesSlices(values, vv,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));
 
   // Convert to factor graph
   GaussianFactorGraph gfg(gbt);
@@ -89,20 +92,22 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {
   // Compute the Hessian numerically
   Matrix hessian = numericalHessian(
       boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
-      LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).vector()));
+      LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).asVector()));
 
   // Compute the gradient numerically
   VectorValues gradientValues = *allocateVectorValues(gbn);
   Vector gradient = numericalGradient(
       boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
-      LieVector(VectorValues::Zero(gradientValues).vector()));
+      LieVector(VectorValues::Zero(gradientValues).asVector()));
-  gradientValues.vector() = gradient;
+  internal::writeVectorValuesSlices(gradient, gradientValues,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
 
   // Compute the gradient using dense matrices
   Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
   LONGS_EQUAL(11, augmentedHessian.cols());
   VectorValues denseMatrixGradient = *allocateVectorValues(gbn);
-  denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);
+  internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
   EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
 
   // Compute the steepest descent point
@@ -269,20 +274,22 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
   // Compute the Hessian numerically
   Matrix hessian = numericalHessian(
       boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
-      LieVector(VectorValues::Zero(*allocateVectorValues(bt)).vector()));
+      LieVector(VectorValues::Zero(*allocateVectorValues(bt)).asVector()));
 
   // Compute the gradient numerically
   VectorValues gradientValues = *allocateVectorValues(bt);
   Vector gradient = numericalGradient(
       boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
-      LieVector(VectorValues::Zero(gradientValues).vector()));
+      LieVector(VectorValues::Zero(gradientValues).asVector()));
-  gradientValues.vector() = gradient;
+  internal::writeVectorValuesSlices(gradient, gradientValues,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
 
   // Compute the gradient using dense matrices
   Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();
   LONGS_EQUAL(11, augmentedHessian.cols());
   VectorValues denseMatrixGradient = *allocateVectorValues(bt);
-  denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);
+  internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
   EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
 
   // Compute the steepest descent point
@@ -338,12 +345,12 @@ TEST(DoglegOptimizer, ComputeBlend) {
   VectorValues xn = optimize(gbn);
 
   // The Newton's method point should be more "adventurous", i.e. larger, than the steepest descent point
-  EXPECT(xu.vector().norm() < xn.vector().norm());
+  EXPECT(xu.asVector().norm() < xn.asVector().norm());
 
   // Compute blend
   double Delta = 1.5;
   VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn);
-  DOUBLES_EQUAL(Delta, xb.vector().norm(), 1e-10);
+  DOUBLES_EQUAL(Delta, xb.asVector().norm(), 1e-10);
 }
 
 /* ************************************************************************* */
@@ -371,12 +378,12 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) {
 
   double Delta1 = 0.5;  // Less than steepest descent
   VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn));
-  DOUBLES_EQUAL(Delta1, actual1.vector().norm(), 1e-5);
+  DOUBLES_EQUAL(Delta1, actual1.asVector().norm(), 1e-5);
 
   double Delta2 = 1.5;  // Between steepest descent and Newton's method
   VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
   VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
-  DOUBLES_EQUAL(Delta2, actual2.vector().norm(), 1e-5);
+  DOUBLES_EQUAL(Delta2, actual2.asVector().norm(), 1e-5);
   EXPECT(assert_equal(expected2, actual2));
 
   double Delta3 = 5.0;  // Larger than Newton's method point

tests/testGaussianFactorGraphB.cpp

@@ -252,7 +252,7 @@ TEST( GaussianFactorGraph, getOrdering)
   Ordering original; original += L(1),X(1),X(2);
   FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
   Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic)));
-  Ordering actual = original; actual.permuteWithInverse((*perm.inverse()));
+  Ordering actual = original; actual.permuteInPlace(perm);
   Ordering expected; expected += L(1),X(2),X(1);
   EXPECT(assert_equal(expected,actual));
 }

tests/testGaussianISAM2.cpp

@@ -207,6 +207,7 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) {
   EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
   EXPECT(assert_equal(orderingExpected, ordering));
 }
+
 /* ************************************************************************* */
 TEST_UNSAFE(ISAM2, ImplRemoveVariables) {
 

tests/timeiSAM2Chain.cpp

@@ -0,0 +1,100 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    timeiSAM2Chain.cpp
+ * @brief   Times each iteration of a long chain in iSAM2, to measure asymptotic performance
+ * @author  Richard Roberts
+ */
+
+#include <gtsam/base/timing.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/Symbol.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/Marginals.h>
+
+#include <fstream>
+#include <boost/archive/binary_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/serialization/export.hpp>
+#include <boost/random/mersenne_twister.hpp>
+#include <boost/random/uniform_real.hpp>
+#include <boost/random/variate_generator.hpp>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtsam::symbol_shorthand;
+
+typedef Pose2 Pose;
+
+typedef NoiseModelFactor1<Pose> NM1;
+typedef NoiseModelFactor2<Pose,Pose> NM2;
+noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim());
+
+int main(int argc, char *argv[]) {
+
+  const size_t steps = 50000;
+
+  cout << "Playing forward " << steps << " time steps..." << endl;
+
+  ISAM2 isam2;
+
+  // Times
+  vector<double> times(steps);
+
+  for(size_t step=0; step < steps; ++step) {
+
+    Values newVariables;
+    NonlinearFactorGraph newFactors;
+
+    // Collect measurements and new variables for the current step
+    gttic_(Create_measurements);
+    if(step == 0) {
+      // Add prior
+      newFactors.add(PriorFactor<Pose>(0, Pose(), noiseModel::Unit::Create(Pose::Dim())));
+      newVariables.insert(0, Pose());
+    } else {
+      Vector eta = Vector::Random(Pose::Dim()) * 0.1;
+      Pose2 between = Pose().retract(eta);
+      // Add between
+      newFactors.add(BetweenFactor<Pose>(step-1, step, between, model));
+      newVariables.insert(step, isam2.calculateEstimate<Pose>(step-1) * between);
+    }
+
+    gttoc_(Create_measurements);
+
+    // Update iSAM2
+    gttic_(Update_ISAM2);
+    isam2.update(newFactors, newVariables);
+    gttoc_(Update_ISAM2);
+
+    tictoc_finishedIteration_();
+
+    tictoc_getNode(node, Update_ISAM2);
+    times[step] = node->time();
+
+    if(step % 1000 == 0) {
+      cout << "Step " << step << endl;
+      tictoc_print_();
+    }
+  }
+
+  tictoc_print_();
+
+  // Write times
+  ofstream timesFile("times.txt");
+  BOOST_FOREACH(double t, times) {
+    timesFile << t << "\n"; }
+
+  return 0;
+}