12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'borglab:develop' into develop

release/4.3a0
JaiXJM-BB 2025-01-10 13:51:05 -05:00 committed by GitHub
parent 20d72d0639 0c44f9f451
commit 074f8896a2
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
8 changed files with 80 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/hybrid/tests/testHybridBayesNet.cpp
View File

@ -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
// 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);
// 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)));
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)));
}
/* ****************************************************************************/

2
gtsam/hybrid/tests/testHybridMotionModel.cpp
View File

@ -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);

12
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -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
}
/* ************************************************************************* */

8
gtsam/linear/tests/testGaussianConditional.cpp
View File

@ -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
}
/* ************************************************************************* */

10
gtsam/navigation/CombinedImuFactor.h
View File

@ -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 {}

9
gtsam/navigation/ImuFactor.h
View File

@ -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.

16
gtsam/navigation/navigation.i
View File

@ -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);

27
gtsam/sfm/tests/testShonanAveraging.cpp
View File

@ -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<SOn>(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<SOn>(0), 1e-5));
}
/* ************************************************************************* */