Merge branch 'develop' into hybrid/hybrid-factor-graph

gtsam/discrete/DecisionTree-inl.h

@@ -291,10 +291,7 @@ namespace gtsam {
         }
 
         os << "\"" << this->id() << "\" -> \"" << branch->id() << "\"";
-        if (B == 2) {
-          if (i == 0) os << " [style=dashed]";
-          if (i > 1) os << " [style=bold]";
-        }
+        if (B == 2 && i == 0) os << " [style=dashed]";
         os << std::endl;
         branch->dot(os, labelFormatter, valueFormatter, showZero);
       }

gtsam/discrete/DecisionTree.h

@@ -220,7 +220,7 @@ namespace gtsam {
     /// @{
 
     /// Make virtual
-    virtual ~DecisionTree() {}
+    virtual ~DecisionTree() = default;
 
     /// Check if tree is empty.
     bool empty() const { return !root_; }

gtsam/discrete/DiscreteFactorGraph.h

@@ -209,6 +209,10 @@ class GTSAM_EXPORT DiscreteFactorGraph
   /// @}
 };  // \ DiscreteFactorGraph
 
+std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
+EliminateForMPE(const DiscreteFactorGraph& factors,
+                const Ordering& frontalKeys);
+
 /// traits
 template <>
 struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {};

gtsam/geometry/Rot3.cpp

@@ -228,6 +228,7 @@ double Rot3::yaw(OptionalJacobian<1, 3> H) const {
 }
 
 /* ************************************************************************* */
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 Vector Rot3::quaternion() const {
   gtsam::Quaternion q = toQuaternion();
   Vector v(4);
@@ -237,6 +238,7 @@ Vector Rot3::quaternion() const {
   v(3) = q.z();
   return v;
 }
+#endif
 
 /* ************************************************************************* */
 pair<Unit3, double> Rot3::axisAngle() const {

gtsam/geometry/Rot3.h

@@ -515,11 +515,16 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
      */
     gtsam::Quaternion toQuaternion() const;
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
     /**
      * Converts to a generic matrix to allow for use with matlab
      * In format: w x y z
+     * @deprecated: use Rot3::toQuaternion() instead.
+     * If still using this API, remind that in the returned Vector `V`,
+     * `V.x()` denotes the actual `qw`, `V.y()` denotes 'qx', `V.z()` denotes `qy`, and `V.w()` denotes 'qz'.
      */
-    Vector quaternion() const;
+    Vector GTSAM_DEPRECATED quaternion() const;
+#endif
 
     /**
      * @brief Spherical Linear intERPolation between *this and other

gtsam/geometry/geometry.i

@@ -355,7 +355,7 @@ class Rot3 {
   double yaw() const;
   pair<gtsam::Unit3, double> axisAngle() const;
   gtsam::Quaternion toQuaternion() const;
-  Vector quaternion() const;
+  // Vector quaternion() const; // @deprecated, see https://github.com/borglab/gtsam/pull/1219
   gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
 
   // enabling serialization functionality

gtsam/hybrid/tests/testGaussianMixture.cpp

@@ -35,7 +35,10 @@ using symbol_shorthand::M;
 using symbol_shorthand::X;
 
 /* ************************************************************************* */
-TEST(GaussianConditional, Equals) {
+/* Check construction of GaussianMixture P(x1 | x2, m1) as well as accessing a
+ * specific mode i.e. P(x1 | x2, m1=1).
+ */
+TEST(GaussianMixture, Equals) {
   // create a conditional gaussian node
   Matrix S1(2, 2);
   S1(0, 0) = 1;
@@ -89,4 +92,4 @@ int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
-/* ************************************************************************* */
+/* ************************************************************************* */

gtsam/inference/FactorGraph.h

@@ -361,7 +361,7 @@ class FactorGraph {
    * less than the original, factors at the end will be removed.  If the new
    * size is larger than the original, null factors will be appended.
    */
-  void resize(size_t size) { factors_.resize(size); }
+  virtual void resize(size_t size) { factors_.resize(size); }
 
   /** delete factor without re-arranging indexes by inserting a nullptr pointer
    */

gtsam/inference/Ordering.cpp

@@ -283,6 +283,17 @@ void Ordering::print(const std::string& str,
   cout.flush();
 }
 
+/* ************************************************************************* */
+Ordering::This& Ordering::operator+=(KeyVector& keys) {
+  this->insert(this->end(), keys.begin(), keys.end());
+  return *this;
+}
+
+/* ************************************************************************* */
+bool Ordering::contains(const Key& key) const {
+  return std::find(this->begin(), this->end(), key) != this->end();
+}
+
 /* ************************************************************************* */
 bool Ordering::equals(const Ordering& other, double tol) const {
   return (*this) == other;

gtsam/inference/Ordering.h

@@ -70,7 +70,23 @@ public:
         boost::assign_detail::call_push_back<This>(*this))(key);
   }
 
-  /// Invert (not reverse) the ordering - returns a map from key to order position
+  /**
+   * @brief Append new keys to the ordering as `ordering += keys`.
+   *
+   * @param key
+   * @return The ordering variable with appended keys.
+   */
+  This& operator+=(KeyVector& keys);
+
+  /// Check if key exists in ordering.
+  bool contains(const Key& key) const;
+
+  /**
+   * @brief Invert (not reverse) the ordering - returns a map from key to order
+   * position.
+   *
+   * @return FastMap<Key, size_t>
+   */
   FastMap<Key, size_t> invert() const;
 
   /// @name Fill-reducing Orderings @{

gtsam/inference/tests/testOrdering.cpp

@@ -199,6 +199,32 @@ TEST(Ordering, csr_format_3) {
   EXPECT(adjExpected == adjAcutal);
 }
 
+/* ************************************************************************* */
+TEST(Ordering, AppendVector) {
+  using symbol_shorthand::X;
+  Ordering actual;
+  KeyVector keys = {X(0), X(1), X(2)};
+  actual += keys;
+
+  Ordering expected;
+  expected += X(0);
+  expected += X(1);
+  expected += X(2);
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(Ordering, Contains) {
+  using symbol_shorthand::X;
+  Ordering ordering;
+  ordering += X(0);
+  ordering += X(1);
+  ordering += X(2);
+
+  EXPECT(ordering.contains(X(1)));
+  EXPECT(!ordering.contains(X(4)));
+}
+
 /* ************************************************************************* */
 #ifdef GTSAM_SUPPORT_NESTED_DISSECTION
 TEST(Ordering, csr_format_4) {

gtsam/linear/GaussianBayesNet.h

@@ -45,7 +45,7 @@ namespace gtsam {
     /// @name Standard Constructors
     /// @{
 
-    /** Construct empty factor graph */
+    /** Construct empty bayes net */
     GaussianBayesNet() {}
 
     /** Construct from iterator over conditionals */

gtsam/linear/GaussianConditional.cpp

@@ -15,10 +15,10 @@
  * @author Christian Potthast, Frank Dellaert
  */
 
-#include <gtsam/linear/linearExceptions.h>
 #include <gtsam/linear/GaussianConditional.h>
-#include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/Sampler.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/linearExceptions.h>
 
 #include <boost/format.hpp>
 #ifdef __GNUC__

gtsam/linear/GaussianConditional.h

@@ -24,6 +24,7 @@
 #include <gtsam/global_includes.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/inference/Conditional.h>
+#include <gtsam/inference/Conditional-inst.h>
 #include <gtsam/linear/VectorValues.h>
 
 #include <random> // for std::mt19937_64 

gtsam/sfm/TranslationRecovery.cpp

@@ -119,21 +119,24 @@ void TranslationRecovery::addPrior(
   graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
                                              priorNoiseModel);
 
-  // Add between factors for optional relative translations.
-  for (auto edge : betweenTranslations) {
-    graph->emplace_shared<BetweenFactor<Point3>>(
-        edge.key1(), edge.key2(), edge.measured(), edge.noiseModel());
-  }
-
   // Add a scale prior only if no other between factors were added.
   if (betweenTranslations.empty()) {
     graph->emplace_shared<PriorFactor<Point3>>(
         edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
+    return;
+  }
+
+  // Add between factors for optional relative translations.
+  for (auto prior_edge : betweenTranslations) {
+    graph->emplace_shared<BetweenFactor<Point3>>(
+        prior_edge.key1(), prior_edge.key2(), prior_edge.measured(),
+        prior_edge.noiseModel());
   }
 }
 
 Values TranslationRecovery::initializeRandomly(
     const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
     std::mt19937 *rng, const Values &initialValues) const {
   uniform_real_distribution<double> randomVal(-1, 1);
   // Create a lambda expression that checks whether value exists and randomly
@@ -155,14 +158,20 @@ Values TranslationRecovery::initializeRandomly(
     insert(edge.key1());
     insert(edge.key2());
   }
+  // There may be nodes in betweenTranslations that do not have a measurement.
+  for (auto edge : betweenTranslations) {
+    insert(edge.key1());
+    insert(edge.key2());
+  }
   return initial;
 }
 
 Values TranslationRecovery::initializeRandomly(
     const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
     const Values &initialValues) const {
-  return initializeRandomly(relativeTranslations, &kRandomNumberGenerator,
-                            initialValues);
+  return initializeRandomly(relativeTranslations, betweenTranslations,
+                            &kRandomNumberGenerator, initialValues);
 }
 
 Values TranslationRecovery::run(
@@ -187,8 +196,8 @@ Values TranslationRecovery::run(
            &graph);
 
   // Uses initial values from params if provided.
-  Values initial =
-      initializeRandomly(nonzeroRelativeTranslations, initialValues);
+  Values initial = initializeRandomly(
+      nonzeroRelativeTranslations, nonzeroBetweenTranslations, initialValues);
 
   // If there are no valid edges, but zero-distance edges exist, initialize one
   // of the nodes in a connected component of zero-distance edges.

gtsam/sfm/TranslationRecovery.h

@@ -112,12 +112,15 @@ class TranslationRecovery {
    *
    * @param relativeTranslations unit translation directions between
    * translations to be estimated
+   * @param betweenTranslations relative translations (with scale) between 2
+   * points in world coordinate frame known a priori.
    * @param rng random number generator
    * @param intialValues (optional) initial values from a prior
    * @return Values
    */
   Values initializeRandomly(
       const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
       std::mt19937 *rng, const Values &initialValues = Values()) const;
 
   /**
@@ -125,11 +128,14 @@ class TranslationRecovery {
    *
    * @param relativeTranslations unit translation directions between
    * translations to be estimated
+   * @param betweenTranslations relative translations (with scale) between 2
+   * points in world coordinate frame known a priori.
    * @param initialValues (optional) initial values from a prior
    * @return Values
    */
   Values initializeRandomly(
       const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
       const Values &initialValues = Values()) const;
 
   /**
@@ -137,11 +143,15 @@ class TranslationRecovery {
    *
    * @param relativeTranslations the relative translations, in world coordinate
    * frames, vector of BinaryMeasurements of Unit3, where each key of a
-   * measurement is a point in 3D.
+   * measurement is a point in 3D. If a relative translation magnitude is zero,
+   * it is treated as a hard same-point constraint (the result of all nodes
+   * connected by a zero-magnitude edge will be the same).
    * @param scale scale for first relative translation which fixes gauge.
    * The scale is only used if betweenTranslations is empty.
    * @param betweenTranslations relative translations (with scale) between 2
-   * points in world coordinate frame known a priori.
+   * points in world coordinate frame known a priori. Unlike
+   * relativeTranslations, zero-magnitude betweenTranslations are not treated as
+   * hard constraints.
    * @param initialValues intial values for optimization. Initializes randomly
    * if not provided.
    * @return Values

gtsam_unstable/timing/timeShonanAveraging.cpp

@@ -79,9 +79,9 @@ void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> poses
         myfile << "VERTEX_SE3:QUAT" << " ";
         myfile << i << " ";
         myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " ";
-        Vector quaternion = Rot3(R).quaternion();
-        myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1)
-               << " " << quaternion(0);
+        Quaternion quaternion = Rot3(R).toQuaternion();
+        myfile << quaternion.x() << " " << quaternion.y() << " " << quaternion.z()
+               << " " << quaternion.w();
         myfile << "\n";
     }
     myfile.close();

python/gtsam/examples/Pose3ISAM2Example.py

@@ -34,7 +34,10 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa
 
     # Plot the newly updated iSAM2 inference.
     fig = plt.figure(0)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
     plt.cla()
 
     i = 1

python/gtsam/examples/VisualISAM2Example.py

@@ -33,7 +33,10 @@ def visual_ISAM2_plot(result):
     fignum = 0
 
     fig = plt.figure(fignum)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
     plt.cla()
 
     # Plot points

python/gtsam/utils/plot.py

@@ -333,7 +333,10 @@ def plot_point3(
 
     """
     fig = plt.figure(fignum)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
     plot_point3_on_axes(axes, point, linespec, P)
 
     axes.set_xlabel(axis_labels[0])
@@ -388,7 +391,7 @@ def plot_3d_points(fignum,
 
     fig = plt.figure(fignum)
     fig.suptitle(title)
-    fig.canvas.set_window_title(title.lower())
+    fig.canvas.manager.set_window_title(title.lower())
 
 
 def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
@@ -490,7 +493,10 @@ def plot_trajectory(
         axis_labels (iterable[string]): List of axis labels to set.
     """
     fig = plt.figure(fignum)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
 
     axes.set_xlabel(axis_labels[0])
     axes.set_ylabel(axis_labels[1])
@@ -522,7 +528,7 @@ def plot_trajectory(
         plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale)
 
     fig.suptitle(title)
-    fig.canvas.set_window_title(title.lower())
+    fig.canvas.manager.set_window_title(title.lower())
 
 
 def plot_incremental_trajectory(fignum: int,
@@ -545,7 +551,10 @@ def plot_incremental_trajectory(fignum: int,
             Used to create animation effect.
     """
     fig = plt.figure(fignum)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
 
     poses = gtsam.utilities.allPose3s(values)
     keys = gtsam.KeyVector(poses.keys())

tests/testTranslationRecovery.cpp

@@ -323,6 +323,36 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
   EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
   EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
 }
+
+TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurements) {
+  // Checks that valid results are obtained when a between translation edge is
+  // provided with a node that does not have any other relative translations.
+  Values poses;
+  poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
+  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
+  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
+  poses.insert<Pose3>(4, Pose3(Rot3(), Point3(1, 2, 1)));
+
+  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {0, 3}, {1, 3}});
+
+  std::vector<BinaryMeasurement<Point3>> betweenTranslations;
+  betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0),
+                                   noiseModel::Constrained::All(3, 1e2));
+  // Node 4 only has this between translation prior, no relative translations.
+  betweenTranslations.emplace_back(0, 4, Point3(1, 2, 1));
+
+  TranslationRecovery algorithm;
+  auto result =
+      algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
+
+  // Check result
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
+  EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
+  EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
+  EXPECT(assert_equal(Point3(1, 2, 1), result.at<Point3>(4), 1e-4));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;