Merge pull request #1507 from borglab/improvements

Various Fixes and Updates

gtsam/base/timing.cpp

@@ -272,20 +272,21 @@ void tic(size_t id, const char *labelC) {
 }
 
 /* ************************************************************************* */
-void toc(size_t id, const char *label) {
+void toc(size_t id, const char *labelC) {
 // disable anything which refers to TimingOutline as well, for good measure
 #ifdef GTSAM_USE_BOOST_FEATURES
+  const std::string label(labelC);
   std::shared_ptr<TimingOutline> current(gCurrentTimer.lock());
   if (id != current->id_) {
     gTimingRoot->print();
     throw std::invalid_argument(
-        "gtsam timing:  Mismatched tic/toc: gttoc(\"" + std::string(label) +
+        "gtsam timing:  Mismatched tic/toc: gttoc(\"" + label +
         "\") called when last tic was \"" + current->label_ + "\".");
   }
   if (!current->parent_.lock()) {
     gTimingRoot->print();
     throw std::invalid_argument(
-        "gtsam timing:  Mismatched tic/toc: extra gttoc(\"" + std::string(label) +
+        "gtsam timing:  Mismatched tic/toc: extra gttoc(\"" + label +
         "\"), already at the root");
   }
   current->toc();

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -248,7 +248,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
 
 #ifdef HYBRID_TIMING
   tictoc_print_();
-  tictoc_reset_();
 #endif
 
   // Separate out decision tree into conditionals and remaining factors.
@@ -416,9 +415,6 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
     return continuousElimination(factors, frontalKeys);
   } else {
     // Case 3: We are now in the hybrid land!
-#ifdef HYBRID_TIMING
-    tictoc_reset_();
-#endif
     return hybridElimination(factors, frontalKeys, continuousSeparator,
                              discreteSeparatorSet);
   }

gtsam/hybrid/HybridSmoother.cpp

@@ -57,8 +57,16 @@ Ordering HybridSmoother::getOrdering(
 
 /* ************************************************************************* */
 void HybridSmoother::update(HybridGaussianFactorGraph graph,
-                            const Ordering &ordering,
-                            std::optional<size_t> maxNrLeaves) {
+                            std::optional<size_t> maxNrLeaves,
+                            const std::optional<Ordering> given_ordering) {
+  Ordering ordering;
+  // If no ordering provided, then we compute one
+  if (!given_ordering.has_value()) {
+    ordering = this->getOrdering(graph);
+  } else {
+    ordering = *given_ordering;
+  }
+
   // Add the necessary conditionals from the previous timestep(s).
   std::tie(graph, hybridBayesNet_) =
       addConditionals(graph, hybridBayesNet_, ordering);

gtsam/hybrid/HybridSmoother.h

@@ -44,13 +44,14 @@ class HybridSmoother {
    * corresponding to the pruned choices.
    *
    * @param graph The new factors, should be linear only
-   * @param ordering The ordering for elimination, only continuous vars are
-   * allowed
    * @param maxNrLeaves The maximum number of leaves in the new discrete factor,
    * if applicable
+   * @param given_ordering The (optional) ordering for elimination, only
+   * continuous variables are allowed
    */
-  void update(HybridGaussianFactorGraph graph, const Ordering& ordering,
-              std::optional<size_t> maxNrLeaves = {});
+  void update(HybridGaussianFactorGraph graph,
+              std::optional<size_t> maxNrLeaves = {},
+              const std::optional<Ordering> given_ordering = {});
 
   Ordering getOrdering(const HybridGaussianFactorGraph& newFactors);
 
@@ -74,4 +75,4 @@ class HybridSmoother {
   const HybridBayesNet& hybridBayesNet() const;
 };
 
-};  // namespace gtsam
+}  // namespace gtsam

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -46,35 +46,6 @@ using namespace gtsam;
 using symbol_shorthand::X;
 using symbol_shorthand::Z;
 
-Ordering getOrdering(HybridGaussianFactorGraph& factors,
-                     const HybridGaussianFactorGraph& newFactors) {
-  factors.push_back(newFactors);
-  // Get all the discrete keys from the factors
-  KeySet allDiscrete = factors.discreteKeySet();
-
-  // Create KeyVector with continuous keys followed by discrete keys.
-  KeyVector newKeysDiscreteLast;
-  const KeySet newFactorKeys = newFactors.keys();
-  // Insert continuous keys first.
-  for (auto& k : newFactorKeys) {
-    if (!allDiscrete.exists(k)) {
-      newKeysDiscreteLast.push_back(k);
-    }
-  }
-
-  // Insert discrete keys at the end
-  std::copy(allDiscrete.begin(), allDiscrete.end(),
-            std::back_inserter(newKeysDiscreteLast));
-
-  const VariableIndex index(factors);
-
-  // Get an ordering where the new keys are eliminated last
-  Ordering ordering = Ordering::ColamdConstrainedLast(
-      index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
-      true);
-  return ordering;
-}
-
 TEST(HybridEstimation, Full) {
   size_t K = 6;
   std::vector<double> measurements = {0, 1, 2, 2, 2, 3};
@@ -117,7 +88,7 @@ TEST(HybridEstimation, Full) {
 
 /****************************************************************************/
 // Test approximate inference with an additional pruning step.
-TEST(HybridEstimation, Incremental) {
+TEST(HybridEstimation, IncrementalSmoother) {
   size_t K = 15;
   std::vector<double> measurements = {0, 1, 2, 2, 2, 2,  3,  4,  5,  6, 6,
                                       7, 8, 9, 9, 9, 10, 11, 11, 11, 11};
@@ -136,7 +107,6 @@ TEST(HybridEstimation, Incremental) {
   initial.insert(X(0), switching.linearizationPoint.at<double>(X(0)));
 
   HybridGaussianFactorGraph linearized;
-  HybridGaussianFactorGraph bayesNet;
 
   for (size_t k = 1; k < K; k++) {
     // Motion Model
@@ -146,11 +116,10 @@ TEST(HybridEstimation, Incremental) {
 
     initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
 
-    bayesNet = smoother.hybridBayesNet();
     linearized = *graph.linearize(initial);
-    Ordering ordering = getOrdering(bayesNet, linearized);
+    Ordering ordering = smoother.getOrdering(linearized);
 
-    smoother.update(linearized, ordering, 3);
+    smoother.update(linearized, 3, ordering);
     graph.resize(0);
   }
 

gtsam/navigation/ConstantVelocityFactor.h

@@ -38,7 +38,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
    public:
     ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
         : NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {}
-    ~ConstantVelocityFactor() override{};
+    ~ConstantVelocityFactor() override {}
 
     /**
      * @brief Caclulate error: (x2 - x1.update(dt)))

gtsam/navigation/ManifoldPreintegration.cpp

@@ -67,9 +67,11 @@ void ManifoldPreintegration::update(const Vector3& measuredAcc,
 
   // Possibly correct for sensor pose
   Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
-  if (p().body_P_sensor)
-    std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
-        D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
+  if (p().body_P_sensor) {
+    std::tie(acc, omega) = correctMeasurementsBySensorPose(
+        acc, omega, D_correctedAcc_acc, D_correctedAcc_omega,
+        D_correctedOmega_omega);
+  }
 
   // Save current rotation for updating Jacobians
   const Rot3 oldRij = deltaXij_.attitude();

gtsam/navigation/ManifoldPreintegration.h

@@ -27,7 +27,7 @@
 namespace gtsam {
 
 /**
- * IMU pre-integration on NavSatet manifold.
+ * IMU pre-integration on NavState manifold.
  * This corresponds to the original RSS paper (with one difference: V is rotated)
  */
 class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {

gtsam/navigation/TangentPreintegration.cpp

@@ -111,9 +111,11 @@ void TangentPreintegration::update(const Vector3& measuredAcc,
 
   // Possibly correct for sensor pose by converting to body frame
   Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
-  if (p().body_P_sensor)
-    std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
-        D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
+  if (p().body_P_sensor) {
+    std::tie(acc, omega) = correctMeasurementsBySensorPose(
+        acc, omega, D_correctedAcc_acc, D_correctedAcc_omega,
+        D_correctedOmega_omega);
+  }
 
   // Do update
   deltaTij_ += dt;