Merged in jlblancoc/gtsam/index-type-for-factors (pull request #409)

Type for Factor indices, dual to "Key"

gtsam.h

@@ -209,6 +209,38 @@ class KeyGroupMap {
   bool insert2(size_t key, int val);
 };
 
+// Actually a FastSet<FactorIndex>
+class FactorIndexSet {
+  FactorIndexSet();
+  FactorIndexSet(const gtsam::FactorIndexSet& set);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  void insert(size_t factorIndex);
+  bool erase(size_t factorIndex); // returns true if value was removed
+  bool count(size_t factorIndex) const; // returns true if value exists
+};
+
+// Actually a vector<FactorIndex>
+class FactorIndices {
+  FactorIndices();
+  FactorIndices(const gtsam::FactorIndices& other);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  size_t at(size_t i) const;
+  size_t front() const;
+  size_t back() const;
+  void push_back(size_t factorIndex) const;
+};
 //*************************************************************************
 // base
 //*************************************************************************
@@ -1219,7 +1251,7 @@ namespace noiseModel {
 #include <gtsam/linear/NoiseModel.h>
 virtual class Base {
   void print(string s) const;
-  // Methods below are available for all noise models. However, can't add them 
+  // Methods below are available for all noise models. However, can't add them
   // because wrap (incorrectly) thinks robust classes derive from this Base as well.
   // bool isConstrained() const;
   // bool isUnit() const;
@@ -1257,7 +1289,7 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
   Vector sigmas() const;
   Vector invsigmas() const;
   Vector precisions() const;
-    
+
   // enabling serialization functionality
   void serializable() const;
 };
@@ -2053,7 +2085,7 @@ virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams {
   bool getUseFixedLambdaFactor();
   string getLogFile() const;
   string getVerbosityLM() const;
-  
+
   void setDiagonalDamping(bool flag);
   void setlambdaFactor(double value);
   void setlambdaInitial(double value);

gtsam/base/types.h

@@ -56,6 +56,9 @@ namespace gtsam {
   /// Integer nonlinear key type
   typedef std::uint64_t Key;
 
+  /// Integer nonlinear factor index type
+  typedef std::uint64_t FactorIndex;
+
   /// The index type for Eigen objects
   typedef ptrdiff_t DenseIndex;
 

gtsam/inference/Factor.h

@@ -28,6 +28,9 @@
 #include <gtsam/inference/Key.h>
 
 namespace gtsam {
+/// Define collection types:
+typedef FastVector<FactorIndex> FactorIndices;
+typedef FastSet<FactorIndex> FactorIndexSet;
 
   /**
    * This is the base class for all factor types.  It is templated on a KEY type,

gtsam/inference/VariableIndex.cpp

@@ -35,8 +35,8 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c
   cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
   for(KeyMap::value_type key_factors: index_) {
     cout << "var " << keyFormatter(key_factors.first) << ":";
-    for(const size_t factor: key_factors.second)
-      cout << " " << factor;
+    for(const auto index: key_factors.second)
+      cout << " " << index;
     cout << "\n";
   }
   cout.flush();
@@ -48,8 +48,8 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
   // run over variables, which will be hyper-edges.
   for(KeyMap::value_type key_factors: index_) {
     // every variable is a hyper-edge covering its factors
-    for(const size_t factor: key_factors.second)
-      os << (factor+1) << " "; // base 1
+    for(const auto index: key_factors.second)
+      os << (index+1) << " "; // base 1
     os << "\n";
   }
   os << flush;

gtsam/inference/VariableIndex.h

@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include <gtsam/inference/Factor.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/base/FastVector.h>
@@ -43,7 +44,7 @@ class GTSAM_EXPORT VariableIndex {
 public:
 
   typedef boost::shared_ptr<VariableIndex> shared_ptr;
-  typedef FastVector<size_t> Factors;
+  typedef FactorIndices Factors;
   typedef Factors::iterator Factor_iterator;
   typedef Factors::const_iterator Factor_const_iterator;
 
@@ -122,7 +123,7 @@ public:
    * solving problems incrementally.
    */
   template<class FG>
-  void augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices = boost::none);
+  void augment(const FG& factors, boost::optional<const FactorIndices&> newFactorIndices = boost::none);
 
   /**
    * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement

gtsam/nonlinear/ISAM2.cpp

@@ -96,7 +96,7 @@ bool ISAM2::equals(const ISAM2& other, double tol) const {
 }
 
 /* ************************************************************************* */
-KeySet ISAM2::getAffectedFactors(const KeyList& keys) const {
+FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const {
   static const bool debug = false;
   if (debug) cout << "Getting affected factors for ";
   if (debug) {
@@ -106,15 +106,14 @@ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const {
   }
   if (debug) cout << endl;
 
-  NonlinearFactorGraph allAffected;
-  KeySet indices;
+  FactorIndexSet indices;
   for (const Key key : keys) {
     const VariableIndex::Factors& factors(variableIndex_[key]);
     indices.insert(factors.begin(), factors.end());
   }
   if (debug) cout << "Affected factors are: ";
   if (debug) {
-    for (const size_t index : indices) {
+    for (const auto index : indices) {
       cout << index << " ";
     }
   }
@@ -132,8 +131,6 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(
   KeySet candidates = getAffectedFactors(affectedKeys);
   gttoc(getAffectedFactors);
 
-  NonlinearFactorGraph nonlinearAffectedFactors;
-
   gttic(affectedKeysSet);
   // for fast lookup below
   KeySet affectedKeysSet;
@@ -589,7 +586,7 @@ ISAM2Result ISAM2::update(
   // Remove the removed factors
   NonlinearFactorGraph removeFactors;
   removeFactors.reserve(removeFactorIndices.size());
-  for (size_t index : removeFactorIndices) {
+  for (const auto index : removeFactorIndices) {
     removeFactors.push_back(nonlinearFactors_[index]);
     nonlinearFactors_.remove(index);
     if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
@@ -823,7 +820,7 @@ void ISAM2::marginalizeLeaves(
   KeySet leafKeysRemoved;
 
   // Keep track of factors that get summarized by removing cliques
-  KeySet factorIndicesToRemove;
+  FactorIndexSet factorIndicesToRemove;
 
   // Remove the subtree and throw away the cliques
   auto trackingRemoveSubtree = [&](const sharedClique& subtreeRoot) {
@@ -937,8 +934,8 @@ void ISAM2::marginalizeLeaves(
           }
         }
         // Create factor graph from factor indices
-        for (size_t i : factorsFromMarginalizedInClique_step1) {
-          graph.push_back(nonlinearFactors_[i]->linearize(theta_));
+        for (const auto index: factorsFromMarginalizedInClique_step1) {
+          graph.push_back(nonlinearFactors_[index]->linearize(theta_));
         }
 
         // Reeliminate the linear graph to get the marginal and discard the
@@ -1011,10 +1008,10 @@ void ISAM2::marginalizeLeaves(
   // Remove the factors to remove that have been summarized in the newly-added
   // marginal factors
   NonlinearFactorGraph removedFactors;
-  for (size_t i : factorIndicesToRemove) {
-    removedFactors.push_back(nonlinearFactors_[i]);
-    nonlinearFactors_.remove(i);
-    if (params_.cacheLinearizedFactors) linearFactors_.remove(i);
+  for (const auto index: factorIndicesToRemove) {
+    removedFactors.push_back(nonlinearFactors_[index]);
+    nonlinearFactors_.remove(index);
+    if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
   }
   variableIndex_.remove(factorIndicesToRemove.begin(),
                         factorIndicesToRemove.end(), removedFactors);

gtsam/nonlinear/ISAM2.h

@@ -292,7 +292,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
    */
   void expmapMasked(const KeySet& mask);
 
-  FastSet<Key> getAffectedFactors(const FastList<Key>& keys) const;
+  FactorIndexSet getAffectedFactors(const FastList<Key>& keys) const;
   GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(
       const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
   GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans);

gtsam/nonlinear/ISAM2Result.h

@@ -31,8 +31,6 @@
 
 namespace gtsam {
 
-typedef FastVector<size_t> FactorIndices;
-
 /**
  * @addtogroup ISAM2
  * This struct is returned from ISAM2::update() and contains information about

gtsam_unstable/discrete/tests/testLoopyBelief.cpp

@@ -175,19 +175,19 @@ private:
 
       // collect all factors involving this key in the original graph
       DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph());
-      for(size_t factorIdx: varIndex[key]) {
-        star->push_back(graph.at(factorIdx));
+      for(size_t factorIndex: varIndex[key]) {
+        star->push_back(graph.at(factorIndex));
 
         // accumulate unary factors
-        if (graph.at(factorIdx)->size() == 1) {
+        if (graph.at(factorIndex)->size() == 1) {
           if (!prodOfUnaries)
             prodOfUnaries = boost::dynamic_pointer_cast<DecisionTreeFactor>(
-                graph.at(factorIdx));
+                graph.at(factorIndex));
           else
             prodOfUnaries = boost::make_shared<DecisionTreeFactor>(
                 *prodOfUnaries
                     * (*boost::dynamic_pointer_cast<DecisionTreeFactor>(
-                        graph.at(factorIdx))));
+                        graph.at(factorIndex))));
         }
       }
 

gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * 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)