diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 989694b269..784d9c95f6 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -34,6 +34,7 @@ #include #include +#include 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 - // double discrete_sum = - // std::accumulate(discrete_samples.begin(), discrete_samples.end(), - // decltype(discrete_samples)::value_type(0)); - // EXPECT_DOUBLES_EQUAL(0.477, discrete_sum / num_samples, 1e-9); - - // VectorValues expected; - // expected.insert({X(0), Vector1(-0.0131207162712)}); - // expected.insert({X(1), Vector1(-0.499026377568)}); - // // regression for specific RNG seed - // EXPECT(assert_equal(expected, average_continuous.scale(1.0 / - // num_samples))); + // regression for specific RNG seed + double discrete_sum = + std::accumulate(discrete_samples.begin(), discrete_samples.end(), + decltype(discrete_samples)::value_type(0)); + EXPECT_DOUBLES_EQUAL(0.477, discrete_sum / num_samples, 1e-9); + + VectorValues expected; + // regression for specific RNG seed +#if __APPLE__ || _WIN32 + expected.insert({X(0), Vector1(-0.0131207162712)}); + expected.insert({X(1), Vector1(-0.499026377568)}); +#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))); } /* ****************************************************************************/ diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp index a4de6a17bd..387094c479 100644 --- a/gtsam/hybrid/tests/testHybridMotionModel.cpp +++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp @@ -101,7 +101,7 @@ std::pair 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); diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index b1cfbe12ca..587597cf14 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/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)); - // EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5)); + + // regressions +#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 } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 68894c3142..0252368eae 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/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 :-( - // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5)); + // regressions +#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 } /* ************************************************************************* */ diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index b4c3ae8dc7..87444e624f 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/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& preintMeasCov = Eigen::Matrix::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& preintMeasCov) - : PreintegrationType(base), preintMeasCov_(preintMeasCov) {} + : PreintegrationType(base), preintMeasCov_(preintMeasCov) { + PreintegrationType::resetIntegration(); + } /// Virtual destructor ~PreintegratedCombinedMeasurements() override {} diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b19989a77a..2e6dff0dc8 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -79,7 +79,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType { /// Default constructor for serialization and wrappers PreintegratedImuMeasurements() { - preintMeasCov_.setZero(); + resetIntegration(); } /** @@ -90,7 +90,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType { PreintegratedImuMeasurements(const std::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : PreintegrationType(p, biasHat) { - preintMeasCov_.setZero(); + resetIntegration(); } /** @@ -101,6 +101,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType { PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov) : PreintegrationType(base), preintMeasCov_(preintMeasCov) { + PreintegrationType::resetIntegration(); } /// Virtual destructor @@ -113,7 +114,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType { /// equals bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const; - /// Re-initialize PreintegratedIMUMeasurements + /// Re-initialize PreintegratedImuMeasurements void resetIntegration() override; /** @@ -159,7 +160,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType { * 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. diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index adb8bf2bb0..ceeab3b355 100644 --- a/gtsam/navigation/navigation.i +++ b/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 virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { PreintegrationCombinedParams(gtsam::Vector n_gravity); diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index dfa725ab6d..e297678b8e 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/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 - // across platforms. - // Matrix expected(4, 4); - // expected << 0.0459224, -0.688689, -0.216922, 0.690321, // - // 0.92381, 0.191931, 0.255854, 0.21042, // - // -0.376669, 0.301589, 0.687953, 0.542111, // - // -0.0508588, 0.630804, -0.643587, 0.43046; - // EXPECT(assert_equal(SOn(expected), initialQ4.at(0), 1e-5)); + Matrix expected(4, 4); +#if __APPLE__ + expected << 0.145767, -0.938445, 0.135713, -0.282233, // + 0.780348, -0.0104323, 0.266238, 0.565743, // + -0.383624, 0.0434887, 0.917211, 0.0983088, // + -0.471849, -0.342523, -0.263482, 0.768514; +#elif __linux__ + 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(0), 1e-5)); } /* ************************************************************************* */