More common parameters, realistic noise parameters
Frank Dellaert
parent
0d07e8764d
commit
2fe803a62e
|
|
@ -35,6 +35,15 @@ static const Bias kZeroBiasHat, kZeroBias;
|
|||
static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
|
||||
static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
|
||||
|
||||
static const double kGravity = 10;
|
||||
static const Vector3 kGravityAlongNavZDown(0, 0, kGravity);
|
||||
|
||||
// Realistic MEMS white noise characteristics. Angular and velocity random walk
|
||||
// expressed in degrees respectively m/s per sqrt(hr).
|
||||
auto radians = [](double t) { return t * M_PI / 180; };
|
||||
static const double kGyroSigma = radians(0.5) / 60; // 0.5 degree ARW
|
||||
static const double kAccelSigma = 0.1 / 60; // 10 cm VRW
|
||||
|
||||
namespace testing {
|
||||
|
||||
struct ImuMeasurement {
|
||||
|
|
|
|||
|
|
@ -34,10 +34,21 @@
|
|||
|
||||
#include "imuFactorTesting.h"
|
||||
|
||||
namespace testing {
|
||||
// Create default parameters with Z-down and above noise parameters
|
||||
static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity);
|
||||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||
p->integrationCovariance = 0.0001 * I_3x3;
|
||||
return p;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( CombinedImuFactor, PreintegratedMeasurements ) {
|
||||
// Linearization point
|
||||
imuBias::ConstantBias 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
|
||||
Vector3 measuredAcc(0.1, 0.0, 0.0);
|
||||
|
|
@ -45,7 +56,7 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
|
|||
double deltaT = 0.5;
|
||||
double tol = 1e-6;
|
||||
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
|
||||
auto p = testing::Params();
|
||||
|
||||
// Actual preintegrated values
|
||||
PreintegratedImuMeasurements expected1(p, bias);
|
||||
|
|
@ -63,18 +74,18 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST( CombinedImuFactor, ErrorWithBiases ) {
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
|
||||
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);
|
||||
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
|
||||
auto p = testing::Params();
|
||||
p->omegaCoriolis = Vector3(0,0.1,0.1);
|
||||
PreintegratedImuMeasurements pim(
|
||||
p, imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||
p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega;
|
||||
|
|
@ -87,7 +98,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
|
|||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
PreintegratedCombinedMeasurements combined_pim(p,
|
||||
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||
Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||
|
||||
combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
|
|
@ -122,7 +133,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
|
||||
auto p = testing::Params();
|
||||
testing::SomeMeasurements measurements;
|
||||
|
||||
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
|
||||
|
|
@ -144,16 +155,14 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(CombinedImuFactor, PredictPositionAndVelocity) {
|
||||
imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
|
||||
const Bias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
|
||||
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
|
||||
auto p = testing::Params();
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3;
|
||||
Vector3 measuredAcc;
|
||||
measuredAcc << 0, 1.1, -9.81;
|
||||
double deltaT = 0.001;
|
||||
const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3;
|
||||
const Vector3 measuredAcc(0, 1.1, -kGravity);
|
||||
const double deltaT = 0.001;
|
||||
|
||||
PreintegratedCombinedMeasurements pim(p, bias);
|
||||
|
||||
|
|
@ -161,40 +170,36 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) {
|
|||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
noiseModel::Gaussian::shared_ptr combinedmodel =
|
||||
const noiseModel::Gaussian::shared_ptr combinedmodel =
|
||||
noiseModel::Gaussian::Covariance(pim.preintMeasCov());
|
||||
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
|
||||
const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
|
||||
|
||||
// Predict
|
||||
NavState actual = pim.predict(NavState(), bias);
|
||||
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
|
||||
Vector3 expectedVelocity;
|
||||
expectedVelocity << 0, 1, 0;
|
||||
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())));
|
||||
EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(CombinedImuFactor, PredictRotation) {
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
|
||||
const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
auto p = testing::Params();
|
||||
PreintegratedCombinedMeasurements pim(p, bias);
|
||||
Vector3 measuredAcc;
|
||||
measuredAcc << 0, 0, -9.81;
|
||||
Vector3 measuredOmega;
|
||||
measuredOmega << 0, 0, M_PI / 10.0;
|
||||
double deltaT = 0.001;
|
||||
double tol = 1e-4;
|
||||
const Vector3 measuredAcc = - kGravityAlongNavZDown;
|
||||
const Vector3 measuredOmega(0, 0, M_PI / 10.0);
|
||||
const double deltaT = 0.001;
|
||||
const double tol = 1e-4;
|
||||
for (int i = 0; i < 1000; ++i)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
|
||||
const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
|
||||
|
||||
// Predict
|
||||
Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2;
|
||||
Vector3 v(0, 0, 0), v2;
|
||||
NavState actual = pim.predict(NavState(x, v), bias);
|
||||
Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
|
||||
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));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -33,8 +33,16 @@
|
|||
|
||||
#include "imuFactorTesting.h"
|
||||
|
||||
static const double kGravity = 10;
|
||||
static const Vector3 kGravityAlongNavZDown(0, 0, kGravity);
|
||||
namespace testing {
|
||||
// Create default parameters with Z-down and above noise parameters
|
||||
static boost::shared_ptr<PreintegrationParams> Params() {
|
||||
auto p = PreintegrationParams::MakeSharedD(kGravity);
|
||||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||
p->integrationCovariance = 0.0001 * I_3x3;
|
||||
return p;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
|
|
@ -47,21 +55,6 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
|
|||
factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3));
|
||||
}
|
||||
|
||||
// Define covariance matrices
|
||||
/* ************************************************************************* */
|
||||
static const double kGyroSigma = 0.02;
|
||||
static const double kAccelerometerSigma = 0.1;
|
||||
|
||||
// Create default parameters with Z-down and above noise paramaters
|
||||
static boost::shared_ptr<PreintegrationParams> defaultParams() {
|
||||
auto p = PreintegrationParams::MakeSharedD(kGravity);
|
||||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma
|
||||
* I_3x3;
|
||||
p->integrationCovariance = 0.0001 * I_3x3;
|
||||
return p;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -78,7 +71,7 @@ TEST(ImuFactor, Accelerating) {
|
|||
Vector3(a, 0, 0));
|
||||
|
||||
const double T = 3.0; // seconds
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10);
|
||||
|
||||
PreintegratedImuMeasurements pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
|
@ -102,7 +95,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
|
|||
double expectedDeltaT1(0.5);
|
||||
|
||||
// Actual pre-integrated values
|
||||
PreintegratedImuMeasurements actual1(defaultParams());
|
||||
PreintegratedImuMeasurements actual1(testing::Params());
|
||||
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij())));
|
||||
|
|
@ -169,7 +162,7 @@ static const NavState state2(x2, v2);
|
|||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PreintegrationBaseMethods) {
|
||||
using namespace common;
|
||||
auto p = defaultParams();
|
||||
auto p = testing::Params();
|
||||
p->omegaCoriolis = Vector3(0.02, 0.03, 0.04);
|
||||
p->use2ndOrderCoriolis = true;
|
||||
|
||||
|
|
@ -203,7 +196,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
|
|||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, ErrorAndJacobians) {
|
||||
using namespace common;
|
||||
PreintegratedImuMeasurements pim(defaultParams());
|
||||
PreintegratedImuMeasurements pim(testing::Params());
|
||||
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias)));
|
||||
|
|
@ -260,7 +253,7 @@ TEST(ImuFactor, ErrorAndJacobians) {
|
|||
Matrix cov = pim.preintMeasCov();
|
||||
Matrix R = RtR(cov.inverse());
|
||||
Vector whitened = R * expectedError;
|
||||
EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-5));
|
||||
EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-4));
|
||||
|
||||
// Make sure linearization is correct
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
|
||||
|
|
@ -282,7 +275,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
|
|||
+ Vector3(0.2, 0.0, 0.0);
|
||||
double deltaT = 1.0;
|
||||
|
||||
auto p = defaultParams();
|
||||
auto p = testing::Params();
|
||||
p->omegaCoriolis = kNonZeroOmegaCoriolis;
|
||||
|
||||
Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1));
|
||||
|
|
@ -328,7 +321,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
|
|||
+ Vector3(0.2, 0.0, 0.0);
|
||||
double deltaT = 1.0;
|
||||
|
||||
auto p = defaultParams();
|
||||
auto p = testing::Params();
|
||||
p->omegaCoriolis = kNonZeroOmegaCoriolis;
|
||||
p->use2ndOrderCoriolis = true;
|
||||
|
||||
|
|
@ -438,13 +431,13 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
|
|||
|
||||
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
|
||||
[=](const Vector3& a, const Vector3& w) {
|
||||
PreintegratedImuMeasurements pim(defaultParams(), Bias(a, w));
|
||||
PreintegratedImuMeasurements pim(testing::Params(), Bias(a, w));
|
||||
testing::integrateMeasurements(measurements, &pim);
|
||||
return pim.preintegrated();
|
||||
};
|
||||
|
||||
// Actual pre-integrated values
|
||||
PreintegratedImuMeasurements pim(defaultParams());
|
||||
PreintegratedImuMeasurements pim(testing::Params());
|
||||
testing::integrateMeasurements(measurements, &pim);
|
||||
|
||||
EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1),
|
||||
|
|
@ -470,7 +463,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
const AcceleratingScenario scenario(nRb, p1, v1, a,
|
||||
Vector3(0, 0, M_PI / 10.0 + 0.3));
|
||||
|
||||
auto p = defaultParams();
|
||||
auto p = testing::Params();
|
||||
p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(0, M_PI / 2, 0)),
|
||||
Point3(0.1, 0.05, 0.01));
|
||||
p->omegaCoriolis = kNonZeroOmegaCoriolis;
|
||||
|
|
@ -562,7 +555,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
|
|||
measuredAcc << 0, 1, -kGravity;
|
||||
double deltaT = 0.001;
|
||||
|
||||
PreintegratedImuMeasurements pim(defaultParams(),
|
||||
PreintegratedImuMeasurements pim(testing::Params(),
|
||||
Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||
|
||||
for (int i = 0; i < 1000; ++i)
|
||||
|
|
@ -590,7 +583,7 @@ TEST(ImuFactor, PredictRotation) {
|
|||
measuredAcc << 0, 0, -kGravity;
|
||||
double deltaT = 0.001;
|
||||
|
||||
PreintegratedImuMeasurements pim(defaultParams(),
|
||||
PreintegratedImuMeasurements pim(testing::Params(),
|
||||
Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||
|
||||
for (int i = 0; i < 1000; ++i)
|
||||
|
|
@ -614,7 +607,7 @@ TEST(ImuFactor, PredictArbitrary) {
|
|||
Vector3(0.1, 0.2, 0), Vector3(M_PI / 10, M_PI / 10, M_PI / 10));
|
||||
|
||||
const double T = 3.0; // seconds
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10);
|
||||
//
|
||||
// PreintegratedImuMeasurements pim = runner.integrate(T);
|
||||
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
|
||||
|
|
@ -629,7 +622,7 @@ TEST(ImuFactor, PredictArbitrary) {
|
|||
Vector3 measuredOmega = runner.actualAngularVelocity(0);
|
||||
Vector3 measuredAcc = runner.actualSpecificForce(0);
|
||||
|
||||
auto p = defaultParams();
|
||||
auto p = testing::Params();
|
||||
p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
|
||||
PreintegratedImuMeasurements pim(p, biasHat);
|
||||
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
|
|
@ -662,7 +655,7 @@ TEST(ImuFactor, bodyPSensorNoBias) {
|
|||
Bias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
|
||||
|
||||
// Rotate sensor (z-down) to body (same as navigation) i.e. z-up
|
||||
auto p = defaultParams();
|
||||
auto p = testing::Params();
|
||||
p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame
|
||||
p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0));
|
||||
|
||||
|
|
@ -717,7 +710,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
// table exerts an equal and opposite force w.r.t gravity
|
||||
Vector3 measuredAcc(0, 0, -kGravity);
|
||||
|
||||
auto p = defaultParams();
|
||||
auto p = testing::Params();
|
||||
p->n_gravity = Vector3(0, 0, -kGravity);
|
||||
p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3());
|
||||
p->accelerometerCovariance = 1e-7 * I_3x3;
|
||||
|
|
@ -826,7 +819,7 @@ struct ImuFactorMergeTest {
|
|||
}
|
||||
auto actual_pim02 = ImuFactor::Merge(pim01, pim12);
|
||||
EXPECT(assert_equal(expected_pim02.preintegrated(), actual_pim02.preintegrated(), tol));
|
||||
// EXPECT(assert_equal(expected_pim02, actual_pim02, tol));
|
||||
EXPECT(assert_equal(expected_pim02, actual_pim02, tol));
|
||||
|
||||
ImuFactor::shared_ptr factor_01 =
|
||||
boost::make_shared<ImuFactor>(X(0), V(0), X(1), V(1), B(0), pim01);
|
||||
|
|
|
|||
|
|
@ -21,30 +21,28 @@
|
|||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
#include "imuFactorTesting.h"
|
||||
|
||||
static const double kDt = 0.1;
|
||||
|
||||
static const double kGyroSigma = 0.02;
|
||||
static const double kAccelSigma = 0.1;
|
||||
|
||||
// Create default parameters with Z-down and above noise parameters
|
||||
static boost::shared_ptr<PreintegrationBase::Params> defaultParams() {
|
||||
auto p = PreintegrationParams::MakeSharedD(10);
|
||||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||
p->integrationCovariance = 0.0000001 * I_3x3;
|
||||
return p;
|
||||
}
|
||||
|
||||
Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
|
||||
return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta);
|
||||
}
|
||||
|
||||
namespace testing {
|
||||
// Create default parameters with Z-down and above noise parameters
|
||||
static boost::shared_ptr<PreintegrationParams> Params() {
|
||||
auto p = PreintegrationParams::MakeSharedD(kGravity);
|
||||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||
p->integrationCovariance = 0.0001 * I_3x3;
|
||||
return p;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PreintegrationBase, UpdateEstimate1) {
|
||||
PreintegrationBase pim(defaultParams());
|
||||
PreintegrationBase pim(testing::Params());
|
||||
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
|
||||
Vector9 zeta;
|
||||
zeta.setZero();
|
||||
|
|
@ -58,7 +56,7 @@ TEST(PreintegrationBase, UpdateEstimate1) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(PreintegrationBase, UpdateEstimate2) {
|
||||
PreintegrationBase pim(defaultParams());
|
||||
PreintegrationBase pim(testing::Params());
|
||||
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
|
||||
Vector9 zeta;
|
||||
zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
|
||||
|
|
@ -73,7 +71,7 @@ TEST(PreintegrationBase, UpdateEstimate2) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(PreintegrationBase, computeError) {
|
||||
PreintegrationBase pim(defaultParams());
|
||||
PreintegrationBase pim(testing::Params());
|
||||
NavState x1, x2;
|
||||
imuBias::ConstantBias bias;
|
||||
Matrix9 aH1, aH2;
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
|
|||
static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
|
||||
|
||||
// Create default parameters with Z-up and above noise parameters
|
||||
static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
|
||||
static boost::shared_ptr<PreintegratedImuMeasurements::Params> testing::Params() {
|
||||
auto p = PreintegrationParams::MakeSharedU(10);
|
||||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||
|
|
@ -51,7 +51,7 @@ TEST(ScenarioRunner, Spin) {
|
|||
const Vector3 W(0, 0, w), V(0, 0, 0);
|
||||
const ConstantTwistScenario scenario(W, V);
|
||||
|
||||
auto p = defaultParams();
|
||||
auto p = testing::Params();
|
||||
ScenarioRunner runner(&scenario, p, kDt);
|
||||
const double T = 2 * kDt; // seconds
|
||||
|
||||
|
|
@ -80,7 +80,7 @@ ConstantTwistScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
|
|||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Forward) {
|
||||
using namespace forward;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), kDt);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), kDt);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
|
|
@ -94,7 +94,7 @@ TEST(ScenarioRunner, Forward) {
|
|||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, ForwardWithBias) {
|
||||
using namespace forward;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), kDt);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), kDt);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
auto pim = runner.integrate(T, kNonZeroBias);
|
||||
|
|
@ -108,7 +108,7 @@ TEST(ScenarioRunner, Circle) {
|
|||
const double v = 2, w = 6 * kDegree;
|
||||
ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
|
||||
|
||||
ScenarioRunner runner(&scenario, defaultParams(), kDt);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), kDt);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
|
|
@ -126,7 +126,7 @@ TEST(ScenarioRunner, Loop) {
|
|||
const double v = 2, w = 6 * kDegree;
|
||||
ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
|
||||
|
||||
ScenarioRunner runner(&scenario, defaultParams(), kDt);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), kDt);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
|
|
@ -157,7 +157,7 @@ const double T = 3; // seconds
|
|||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Accelerating) {
|
||||
using namespace accelerating;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10);
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
|
@ -170,7 +170,7 @@ TEST(ScenarioRunner, Accelerating) {
|
|||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, AcceleratingWithBias) {
|
||||
using namespace accelerating;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10, kNonZeroBias);
|
||||
|
||||
auto pim = runner.integrate(T, kNonZeroBias);
|
||||
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
|
||||
|
|
@ -185,7 +185,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
|
|||
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
|
||||
|
||||
const double T = 10; // seconds
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10);
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
|
@ -216,7 +216,7 @@ const double T = 3; // seconds
|
|||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Accelerating2) {
|
||||
using namespace accelerating2;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10);
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
|
@ -229,7 +229,7 @@ TEST(ScenarioRunner, Accelerating2) {
|
|||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, AcceleratingWithBias2) {
|
||||
using namespace accelerating2;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10, kNonZeroBias);
|
||||
|
||||
auto pim = runner.integrate(T, kNonZeroBias);
|
||||
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
|
||||
|
|
@ -244,7 +244,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating2) {
|
|||
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
|
||||
|
||||
const double T = 10; // seconds
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10);
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
|
@ -276,7 +276,7 @@ const double T = 3; // seconds
|
|||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Accelerating3) {
|
||||
using namespace accelerating3;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10);
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
|
@ -289,7 +289,7 @@ TEST(ScenarioRunner, Accelerating3) {
|
|||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, AcceleratingWithBias3) {
|
||||
using namespace accelerating3;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10, kNonZeroBias);
|
||||
|
||||
auto pim = runner.integrate(T, kNonZeroBias);
|
||||
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
|
||||
|
|
@ -304,7 +304,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating3) {
|
|||
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
|
||||
|
||||
const double T = 10; // seconds
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10);
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
|
@ -337,7 +337,7 @@ const double T = 3; // seconds
|
|||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Accelerating4) {
|
||||
using namespace accelerating4;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10);
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
|
@ -350,7 +350,7 @@ TEST(ScenarioRunner, Accelerating4) {
|
|||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, AcceleratingWithBias4) {
|
||||
using namespace accelerating4;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10, kNonZeroBias);
|
||||
|
||||
auto pim = runner.integrate(T, kNonZeroBias);
|
||||
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
|
||||
|
|
@ -365,7 +365,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) {
|
|||
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
|
||||
|
||||
const double T = 10; // seconds
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
ScenarioRunner runner(&scenario, testing::Params(), T / 10);
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
|
|
|||
Loading…
Reference in New Issue