Merge branch 'borglab:develop' into develop

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -34,6 +34,7 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <memory>
+#include <numeric>
 
 using namespace std;
 using namespace gtsam;
@@ -552,19 +553,23 @@ TEST(HybridBayesNet, Sampling) {
   EXPECT_LONGS_EQUAL(2, average_continuous.size());
   EXPECT_LONGS_EQUAL(num_samples, discrete_samples.size());
 
-  // Regressions don't work across platforms :-(
+  // regression for specific RNG seed
-  // // regression for specific RNG seed
+  double discrete_sum =
-  // double discrete_sum =
+      std::accumulate(discrete_samples.begin(), discrete_samples.end(),
-  //     std::accumulate(discrete_samples.begin(), discrete_samples.end(),
+                      decltype(discrete_samples)::value_type(0));
-  //                     decltype(discrete_samples)::value_type(0));
+  EXPECT_DOUBLES_EQUAL(0.477, discrete_sum / num_samples, 1e-9);
-  // EXPECT_DOUBLES_EQUAL(0.477, discrete_sum / num_samples, 1e-9);
 
-  // VectorValues expected;
+  VectorValues expected;
-  // expected.insert({X(0), Vector1(-0.0131207162712)});
+  // regression for specific RNG seed
-  // expected.insert({X(1), Vector1(-0.499026377568)});
+#if __APPLE__ || _WIN32
-  // // regression for specific RNG seed
+  expected.insert({X(0), Vector1(-0.0131207162712)});
-  // EXPECT(assert_equal(expected, average_continuous.scale(1.0 /
+  expected.insert({X(1), Vector1(-0.499026377568)});
-  // num_samples)));
+#elif __linux__
+  expected.insert({X(0), Vector1(-0.00799425182219)});
+  expected.insert({X(1), Vector1(-0.526463854268)});
+#endif
+
+  EXPECT(assert_equal(expected, average_continuous.scale(1.0 / num_samples)));
 }
 
 /* ****************************************************************************/

gtsam/hybrid/tests/testHybridMotionModel.cpp

@@ -101,7 +101,7 @@ std::pair<double, double> approximateDiscreteMarginal(
   // Do importance sampling
   double w0 = 0.0, w1 = 0.0;
   std::mt19937_64 rng(42);
-  for (int i = 0; i < N; i++) {
+  for (size_t i = 0; i < N; i++) {
     HybridValues sample = q.sample(&rng);
     sample.insert(given);
     double weight = hbn.evaluate(sample) / q.evaluate(sample);

gtsam/linear/tests/testGaussianBayesNet.cpp

@@ -186,9 +186,15 @@ TEST(GaussianBayesNet, sample) {
   std::mt19937_64 rng(4242);
   auto actual3 = gbn.sample(&rng);
   EXPECT_LONGS_EQUAL(2, actual.size());
-  // regression is not repeatable across platforms/versions :-(
+
-  // EXPECT(assert_equal(Vector2(20.0129382, 40.0039798), actual[X(1)], 1e-5));
+  // regressions
-  // EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5));
+#if __APPLE__ || _WIN32
+  EXPECT(assert_equal(Vector2(20.0129382, 40.0039798), actual[X(1)], 1e-5));
+  EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5));
+#elif __linux__
+  EXPECT(assert_equal(Vector2(20.0070499, 39.9942591), actual[X(1)], 1e-5));
+  EXPECT(assert_equal(Vector2(109.976501, 229.990945), actual[X(0)], 1e-5));
+#endif
 }
 
 /* ************************************************************************* */

gtsam/linear/tests/testGaussianConditional.cpp

@@ -464,8 +464,12 @@ TEST(GaussianConditional, sample) {
   std::mt19937_64 rng(4242);
   auto actual3 = conditional.sample(given, &rng);
   EXPECT_LONGS_EQUAL(1, actual2.size());
-  // regression is not repeatable across platforms/versions :-(
+  // regressions
-  // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5));
+#if __APPLE__ || _WIN32
+  EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5));
+#elif __linux__
+  EXPECT(assert_equal(Vector2(30.9809331, 64.9927588), actual2[X(0)], 1e-5));
+#endif
 }
 
 /* ************************************************************************* */

gtsam/navigation/CombinedImuFactor.h

@@ -84,7 +84,7 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements
   /// @{
 
   /// Default constructor only for serialization and wrappers
-  PreintegratedCombinedMeasurements() { preintMeasCov_.setZero(); }
+  PreintegratedCombinedMeasurements() { resetIntegration(); }
 
   /**
    *  Default constructor, initializes the class with no measurements
@@ -97,7 +97,9 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements
       const imuBias::ConstantBias& biasHat = imuBias::ConstantBias(),
       const Eigen::Matrix<double, 15, 15>& preintMeasCov =
           Eigen::Matrix<double, 15, 15>::Zero())
-      : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {}
+      : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {
+    PreintegrationType::resetIntegration();
+  }
 
   /**
    *  Construct preintegrated directly from members: base class and
@@ -108,7 +110,9 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements
   PreintegratedCombinedMeasurements(
       const PreintegrationType& base,
       const Eigen::Matrix<double, 15, 15>& preintMeasCov)
-      : PreintegrationType(base), preintMeasCov_(preintMeasCov) {}
+      : PreintegrationType(base), preintMeasCov_(preintMeasCov) {
+    PreintegrationType::resetIntegration();
+  }
 
   /// Virtual destructor
   ~PreintegratedCombinedMeasurements() override {}

gtsam/navigation/ImuFactor.h

@@ -79,7 +79,7 @@ public:
 
   /// Default constructor for serialization and wrappers
   PreintegratedImuMeasurements() {
-    preintMeasCov_.setZero();
+    resetIntegration();
   }
 
  /**
@@ -90,7 +90,7 @@ public:
   PreintegratedImuMeasurements(const std::shared_ptr<PreintegrationParams>& p,
       const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
       PreintegrationType(p, biasHat) {
-    preintMeasCov_.setZero();
+    resetIntegration();
   }
 
 /**
@@ -101,6 +101,7 @@ public:
   PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
      : PreintegrationType(base),
        preintMeasCov_(preintMeasCov) {
+    PreintegrationType::resetIntegration();
   }
 
   /// Virtual destructor
@@ -113,7 +114,7 @@ public:
   /// equals
   bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
 
-  /// Re-initialize PreintegratedIMUMeasurements
+  /// Re-initialize PreintegratedImuMeasurements
   void resetIntegration() override;
 
   /**
@@ -159,7 +160,7 @@ public:
  * the vehicle at previous time step), current state (pose and velocity at
  * current time step), and the bias estimate. Following the preintegration
  * scheme proposed in [2], the ImuFactor includes many IMU measurements, which
- * are "summarized" using the PreintegratedIMUMeasurements class.
+ * are "summarized" using the PreintegratedImuMeasurements class.
  * Note that this factor does not model "temporal consistency" of the biases
  * (which are usually slowly varying quantities), which is up to the caller.
  * See also CombinedImuFactor for a class that does this for you.

gtsam/navigation/navigation.i

@@ -156,6 +156,22 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
   void serialize() const;
 };
 
+virtual class ImuFactor2: gtsam::NonlinearFactor {
+  ImuFactor2();
+  ImuFactor2(size_t state_i, size_t state_j,
+      size_t bias,
+      const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements);
+
+  // Standard Interface
+  gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
+  gtsam::Vector evaluateError(const gtsam::NavState& state_i,
+                              gtsam::NavState& state_j,
+                              const gtsam::imuBias::ConstantBias& bias_i);
+
+  // enable serialization functionality
+  void serialize() const;
+};
+
 #include <gtsam/navigation/CombinedImuFactor.h>
 virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
   PreintegrationCombinedParams(gtsam::Vector n_gravity);

gtsam/sfm/tests/testShonanAveraging.cpp

@@ -205,14 +205,25 @@ TEST(ShonanAveraging3, CheckWithEigen) {
       ShonanAveraging3::LiftwithDescent(4, Qstar3, descentDirection);
   EXPECT_LONGS_EQUAL(5, initialQ4.size());
 
-  // TODO(frank): uncomment this regression test: currently not repeatable
+  Matrix expected(4, 4);
-  // across platforms.
+#if __APPLE__
-  // Matrix expected(4, 4);
+  expected << 0.145767, -0.938445, 0.135713, -0.282233,  //
-  // expected << 0.0459224, -0.688689, -0.216922, 0.690321, //
+      0.780348, -0.0104323, 0.266238, 0.565743,          //
-  //     0.92381, 0.191931, 0.255854, 0.21042,              //
+      -0.383624, 0.0434887, 0.917211, 0.0983088,         //
-  //     -0.376669, 0.301589, 0.687953, 0.542111,           //
+      -0.471849, -0.342523, -0.263482, 0.768514;
-  //     -0.0508588, 0.630804, -0.643587, 0.43046;
+#elif __linux__
-  // EXPECT(assert_equal(SOn(expected), initialQ4.at<SOn>(0), 1e-5));
+  expected << 0.100724, -0.987231, 0.104092, 0.0662867,  //
+      0.571527, 0.0292782, 0.226546, -0.788147,          //
+      -0.349294, 0.064102, 0.93465, 0.0177471,           //
+      0.735667, 0.142857, 0.253519, 0.611649;
+#elif _WIN32
+  expected << 0.0825862, -0.645931, 0.271896, 0.708537,  //
+      0.927285, -0.0156335, 0.291603, -0.234236,         //
+      -0.36419, -0.132115, 0.831933, -0.39724,           //
+      0.0262425, 0.751715, 0.385912, 0.534143;
+#endif
+
+  EXPECT(assert_equal(SOn(expected), initialQ4.at<SOn>(0), 1e-5));
 }
 
 /* ************************************************************************* */