Merge branch 'develop' into model-selection-integration

Author: varunagrawal

.github/workflows/build-python.yml

@@ -109,7 +109,7 @@ jobs:
         uses: ilammy/msvc-dev-cmd@v1
         with:
           arch: x${{matrix.platform}}
-          toolset: 14.38
+          toolset: 14.2
 
       - name: cl version (Windows)
         if: runner.os == 'Windows'

.github/workflows/build-windows.yml

@@ -50,7 +50,7 @@ jobs:
         uses: ilammy/msvc-dev-cmd@v1
         with:
           arch: x${{ matrix.platform }}
-          toolset: 14.38
+          toolset: 14.2
 
       - name: cl version
         shell: cmd
@@ -124,6 +124,7 @@ jobs:
           # https://stackoverflow.com/a/24470998/1236990
           cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam
           cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target all.tests
 
       - name: Test
         shell: bash

examples/IMUKittiExampleGPS.cpp

@@ -241,14 +241,14 @@ int main(int argc, char* argv[]) {
       "-- Starting main loop: inference is performed at each time step, but we "
       "plot trajectory every 10 steps\n");
   size_t j = 0;
-  size_t included_imu_measurement_count = 0;
 
   for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
     // At each non=IMU measurement we initialize a new node in the graph
     auto current_pose_key = X(i);
     auto current_vel_key = V(i);
     auto current_bias_key = B(i);
     double t = gps_measurements[i].time;
+    size_t included_imu_measurement_count = 0;
 
     if (i == first_gps_pose) {
       // Create initial estimate and prior on initial pose, velocity, and biases

gtsam/3rdparty/cephes/CMakeLists.txt

@@ -16,7 +16,8 @@ set(CEPHES_HEADER_FILES
     cephes/lanczos.h
     cephes/mconf.h
     cephes/polevl.h
-    cephes/sf_error.h)
+    cephes/sf_error.h
+    dllexport.h)
 
 # Add header files
 install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes)

gtsam/geometry/geometry.i

@@ -69,6 +69,7 @@ class StereoPoint2 {
   // Standard Constructors
   StereoPoint2();
   StereoPoint2(double uL, double uR, double v);
+  StereoPoint2(const gtsam::Vector3 &v);
 
   // Testable
   void print(string s = "") const;
@@ -836,6 +837,12 @@ class Cal3_S2Stereo {
   Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
   Cal3_S2Stereo(Vector v);
 
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Cal3_S2Stereo retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const;
+
   // Testable
   void print(string s = "") const;
   bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
@@ -846,8 +853,11 @@ class Cal3_S2Stereo {
   double skew() const;
   double px() const;
   double py() const;
+  Matrix K() const;
   gtsam::Point2 principalPoint() const;
   double baseline() const;
+  Vector6 vector() const;
+  Matrix inverse() const;
 };
 
 #include <gtsam/geometry/Cal3Bundler.h>

gtsam/linear/HessianFactor.cpp

@@ -38,7 +38,7 @@ using namespace std;
 namespace gtsam {
 
 // Typedefs used in constructors below.
-using Dims = std::vector<Eigen::Index>;
+using Dims = std::vector<Key>;
 
 /* ************************************************************************* */
 void HessianFactor::Allocate(const Scatter& scatter) {
@@ -72,7 +72,7 @@ HessianFactor::HessianFactor() :
 
 /* ************************************************************************* */
 HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f)
-    : GaussianFactor(KeyVector{j}), info_(Dims{G.cols(), 1}) {
+    : GaussianFactor(KeyVector{j}), info_(Dims{static_cast<Key>(G.cols()), 1}) {
   if (G.rows() != G.cols() || G.rows() != g.size())
     throw invalid_argument(
         "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
@@ -85,7 +85,7 @@ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f)
 // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
 // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
 HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma)
-    : GaussianFactor(KeyVector{j}), info_(Dims{Sigma.cols(), 1}) {
+    : GaussianFactor(KeyVector{j}), info_(Dims{static_cast<Key>(Sigma.cols()), 1}) {
   if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size())
     throw invalid_argument(
         "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
@@ -99,7 +99,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11,
     const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2,
     double f) :
     GaussianFactor(KeyVector{j1,j2}), info_(
-        Dims{G11.cols(),G22.cols(),1}) {
+        Dims{static_cast<Key>(G11.cols()),static_cast<Key>(G22.cols()),1}) {
   info_.setDiagonalBlock(0, G11);
   info_.setOffDiagonalBlock(0, 1, G12);
   info_.setDiagonalBlock(1, G22);
@@ -113,7 +113,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11,
     const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3,
     double f) :
     GaussianFactor(KeyVector{j1,j2,j3}), info_(
-        Dims{G11.cols(),G22.cols(),G33.cols(),1}) {
+        Dims{static_cast<Key>(G11.cols()),static_cast<Key>(G22.cols()),static_cast<Key>(G33.cols()),1}) {
   if (G11.rows() != G11.cols() || G11.rows() != G12.rows()
       || G11.rows() != G13.rows() || G11.rows() != g1.size()
       || G22.cols() != G12.cols() || G33.cols() != G13.cols()

gtsam/linear/JacobianFactor.cpp

@@ -40,8 +40,8 @@ using namespace std;
 namespace gtsam {
 
 // Typedefs used in constructors below.
-using Dims = std::vector<Eigen::Index>;
-using Pairs = std::vector<std::pair<Eigen::Index, Matrix>>;
+using Dims = std::vector<Key>;
+using Pairs = std::vector<std::pair<Key, Matrix>>;
 
 /* ************************************************************************* */
 JacobianFactor::JacobianFactor() :

gtsam/linear/VectorValues.cpp

@@ -64,6 +64,13 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************ */
+  std::map<Key, Vector> VectorValues::sorted() const {
+    std::map<Key, Vector> ordered;
+    for (const auto& kv : *this) ordered.emplace(kv);
+    return ordered;
+  }
+
   /* ************************************************************************ */
   VectorValues VectorValues::Zero(const VectorValues& other)
   {
@@ -130,11 +137,7 @@ namespace gtsam {
   GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const VectorValues& v) {
     // Change print depending on whether we are using TBB
 #ifdef GTSAM_USE_TBB
-    std::map<Key, Vector> sorted;
-    for (const auto& [key, value] : v) {
-      sorted.emplace(key, value);
-    }
-    for (const auto& [key, value] : sorted)
+    for (const auto& [key, value] : v.sorted())
 #else
     for (const auto& [key,value] : v)
 #endif
@@ -176,7 +179,12 @@ namespace gtsam {
     // Copy vectors
     Vector result(totalDim);
     DenseIndex pos = 0;
+#ifdef GTSAM_USE_TBB
+    // TBB uses un-ordered map, so inefficiently order them:
+    for (const auto& [key, value] : sorted()) {
+#else
     for (const auto& [key, value] : *this) {
+#endif
       result.segment(pos, value.size()) = value;
       pos += value.size();
     }
@@ -392,9 +400,7 @@ namespace gtsam {
     // Print out all rows.
 #ifdef GTSAM_USE_TBB
     // TBB uses un-ordered map, so inefficiently order them:
-    std::map<Key, Vector> ordered;
-    for (const auto& kv : *this) ordered.emplace(kv);
-    for (const auto& kv : ordered) {
+    for (const auto& kv : sorted()) {
 #else
     for (const auto& kv : *this) {
 #endif

gtsam/linear/VectorValues.h

@@ -77,6 +77,9 @@ namespace gtsam {
     typedef ConcurrentMap<Key, Vector> Values;  ///< Collection of Vectors making up a VectorValues
     Values values_;                             ///< Vectors making up this VectorValues
 
+    /** Sort by key (primarily for use with TBB, which uses an unordered map)*/
+    std::map<Key, Vector> sorted() const;
+
    public:
     typedef Values::iterator iterator;              ///< Iterator over vector values
     typedef Values::const_iterator const_iterator;  ///< Const iterator over vector values

gtsam/linear/linear.i

@@ -128,6 +128,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
 
 virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
   Cauchy(double k);
+  Cauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
   static gtsam::noiseModel::mEstimator::Cauchy* Create(double k);
 
   // enabling serialization functionality
@@ -139,6 +140,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
 
 virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
   Tukey(double k);
+  Tukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
   static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
 
   // enabling serialization functionality
@@ -150,6 +152,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
 
 virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
   Welsch(double k);
+  Welsch(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
   static gtsam::noiseModel::mEstimator::Welsch* Create(double k);
 
   // enabling serialization functionality
@@ -161,6 +164,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
 
 virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
   GemanMcClure(double c);
+  GemanMcClure(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
   static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c);
 
   // enabling serialization functionality
@@ -172,6 +176,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
 
 virtual class DCS: gtsam::noiseModel::mEstimator::Base {
   DCS(double c);
+  DCS(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
   static gtsam::noiseModel::mEstimator::DCS* Create(double c);
 
   // enabling serialization functionality
@@ -183,6 +188,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base {
 
 virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
   L2WithDeadZone(double k);
+  L2WithDeadZone(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
   static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k);
 
   // enabling serialization functionality
@@ -193,6 +199,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
 };
 
 virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base {
+  AsymmetricTukey(double k);
   AsymmetricTukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
   static gtsam::noiseModel::mEstimator::AsymmetricTukey* Create(double k);
 
@@ -203,6 +210,18 @@ virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base {
   double loss(double error) const;
 };
 
+virtual class AsymmetricCauchy: gtsam::noiseModel::mEstimator::Base {
+  AsymmetricCauchy(double k);
+  AsymmetricCauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight);
+  static gtsam::noiseModel::mEstimator::AsymmetricCauchy* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
 virtual class Custom: gtsam::noiseModel::mEstimator::Base {
   Custom(gtsam::noiseModel::mEstimator::CustomWeightFunction weight,
          gtsam::noiseModel::mEstimator::CustomLossFunction loss,
@@ -356,6 +375,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   void serialize() const;
 };
 
+pair<gtsam::GaussianConditional, gtsam::JacobianFactor*> EliminateQR(const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys);
+
 #include <gtsam/linear/HessianFactor.h>
 virtual class HessianFactor : gtsam::GaussianFactor {
   //Constructors

gtsam/navigation/PreintegrationBase.cpp

@@ -129,7 +129,7 @@ NavState PreintegrationBase::predict(const NavState& state_i,
   Matrix9 D_predict_state, D_predict_delta;
   NavState state_j = state_i.retract(xi,
                                      H1 ? &D_predict_state : nullptr,
-                                     H2 || H2 ? &D_predict_delta : nullptr);
+                                     H1 || H2 ? &D_predict_delta : nullptr);
   if (H1)
     *H1 = D_predict_state + D_predict_delta * D_delta_state;
   if (H2)

gtsam/navigation/PreintegrationCombinedParams.h

@@ -25,7 +25,6 @@
 /* GTSAM includes */
 #include <gtsam/base/Matrix.h>
 #include <gtsam/navigation/ManifoldPreintegration.h>
-#include <gtsam/navigation/PreintegrationCombinedParams.h>
 #include <gtsam/navigation/TangentPreintegration.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 

gtsam/navigation/navigation.i

@@ -200,7 +200,7 @@ class PreintegratedCombinedMeasurements {
       const gtsam::imuBias::ConstantBias& bias) const;
 };
 
-virtual class CombinedImuFactor: gtsam::NonlinearFactor {
+virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
   CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
       size_t bias_i, size_t bias_j,
       const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements);

gtsam/nonlinear/nonlinear.i

@@ -130,6 +130,7 @@ virtual class NonlinearFactor : gtsam::Factor {
 virtual class NoiseModelFactor : gtsam::NonlinearFactor {
   bool equals(const gtsam::NoiseModelFactor& other, double tol) const;
   gtsam::noiseModel::Base* noiseModel() const;
+  gtsam::NoiseModelFactor* cloneWithNewNoiseModel(gtsam::noiseModel::Base* newNoise) const;
   Vector unwhitenedError(const gtsam::Values& x) const;
   Vector whitenedError(const gtsam::Values& x) const;
 };
@@ -320,6 +321,8 @@ virtual class GncParams {
   enum Verbosity {
     SILENT,
     SUMMARY,
+    MU,
+    WEIGHTS,
     VALUES
   };
 };

gtsam/slam/StereoFactor.h

@@ -169,6 +169,11 @@ class GenericStereoFactor: public NoiseModelFactorN<POSE, LANDMARK> {
   /** return flag for throwing cheirality exceptions */
   inline bool throwCheirality() const { return throwCheirality_; }
 
+  /** return the (optional) sensor pose with respect to the vehicle frame */
+  const std::optional<POSE>& body_P_sensor() const {
+    return body_P_sensor_;
+  }
+
 private:
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */

gtsam/slam/slam.i

@@ -171,6 +171,20 @@ virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
   GenericStereoFactor(const gtsam::StereoPoint2& measured,
                       const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
                       size_t landmarkKey, const gtsam::Cal3_S2Stereo* K);
+  GenericStereoFactor(const gtsam::StereoPoint2& measured,
+                      const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
+                      size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
+                      POSE body_P_sensor);
+
+  GenericStereoFactor(const gtsam::StereoPoint2& measured,
+                      const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
+                      size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
+                      bool throwCheirality, bool verboseCheirality);
+  GenericStereoFactor(const gtsam::StereoPoint2& measured,
+                      const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
+                      size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
+                      bool throwCheirality, bool verboseCheirality,
+                      POSE body_P_sensor);
   gtsam::StereoPoint2 measured() const;
   gtsam::Cal3_S2Stereo* calibration() const;