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

add test for checking covariances between ImuFactor and CombinedImuFactor

release/4.3a0
Varun Agrawal 2021-09-19 10:51:24 -04:00
parent 61f2cf7fef
commit 28d0393abd
1 changed files with 230 additions and 165 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

395
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -18,16 +18,16 @@
* @author Stephen Williams * @author Stephen Williams
*/ */
#include <gtsam/navigation/ImuFactor.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/ScenarioRunner.h>
#include <gtsam/nonlinear/Values.h>
#include <list> #include <list>
@ -44,207 +44,272 @@ static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
} }
} }
/* ************************************************************************* */ // /* ************************************************************************* */
TEST( CombinedImuFactor, PreintegratedMeasurements ) { // TEST(CombinedImuFactor, PreintegratedMeasurements ) {
// Linearization point // // Linearization point
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases // Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
// Measurements // // Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0); // Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); // Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
double deltaT = 0.5; // double deltaT = 0.5;
double tol = 1e-6; // double tol = 1e-6;
auto p = testing::Params(); // auto p = testing::Params();
// Actual preintegrated values // // Actual preintegrated values
PreintegratedImuMeasurements expected1(p, bias); // PreintegratedImuMeasurements expected1(p, bias);
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
PreintegratedCombinedMeasurements actual1(p, bias); // PreintegratedCombinedMeasurements actual1(p, bias);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol)); // EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol));
EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol)); // EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol));
EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol)); // EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol));
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); // DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
} // }
/* ************************************************************************* */ // /* ************************************************************************* */
TEST( CombinedImuFactor, ErrorWithBiases ) { // TEST(CombinedImuFactor, Accelerating) {
Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) // const double a = 0.2, v = 50;
Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
Vector3 v1(0.5, 0.0, 0.0);
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
Vector3 v2(0.5, 0.0, 0.0);
auto p = testing::Params(); // // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X
p->omegaCoriolis = Vector3(0,0.1,0.1); // // The body itself has Z axis pointing down
PreintegratedImuMeasurements pim( // const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); // const Point3 initial_position(10, 20, 0);
// const Vector3 initial_velocity(v, 0, 0);
// Measurements // const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
Vector3 measuredOmega; // Vector3(a, 0, 0));
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc =
x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
double tol = 1e-6;
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // const double T = 3.0; // seconds
// CombinedScenarioRunner runner(scenario, testing::Params(), T / 10);
PreintegratedCombinedMeasurements combined_pim(p, // PreintegratedCombinedMeasurements pim = runner.integrate(T);
Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); // EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // auto estimatedCov = runner.estimateCovariance(T, 100);
// Eigen::Matrix<double, 15, 15> expected = pim.preintMeasCov();
// // EXPECT(assert_equal(estimatedCov, expected, 0.1));
// }
// Create factor // /* ************************************************************************* */
ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim); // TEST(CombinedImuFactor, ErrorWithBiases ) {
// Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
// Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
// Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
// Vector3 v1(0.5, 0.0, 0.0);
// Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
// Point3(5.5, 1.0, -50.0));
// Vector3 v2(0.5, 0.0, 0.0);
noiseModel::Gaussian::shared_ptr Combinedmodel = // auto p = testing::Params();
noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov()); // p->omegaCoriolis = Vector3(0,0.1,0.1);
CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), // PreintegratedImuMeasurements pim(
combined_pim); // p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias); // // Measurements
Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias, // Vector3 measuredOmega;
bias2); // measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); // Vector3 measuredAcc =
// x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0);
// double deltaT = 1.0;
// double tol = 1e-6;
// Expected Jacobians // pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
Matrix H1e, H2e, H3e, H4e, H5e;
(void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
// Actual Jacobians // PreintegratedCombinedMeasurements combined_pim(p,
Matrix H1a, H2a, H3a, H4a, H5a, H6a; // Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
(void) combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a,
H3a, H4a, H5a, H6a);
EXPECT(assert_equal(H1e, H1a.topRows(9))); // combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(H2e, H2a.topRows(9)));
EXPECT(assert_equal(H3e, H3a.topRows(9)));
EXPECT(assert_equal(H4e, H4a.topRows(9)));
EXPECT(assert_equal(H5e, H5a.topRows(9)));
}
/* ************************************************************************* */ // // Create factor
#ifdef GTSAM_TANGENT_PREINTEGRATION // ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim);
TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
auto p = testing::Params();
testing::SomeMeasurements measurements;
auto preintegrated = [=](const Vector3& a, const Vector3& w) { // noiseModel::Gaussian::shared_ptr Combinedmodel =
PreintegratedImuMeasurements pim(p, Bias(a, w)); // noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov());
testing::integrateMeasurements(measurements, &pim); // CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
return pim.preintegrated(); // combined_pim);
};
// Actual pre-integrated values // Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias);
PreintegratedCombinedMeasurements pim(p); // Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias,
testing::integrateMeasurements(measurements, &pim); // bias2);
// EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
EXPECT(assert_equal(numericalDerivative21<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1), // // Expected Jacobians
pim.preintegrated_H_biasAcc())); // Matrix H1e, H2e, H3e, H4e, H5e;
EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1), // (void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
pim.preintegrated_H_biasOmega(), 1e-3));
}
#endif
/* ************************************************************************* */ // // Actual Jacobians
TEST(CombinedImuFactor, PredictPositionAndVelocity) { // Matrix H1a, H2a, H3a, H4a, H5a, H6a;
const Bias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) // (void) combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a,
// H3a, H4a, H5a, H6a);
auto p = testing::Params(); // EXPECT(assert_equal(H1e, H1a.topRows(9)));
// EXPECT(assert_equal(H2e, H2a.topRows(9)));
// EXPECT(assert_equal(H3e, H3a.topRows(9)));
// EXPECT(assert_equal(H4e, H4a.topRows(9)));
// EXPECT(assert_equal(H5e, H5a.topRows(9)));
// }
// Measurements // /* ************************************************************************* */
const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3; // #ifdef GTSAM_TANGENT_PREINTEGRATION
const Vector3 measuredAcc(0, 1.1, -kGravity); // TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
const double deltaT = 0.01; // auto p = testing::Params();
// testing::SomeMeasurements measurements;
PreintegratedCombinedMeasurements pim(p, bias); // auto preintegrated = [=](const Vector3& a, const Vector3& w) {
// PreintegratedImuMeasurements pim(p, Bias(a, w));
// testing::integrateMeasurements(measurements, &pim);
// return pim.preintegrated();
// };
for (int i = 0; i < 100; ++i) // // Actual pre-integrated values
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // PreintegratedCombinedMeasurements pim(p);
// testing::integrateMeasurements(measurements, &pim);
// Create factor // EXPECT(assert_equal(numericalDerivative21<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
const noiseModel::Gaussian::shared_ptr combinedmodel = // pim.preintegrated_H_biasAcc()));
noiseModel::Gaussian::Covariance(pim.preintMeasCov()); // EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); // pim.preintegrated_H_biasOmega(), 1e-3));
// }
// #endif
// Predict // /* ************************************************************************* */
const NavState actual = pim.predict(NavState(), bias); // TEST(CombinedImuFactor, PredictPositionAndVelocity) {
const Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); // const Bias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
const Vector3 expectedVelocity(0, 1, 0);
EXPECT(assert_equal(expectedPose, actual.pose()));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity())));
}
/* ************************************************************************* */ // auto p = testing::Params();
TEST(CombinedImuFactor, PredictRotation) {
const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
auto p = testing::Params();
PreintegratedCombinedMeasurements pim(p, bias);
const Vector3 measuredAcc = - kGravityAlongNavZDown;
const Vector3 measuredOmega(0, 0, M_PI / 10.0);
const double deltaT = 0.01;
const double tol = 1e-4;
for (int i = 0; i < 100; ++i)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
// Predict // // Measurements
const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2; // const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3;
const Vector3 v(0, 0, 0), v2; // const Vector3 measuredAcc(0, 1.1, -kGravity);
const NavState actual = pim.predict(NavState(x, v), bias); // const double deltaT = 0.01;
const Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expectedPose, actual.pose(), tol));
}
/* ************************************************************************* */ // PreintegratedCombinedMeasurements pim(p, bias);
// Testing covariance to check if all the jacobians are accounted for.
TEST(CombinedImuFactor, CheckCovariance) { // for (int i = 0; i < 100; ++i)
auto params = PreintegrationCombinedParams::MakeSharedU(9.81); // pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// // Create factor
// const noiseModel::Gaussian::shared_ptr combinedmodel =
// noiseModel::Gaussian::Covariance(pim.preintMeasCov());
// const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
// // Predict
// const NavState actual = pim.predict(NavState(), bias);
// const Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
// const Vector3 expectedVelocity(0, 1, 0);
// EXPECT(assert_equal(expectedPose, actual.pose()));
// EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity())));
// }
// /* ************************************************************************* */
// TEST(CombinedImuFactor, PredictRotation) {
// const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// auto p = testing::Params();
// PreintegratedCombinedMeasurements pim(p, bias);
// const Vector3 measuredAcc = - kGravityAlongNavZDown;
// const Vector3 measuredOmega(0, 0, M_PI / 10.0);
// const double deltaT = 0.01;
// const double tol = 1e-4;
// for (int i = 0; i < 100; ++i)
// pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
// // Predict
// const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2;
// const Vector3 v(0, 0, 0), v2;
// const NavState actual = pim.predict(NavState(x, v), bias);
// const Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
// EXPECT(assert_equal(expectedPose, actual.pose(), tol));
// }
// /* ************************************************************************* */
// // Testing covariance to check if all the jacobians are accounted for.
// TEST(CombinedImuFactor, CheckCovariance) {
// auto params = PreintegrationCombinedParams::MakeSharedU(9.81);
// params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
// params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
// params->setIntegrationCovariance(pow(0.0, 2) * I_3x3);
// params->setOmegaCoriolis(Vector3::Zero());
// imuBias::ConstantBias currentBias;
// PreintegratedCombinedMeasurements actual(params, currentBias);
// // Measurements
// Vector3 measuredAcc(0.1577, -0.8251, 9.6111);
// Vector3 measuredOmega(-0.0210, 0.0311, 0.0145);
// double deltaT = 0.01;
// actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Eigen::Matrix<double, 15, 15> expected;
// expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
// 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
// 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
// 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
// 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
// 0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
// 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, //
// 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, //
// 0, 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, //
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, //
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, //
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, //
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, //
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, //
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
// // regression
// EXPECT(assert_equal(expected, actual.preintMeasCov()));
// }
TEST(CombinedImuFactor, SameCovarianceCombined) {
auto params = PreintegrationCombinedParams::MakeSharedU();
params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3); params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3); params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
params->setIntegrationCovariance(pow(0.0, 2) * I_3x3); params->setIntegrationCovariance(pow(0, 2) * I_3x3);
params->setOmegaCoriolis(Vector3::Zero()); params->setOmegaCoriolis(Vector3::Zero());
imuBias::ConstantBias currentBias; imuBias::ConstantBias currentBias;
PreintegratedCombinedMeasurements actual(params, currentBias); PreintegratedCombinedMeasurements pim(params, currentBias);
// Measurements Vector3 accMeas(0.1577, -0.8251, 9.6111);
Vector3 measuredAcc(0.1577, -0.8251, 9.6111); Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
Vector3 measuredOmega(-0.0210, 0.0311, 0.0145);
double deltaT = 0.01; double deltaT = 0.01;
pim.integrateMeasurement(accMeas, omegaMeas, deltaT);
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); std::cout << pim.preintMeasCov() << std::endl << std::endl;
Eigen::Matrix<double, 15, 15> expected;
expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
// regression
EXPECT(assert_equal(expected, actual.preintMeasCov()));
} }
TEST(CombinedImuFactor, SameCovariance) {
auto params = PreintegrationParams::MakeSharedU();
params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
params->setIntegrationCovariance(pow(0, 2) * I_3x3);
params->setOmegaCoriolis(Vector3::Zero());
imuBias::ConstantBias currentBias;
PreintegratedImuMeasurements pim(params, currentBias);
Vector3 accMeas(0.1577, -0.8251, 9.6111);
Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
double deltaT = 0.01;
pim.integrateMeasurement(accMeas, omegaMeas, deltaT);
std::cout << pim.preintMeasCov() << std::endl << std::endl;
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;