A bunch of minor fixes

gtsam/discrete/DiscreteLookupDAG.h

@@ -46,7 +46,7 @@ class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional {
    * @brief Construct a new Discrete Lookup Table object
    *
    * @param nFrontals number of frontal variables
-   * @param keys a orted list of gtsam::Keys
+   * @param keys a sorted list of gtsam::Keys
    * @param potentials the algebraic decision tree with lookup values
    */
   DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys,

gtsam/geometry/Pose3.cpp

@@ -59,7 +59,7 @@ Matrix6 Pose3::AdjointMap() const {
   const Matrix3 R = R_.matrix();
   Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
   Matrix6 adj;
-  adj << R, Z_3x3, A, R;
+  adj << R, Z_3x3, A, R;  // Gives [R 0; A R]
   return adj;
 }
 

gtsam/inference/BayesTree.h

@@ -139,7 +139,7 @@ namespace gtsam {
       return nodes_.empty();
     }
 
-    /** return nodes */
+    /** Return nodes. Each node is a clique of variables obtained after elimination. */
     const Nodes& nodes() const { return nodes_; }
 
     /** Access node by variable */

gtsam/navigation/PreintegrationParams.cpp

@@ -34,7 +34,6 @@ void PreintegrationParams::print(const string& s) const {
        << endl;
   if (omegaCoriolis && use2ndOrderCoriolis)
     cout << "Using 2nd-order Coriolis" << endl;
-  if (body_P_sensor) body_P_sensor->print("    ");
   cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
 }
 

gtsam/nonlinear/ISAM2.h

@@ -295,6 +295,17 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
       const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
       const KeySet& relinKeys);
 
+  /**
+   * @brief Perform an incremental update of the factor graph to return a new
+   * Bayes Tree with affected keys.
+   *
+   * @param updateParams Parameters for the ISAM2 update.
+   * @param relinKeys Keys of variables to relinearize.
+   * @param affectedKeys The set of keys which are affected in the update.
+   * @param affectedKeysSet [output] Affected and contaminated keys.
+   * @param orphans [output] List of orphanes cliques after elimination.
+   * @param result [output] The result of the incremental update step.
+   */
   void recalculateIncremental(const ISAM2UpdateParams& updateParams,
                               const KeySet& relinKeys,
                               const FastList<Key>& affectedKeys,

gtsam/slam/BetweenFactor.h

@@ -80,7 +80,9 @@ namespace gtsam {
     /// @{
 
     /// print with optional string
-    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
+    void print(
+        const std::string& s = "",
+        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "BetweenFactor("
           << keyFormatter(this->key1()) << ","
           << keyFormatter(this->key2()) << ")\n";

python/gtsam/tests/test_GaussianFactorGraph.py

@@ -23,23 +23,23 @@
 def create_graph():
     """Create a basic linear factor graph for testing"""
     graph = gtsam.GaussianFactorGraph()
-    
+
     x0 = X(0)
     x1 = X(1)
     x2 = X(2)
-    
+
     BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
     PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
 
     graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
-    graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE)
+    graph.add(x2, np.eye(1), x1, -np.eye(1), 2 * np.ones(1), BETWEEN_NOISE)
     graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE)
 
     return graph, (x0, x1, x2)
 
+
 class TestGaussianFactorGraph(GtsamTestCase):
     """Tests for Gaussian Factor Graphs."""
-
     def test_fg(self):
         """Test solving a linear factor graph"""
         graph, X = create_graph()
@@ -71,7 +71,7 @@ def test_convertNonlinear(self):
         self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
         self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
         self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
-    
+
     def test_linearMarginalization(self):
         """Marginalize a linear factor graph"""
         graph, X = create_graph()
@@ -88,5 +88,22 @@ def test_linearMarginalization(self):
         self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
         self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
 
+    def test_ordering(self):
+        """Test ordering"""
+        gfg, keys = create_graph()
+        ordering = gtsam.Ordering()
+        for key in keys[::-1]:
+            ordering.push_back(key)
+
+        bn = gfg.eliminateSequential(ordering)
+        self.assertEqual(bn.size(), 3)
+
+        keyVector = gtsam.KeyVector()
+        keyVector.append(keys[2])
+        ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector)
+        bn = gfg.eliminateSequential(ordering)
+        self.assertEqual(bn.size(), 3)
+
+
 if __name__ == '__main__':
     unittest.main()