New add_factors method

borglab · Jun 1, 2019 · 495a840 · 495a840
1 parent da1b7f9
commit 495a840
Show file tree

Hide file tree

Showing 3 changed files with 240 additions and 200 deletions.
diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h
@@ -26,74 +26,105 @@
 #include <boost/bind.hpp>
 
 #include <stdio.h>
+#include <algorithm>
+#include <iostream>  // for cout :-(
 #include <sstream>
-#include <iostream> // for cout :-(
+#include <string>
 
 namespace gtsam {
 
-  /* ************************************************************************* */
-  template<class FACTOR>
-  void FactorGraph<FACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
-    std::cout << s << std::endl;
-    std::cout << "size: " << size() << std::endl;
-    for (size_t i = 0; i < factors_.size(); i++) {
-      std::stringstream ss;
-      ss << "factor " << i << ": ";
-      if (factors_[i])
-        factors_[i]->print(ss.str(), formatter);
-    }
+/* ************************************************************************* */
+template <class FACTOR>
+void FactorGraph<FACTOR>::print(const std::string& s,
+                                const KeyFormatter& formatter) const {
+  std::cout << s << std::endl;
+  std::cout << "size: " << size() << std::endl;
+  for (size_t i = 0; i < factors_.size(); i++) {
+    std::stringstream ss;
+    ss << "factor " << i << ": ";
+    if (factors_[i]) factors_[i]->print(ss.str(), formatter);
   }
-
-  /* ************************************************************************* */
-  template<class FACTOR>
-  bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
-    /** check whether the two factor graphs have the same number of factors_ */
-    if (factors_.size() != fg.size()) return false;
-
-    /** check whether the factors_ are the same */
-    for (size_t i = 0; i < factors_.size(); i++) {
-      // TODO: Doesn't this force order of factor insertion?
-      sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
-      if (f1 == NULL && f2 == NULL) continue;
-      if (f1 == NULL || f2 == NULL) return false;
-      if (!f1->equals(*f2, tol)) return false;
-    }
-    return true;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
+  // check whether the two factor graphs have the same number of factors.
+  if (factors_.size() != fg.size()) return false;
+
+  // check whether the factors are the same, in same order.
+  for (size_t i = 0; i < factors_.size(); i++) {
+    sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
+    if (f1 == NULL && f2 == NULL) continue;
+    if (f1 == NULL || f2 == NULL) return false;
+    if (!f1->equals(*f2, tol)) return false;
   }
-
-  /* ************************************************************************* */
-  template<class FACTOR>
-  size_t FactorGraph<FACTOR>::nrFactors() const {
-    size_t size_ = 0;
-    for(const sharedFactor& factor: factors_)
-      if (factor) size_++;
-    return size_;
+  return true;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+size_t FactorGraph<FACTOR>::nrFactors() const {
+  size_t size_ = 0;
+  for (const sharedFactor& factor : factors_)
+    if (factor) size_++;
+  return size_;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+KeySet FactorGraph<FACTOR>::keys() const {
+  KeySet keys;
+  for (const sharedFactor& factor : this->factors_) {
+    if (factor) keys.insert(factor->begin(), factor->end());
   }
-
-  /* ************************************************************************* */
-  template<class FACTOR>
-  KeySet FactorGraph<FACTOR>::keys() const {
-    KeySet keys;
-    for(const sharedFactor& factor: this->factors_) {
-      if(factor)
-        keys.insert(factor->begin(), factor->end());
+  return keys;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+KeyVector FactorGraph<FACTOR>::keyVector() const {
+  KeyVector keys;
+  keys.reserve(2 * size());  // guess at size
+  for (const sharedFactor& factor : factors_)
+    if (factor) keys.insert(keys.end(), factor->begin(), factor->end());
+  std::sort(keys.begin(), keys.end());
+  auto last = std::unique(keys.begin(), keys.end());
+  keys.erase(last, keys.end());
+  return keys;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+template <typename CONTAINER, typename>
+FactorIndices FactorGraph<FACTOR>::add_factors(const CONTAINER& factors,
+                                               bool useEmptySlots) {
+  const size_t num_factors = factors.size();
+  FactorIndices newFactorIndices(num_factors);
+  if (useEmptySlots) {
+    size_t i = 0;
+    for (size_t j = 0; j < num_factors; ++j) {
+      // Loop to find the next available factor slot
+      do {
+        if (i >= size())
+          // Make room for remaining factors, happens only once.
+          resize(size() + num_factors - j);
+        else if (at(i))
+          ++i;  // Move on to the next slot or past end.
+        else
+          break;  // We found an empty slot, break to fill it.
+      } while (true);
+
+      // Use the current slot, updating graph and newFactorSlots.
+      at(i) = factors[j];
+      newFactorIndices[j] = i;
     }
-    return keys;
-  }
-
-  /* ************************************************************************* */
-  template <class FACTOR>
-  KeyVector FactorGraph<FACTOR>::keyVector() const {
-    KeyVector keys;
-    keys.reserve(2 * size());  // guess at size
-    for (const sharedFactor& factor: factors_)
-      if (factor)
-        keys.insert(keys.end(), factor->begin(), factor->end());
-    std::sort(keys.begin(), keys.end());
-    auto last = std::unique(keys.begin(), keys.end());
-    keys.erase(last, keys.end());
-    return keys;
+  } else {
+    // We're not looking for unused slots, so just add the factors at the end.
+    for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size();
+    push_back(factors);
   }
+  return newFactorIndices;
+}
 
-  /* ************************************************************************* */
-} // namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
@@ -273,6 +273,14 @@ class FactorGraph {
     bayesTree.addFactorsToGraph(*this);
   }
 
+  /**
+   * Add new factors to a factor graph and returns a list of new factor indices,
+   * optionally finding and reusing empty factor slots.
+   */
+  template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
+  FactorIndices add_factors(const CONTAINER& factors,
+                            bool useEmptySlots = false);
+
   /// @}
   /// @name Testable
   /// @{