Merge pull request #1640 from borglab/fixes

Various Fixes and TODOs

gtsam/CMakeLists.txt

@@ -145,10 +145,6 @@ if (GTSAM_USE_EIGEN_MKL)
   target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})
 endif()
 
-if(GTSAM_USE_TBB)
-  target_include_directories(gtsam PUBLIC ${TBB_INCLUDE_DIRS})
-endif()
-
 # Add includes for source directories 'BEFORE' boost and any system include
 # paths so that the compiler uses GTSAM headers in our source directory instead
 # of any previously installed GTSAM headers.

gtsam/discrete/DecisionTreeFactor.cpp

@@ -164,15 +164,23 @@ namespace gtsam {
     }
 
     // create new factor, note we collect keys that are not in frontalKeys
-    // TODO(frank): why do we need this??? result should contain correct keys!!!
+    /*
+    Due to branch merging, the labels in `result` may be missing some keys
+    E.g. After branch merging, we may get a ADT like:
+      Leaf [2] 1.0204082
+
+    This is missing the key values used for branching.
+    */
+    KeyVector difference, frontalKeys_(frontalKeys), keys_(keys());
+    // Get the difference of the frontalKeys and the factor keys using set_difference
+    std::sort(keys_.begin(), keys_.end());
+    std::sort(frontalKeys_.begin(), frontalKeys_.end());
+    std::set_difference(keys_.begin(), keys_.end(), frontalKeys_.begin(),
+                        frontalKeys_.end(), back_inserter(difference));
+
     DiscreteKeys dkeys;
-    for (i = 0; i < keys().size(); i++) {
+    for (Key key : difference) {
-      Key j = keys()[i];
+      dkeys.push_back(DiscreteKey(key, cardinality(key)));
-      // TODO(frank): inefficient!
-      if (std::find(frontalKeys.begin(), frontalKeys.end(), j) !=
-          frontalKeys.end())
-        continue;
-      dkeys.push_back(DiscreteKey(j, cardinality(j)));
     }
     return std::make_shared<DecisionTreeFactor>(dkeys, result);
   }

gtsam/discrete/DiscreteBayesTree.h

@@ -110,4 +110,12 @@ class GTSAM_EXPORT DiscreteBayesTree
   /// @}
 };
 
+/// traits
+template <>
+struct traits<DiscreteBayesTreeClique>
+    : public Testable<DiscreteBayesTreeClique> {};
+
+template <>
+struct traits<DiscreteBayesTree> : public Testable<DiscreteBayesTree> {};
+
 }  // namespace gtsam

gtsam/discrete/DiscreteKey.cpp

@@ -49,6 +49,7 @@ namespace gtsam {
 
   void DiscreteKeys::print(const std::string& s,
                            const KeyFormatter& keyFormatter) const {
+    std::cout << (s.empty() ? "" : s + " ") << std::endl;
     for (auto&& dkey : *this) {
       std::cout << DefaultKeyFormatter(dkey.first) << " " << dkey.second
                 << std::endl;

gtsam/discrete/tests/testDiscreteMarginals.cpp

@@ -121,7 +121,7 @@ TEST_UNSAFE( DiscreteMarginals, truss ) {
 
   Clique expected0(std::make_shared<DiscreteConditional>((key[0] | key[2], key[4]) = "2/1 2/1 2/1 2/1"));
   Clique::shared_ptr actual0 = (*bayesTree)[0];
-//  EXPECT(assert_equal(expected0, *actual0)); // TODO, correct but fails
+  EXPECT(assert_equal(expected0, *actual0));
 
   Clique expected1(std::make_shared<DiscreteConditional>((key[1] | key[3], key[4]) = "1/2 1/2 1/2 1/2"));
   Clique::shared_ptr actual1 = (*bayesTree)[1];

gtsam/geometry/CameraSet.h

@@ -438,8 +438,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
 
       // (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
       // add contribution of current factor
-      // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for
+      // Eigen doesn't let us pass the expression so we call eval()
-      // now...
       augmentedHessian.updateDiagonalBlock(
           aug_i,
           ((FiT *

gtsam/geometry/Similarity2.h

@@ -74,7 +74,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
   bool operator==(const Similarity2& other) const;
 
   /// Print with optional string
-  void print(const std::string& s) const;
+  void print(const std::string& s = "") const;
 
   GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
                                                const Similarity2& p);

gtsam/geometry/Similarity3.cpp

@@ -171,7 +171,7 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
   return internal::align(d_abPointPairs, aRb, centroids);
 }
 
-Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
+Similarity3 Similarity3::Align(const Pose3Pairs &abPosePairs) {
   const size_t n = abPosePairs.size();
   if (n < 2)
     throw std::runtime_error("input should have at least 2 pairs of poses");

gtsam/geometry/Similarity3.h

@@ -75,7 +75,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   bool operator==(const Similarity3& other) const;
 
   /// Print with optional string
-  void print(const std::string& s) const;
+  void print(const std::string& s = "") const;
 
   GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
                                                const Similarity3& p);
@@ -133,7 +133,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
    * computed using the algorithm described here:
    * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
    */
-  static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
+  static Similarity3 Align(const Pose3Pairs& abPosePairs);
 
   /// @}
   /// @name Lie Group

gtsam/geometry/geometry.i

@@ -1082,6 +1082,7 @@ class Similarity2 {
 
   // Standard Interface
   bool equals(const gtsam::Similarity2& sim, double tol) const;
+  void print(const std::string& s = "") const;
   Matrix matrix() const;
   gtsam::Rot2& rotation();
   gtsam::Point2& translation();
@@ -1105,6 +1106,7 @@ class Similarity3 {
 
   // Standard Interface
   bool equals(const gtsam::Similarity3& sim, double tol) const;
+  void print(const std::string& s = "") const;
   Matrix matrix() const;
   gtsam::Rot3& rotation();
   gtsam::Point3& translation();

gtsam/hybrid/HybridBayesTree.h

@@ -123,6 +123,10 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
 };
 
 /// traits
+template <>
+struct traits<HybridBayesTreeClique> : public Testable<HybridBayesTreeClique> {
+};
+
 template <>
 struct traits<HybridBayesTree> : public Testable<HybridBayesTree> {};
 

gtsam/hybrid/HybridGaussianISAM.h

@@ -29,7 +29,8 @@
 namespace gtsam {
 
 /**
- * @brief 
+ * @brief Incremental Smoothing and Mapping (ISAM) algorithm
+ * for hybrid factor graphs.
  *
  * @ingroup hybrid
  */

gtsam/hybrid/HybridNonlinearFactorGraph.cpp

@@ -56,8 +56,6 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
   for (auto& f : factors_) {
     // First check if it is a valid factor
     if (!f) {
-      // TODO(dellaert): why?
-      linearFG->push_back(GaussianFactor::shared_ptr());
       continue;
     }
     // Check if it is a nonlinear mixture factor

gtsam_unstable/partition/FindSeparator-inl.h

@@ -39,7 +39,7 @@ namespace gtsam { namespace partition {
    * whether node j is in the left part of the graph, the right part, or the
    * separator, respectively
    */
-  std::pair<int, sharedInts> separatorMetis(idx_t n, const sharedInts& xadj,
+  std::pair<idx_t, sharedInts> separatorMetis(idx_t n, const sharedInts& xadj,
     const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) {
 
     // control parameters
@@ -277,7 +277,7 @@ namespace gtsam { namespace partition {
       //throw runtime_error("separatorPartitionByMetis:stop for debug");
     }
 
-    if(result.C.size() != sepsize) {
+    if(result.C.size() != size_t(sepsize)) {
       std::cout << "total key: " << keys.size()
           << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size()
           << "; sepsize from Metis = " << sepsize << std::endl;

python/gtsam/tests/test_backwards_compatibility.py

@@ -29,7 +29,7 @@ UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
 
 
 class TestBackwardsCompatibility(GtsamTestCase):
-    """Tests for the backwards compatibility for the Python wrapper."""
+    """Tests for backwards compatibility of the Python wrapper."""
 
     def setUp(self):
         """Setup test fixtures"""