Merge branch 'borglab:develop' into patch-4

.clang-format

@@ -0,0 +1,8 @@
+BasedOnStyle: Google
+
+BinPackArguments: false
+BinPackParameters: false
+ColumnLimit: 100
+DerivePointerAlignment: false
+IncludeBlocks: Preserve
+PointerAlignment: Left

cmake/GtsamBuildTypes.cmake

@@ -150,7 +150,7 @@ if (NOT CMAKE_VERSION VERSION_LESS 3.8)
     set(CMAKE_CXX_EXTENSIONS OFF)
     if (MSVC)
       # NOTE(jlblanco): seems to be required in addition to the cxx_std_17 above?
-      list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++latest)
+      list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++17)
     endif()
 else()
   # Old cmake versions:

gtsam/base/std_optional_serialization.h

@@ -76,8 +76,7 @@ void save(Archive& ar, const std::optional<T>& t, const unsigned int /*version*/
 }
 
 template <class Archive, class T>
-void load(Archive& ar, std::optional<T>& t, const unsigned int /*version*/
-) {
+void load(Archive& ar, std::optional<T>& t, const unsigned int /*version*/) {
   bool tflag;
   ar >> boost::serialization::make_nvp("initialized", tflag);
   if (!tflag) {

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/discrete/DecisionTreeFactor.cpp

@@ -94,7 +94,10 @@ namespace gtsam {
     for (Key j : f.keys()) cs[j] = f.cardinality(j);
     // Convert map into keys
     DiscreteKeys keys;
-    for (const std::pair<const Key, size_t>& key : cs) keys.push_back(key);
+    keys.reserve(cs.size());
+    for (const auto& key : cs) {
+      keys.emplace_back(key);
+    }
     // apply operand
     ADT result = ADT::apply(f, op);
     // Make a new factor

gtsam/geometry/Line3.cpp

@@ -111,8 +111,8 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
   }
   if (Dline) {
     Dline->setIdentity();
-    (*Dline)(0, 3) = -t[2];
-    (*Dline)(1, 2) = t[2];
+    (*Dline)(3, 0) = -t[2];
+    (*Dline)(2, 1) = t[2];
   }
   return Line3(cRl, c_ab[0], c_ab[1]);
 }

gtsam/geometry/tests/testLine3.cpp

@@ -123,10 +123,10 @@ TEST(Line3, localCoordinatesOfRetract) {
 // transform from world to camera test
 TEST(Line3, transformToExpressionJacobians) {
   Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0));
-  Vector3 t(0, 0, 0);
+  Vector3 t(-2.0, 2.0, 3.0);
   Pose3 p(r, t);
 
-  Line3 l_c(r.inverse(), 1, 1);
+  Line3 l_c(r.inverse(), 3, -1);
   Line3 l_w(Rot3(), 1, 1);
   EXPECT(l_c.equals(transformTo(p, l_w)));
 

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/linear/VectorValues.cpp

@@ -41,7 +41,7 @@ namespace gtsam {
   /* ************************************************************************ */
   VectorValues::VectorValues(const Vector& x, const Dims& dims) {
     size_t j = 0;
-    for (const auto& [key,n] : dims)  {
+    for (const auto& [key, n] : dims) {
 #ifdef TBB_GREATER_EQUAL_2020
       values_.emplace(key, x.segment(j, n));
 #else
@@ -68,7 +68,7 @@ namespace gtsam {
   VectorValues VectorValues::Zero(const VectorValues& other)
   {
     VectorValues result;
-    for(const auto& [key,value]: other)
+    for (const auto& [key, value] : other)
 #ifdef TBB_GREATER_EQUAL_2020
       result.values_.emplace(key, Vector::Zero(value.size()));
 #else
@@ -79,7 +79,7 @@ namespace gtsam {
 
   /* ************************************************************************ */
   VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
-    std::pair<iterator, bool> result = values_.insert(key_value);
+    const std::pair<iterator, bool> result = values_.insert(key_value);
     if(!result.second)
       throw std::invalid_argument(
       "Requested to insert variable '" + DefaultKeyFormatter(key_value.first)
@@ -90,7 +90,7 @@ namespace gtsam {
   /* ************************************************************************ */
   VectorValues& VectorValues::update(const VectorValues& values) {
     iterator hint = begin();
-    for (const auto& [key,value] : values) {
+    for (const auto& [key, value] : values) {
       // Use this trick to find the value using a hint, since we are inserting
       // from another sorted map
       size_t oldSize = values_.size();
@@ -131,10 +131,10 @@ namespace gtsam {
     // Change print depending on whether we are using TBB
 #ifdef GTSAM_USE_TBB
     std::map<Key, Vector> sorted;
-    for (const auto& [key,value] : v) {
+    for (const auto& [key, value] : v) {
       sorted.emplace(key, value);
     }
-    for (const auto& [key,value] : sorted)
+    for (const auto& [key, value] : sorted)
 #else
     for (const auto& [key,value] : v)
 #endif
@@ -344,14 +344,13 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  VectorValues operator*(const double a, const VectorValues &v)
-  {
+  VectorValues operator*(const double a, const VectorValues& c) {
     VectorValues result;
-    for(const VectorValues::KeyValuePair& key_v: v)
+    for (const auto& [key, value] : c)
 #ifdef TBB_GREATER_EQUAL_2020
-      result.values_.emplace(key_v.first, a * key_v.second);
+      result.values_.emplace(key, a * value);
 #else
-      result.values_.insert({key_v.first, a * key_v.second});
+      result.values_.insert({key, a * value});
 #endif
     return result;
   }

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;

python/CMakeLists.txt

@@ -198,9 +198,9 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
             "${GTSAM_UNSTABLE_MODULE_PATH}")
 
     # Hack to get python test files copied every time they are modified
-    file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py")
+    file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/" "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py")
     foreach(test_file ${GTSAM_UNSTABLE_PYTHON_TEST_FILES})
-        configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY)
+        configure_file("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/${test_file}" "${GTSAM_UNSTABLE_MODULE_PATH}/${test_file}" COPYONLY)
     endforeach()
 
     # Add gtsam_unstable to the install target