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

Re-formatted to BORG-style

release/4.3a0
dellaert 2015-07-15 23:24:24 -07:00
parent 0c3bb85547
commit e7c2e81831
1 changed files with 218 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

387
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -43,19 +43,20 @@ static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
namespace {
// Auxiliary functions to test evaluate error in ImuFactor
/* ************************************************************************* */
Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias) {
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3));
Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias) {
return Rot3::Expmap(
factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3));
}
// Auxiliary functions to test Jacobians F and G used for
// covariance propagation during preintegration
/* ************************************************************************* */
Vector updatePreintegratedPosVel(const Vector3 deltaPij_old, const Vector3& deltaVij_old,
const Rot3& deltaRij_old, const Vector3& correctedAcc,
const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration_) {
Vector updatePreintegratedPosVel(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const Vector3& correctedAcc, const Vector3& correctedOmega,
const double deltaT, const bool use2ndOrderIntegration_) {
Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * correctedAcc * deltaT;
Vector3 deltaPij_new;
@ -71,8 +72,8 @@ Vector updatePreintegratedPosVel(const Vector3 deltaPij_old, const Vector3& delt
return result;
}
Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, const Vector3& correctedOmega,
const double deltaT) {
Rot3 updatePreintegratedRot(const Rot3& deltaRij_old,
const Vector3& correctedOmega, const double deltaT) {
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT);
return deltaRij_new;
}
@ -94,8 +95,8 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const list<Vector3>& measuredOmegas, const list<double>& deltaTs,
const bool use2ndOrderIntegration = false) {
ImuFactor::PreintegratedMeasurements result(bias, kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
use2ndOrderIntegration);
kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
use2ndOrderIntegration);
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -106,29 +107,30 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
return result;
}
Vector3 evaluatePreintegratedMeasurementsPosition(const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs) {
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaPij();
Vector3 evaluatePreintegratedMeasurementsPosition(
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs).deltaPij();
}
Vector3 evaluatePreintegratedMeasurementsVelocity(const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs) {
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaVij();
Vector3 evaluatePreintegratedMeasurementsVelocity(
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs).deltaVij();
}
Rot3 evaluatePreintegratedMeasurementsRotation(const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs) {
Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
return Rot3(
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaRij());
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs).deltaRij());
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) {
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
const double deltaT) {
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
}
@ -136,7 +138,7 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
}
} // namespace
} // namespace
/* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurements) {
@ -158,19 +160,22 @@ TEST(ImuFactor, PreintegratedMeasurements) {
bool use2ndOrderIntegration = true;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual1(bias, kMeasuredAccCovariance,
kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, use2ndOrderIntegration);
kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
use2ndOrderIntegration);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
// Integrate again
Vector3 expectedDeltaP2;
expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0;
Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5;
Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0)
+ expectedDeltaR1.matrix() * measuredAcc * 0.5;
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT2(1);
@ -178,42 +183,49 @@ TEST(ImuFactor, PreintegratedMeasurements) {
ImuFactor::PreintegratedMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
}
// Common linearization point and measurements for tests
namespace common {
imuBias::ConstantBias bias; // Bias
Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), Point3(5.0, 1.0, -50.0));
imuBias::ConstantBias bias; // Bias
Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.0, 1.0, -50.0));
Vector3 v1(Vector3(0.5, 0.0, 0.0));
Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0), Point3(5.5, 1.0, -50.0));
Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.5, 1.0, -50.0));
Vector3 v2(Vector3(0.5, 0.0, 0.0));
// Measurements
Vector3 measuredOmega(M_PI / 100, 0, 0);
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector();
double deltaT = 1.0;
} // namespace common
} // namespace common
/* ************************************************************************* */
TEST(ImuFactor, ErrorAndJacobians) {
using namespace common;
bool use2ndOrderIntegration = true;
ImuFactor::PreintegratedMeasurements pre_int_data(
bias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
use2ndOrderIntegration);
ImuFactor::PreintegratedMeasurements pre_int_data(bias,
kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, use2ndOrderIntegration);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, kZeroOmegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
kZeroOmegaCoriolis);
// Expected error
Vector errorExpected(9);
errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
EXPECT(assert_equal(errorExpected, factor.evaluateError(x1, v1, x2, v2, bias), 1e-6));
EXPECT(
assert_equal(errorExpected, factor.evaluateError(x1, v1, x2, v2, bias),
1e-6));
Values values;
values.insert(X(1), x1);
@ -229,7 +241,7 @@ TEST(ImuFactor, ErrorAndJacobians) {
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a;
(void)factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
// Make sure rotation part is correct when error is interpreted as axis-angle
// Jacobians are around zero, so the rotation part is the same as:
@ -245,7 +257,9 @@ TEST(ImuFactor, ErrorAndJacobians) {
Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1);
values.update(V(2), v2_wrong);
errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0;
EXPECT(assert_equal(errorExpected, factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-6));
EXPECT(
assert_equal(errorExpected,
factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-6));
EXPECT(assert_equal(errorExpected, factor.unwhitenedError(values), 1e-6));
// Make sure the whitening is done correctly
@ -263,24 +277,28 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
using common::x1;
using common::v1;
using common::v2;
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0));
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
// Measurements
Vector3 nonZeroOmegaCoriolis;
nonZeroOmegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector() + Vector3(0.2, 0.0, 0.0);
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, nonZeroOmegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
nonZeroOmegaCoriolis);
Values values;
values.insert(X(1), x1);
@ -299,27 +317,30 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
using common::x1;
using common::v1;
using common::v2;
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0));
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
// Measurements
Vector3 nonZeroOmegaCoriolis;
nonZeroOmegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector() + Vector3(0.2, 0.0, 0.0);
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
Pose3 bodyPsensor = Pose3();
bool use2ndOrderCoriolis = true;
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, nonZeroOmegaCoriolis,
bodyPsensor, use2ndOrderCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
nonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
Values values;
values.insert(X(1), x1);
@ -336,7 +357,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
/* ************************************************************************* */
TEST(ImuFactor, PartialDerivative_wrt_Bias) {
// Linearization point
Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias
Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias
// Measurements
Vector3 measuredOmega(0.1, 0, 0);
@ -344,11 +365,13 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
// Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT),
Vector3(biasOmega));
const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
const Matrix3 Jr = Rot3::ExpmapDerivative(
(measuredOmega - biasOmega) * deltaT);
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
// Compare Jacobians
// 1e-3 needs to be added only when using quaternions for rotations
@ -358,7 +381,7 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
/* ************************************************************************* */
TEST(ImuFactor, PartialDerivativeLogmap) {
// Linearization point
Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias
Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias
// Measurements
Vector3 deltatheta(0, 0, 0);
@ -376,7 +399,7 @@ TEST(ImuFactor, PartialDerivativeLogmap) {
/* ************************************************************************* */
TEST(ImuFactor, fistOrderExponential) {
// Linearization point
Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias
Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias
// Measurements
Vector3 measuredOmega(0.1, 0, 0);
@ -387,15 +410,18 @@ TEST(ImuFactor, fistOrderExponential) {
Vector3 deltabiasOmega;
deltabiasOmega << alpha, alpha, alpha;
const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
const Matrix3 Jr = Rot3::ExpmapDerivative(
(measuredOmega - biasOmega) * deltaT);
Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
const Matrix expectedRot =
Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix expectedRot = Rot3::Expmap(
(measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 actualRot = hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
const Matrix3 hatRot =
Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 actualRot = hatRot
* Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
// hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
// This is a first order expansion so the equality is only an approximation
@ -405,7 +431,7 @@ TEST(ImuFactor, fistOrderExponential) {
/* ************************************************************************* */
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
// Linearization point
imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
@ -420,51 +446,56 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
deltaTs.push_back(0.01);
for (int i = 1; i < 100; i++) {
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01);
}
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs);
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs);
// Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas,
deltaTs),
bias);
Matrix expectedDelPdelBias = numericalDerivative11<Vector,
imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs,
measuredOmegas, deltaTs), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas,
deltaTs),
bias);
Matrix expectedDelVdelBias = numericalDerivative11<Vector,
imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs,
measuredOmegas, deltaTs), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
Matrix expectedDelRdelBias = numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas,
deltaTs),
bias);
Matrix expectedDelRdelBias =
numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
EXPECT(
assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(
assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
/* ************************************************************************* */
TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
// Linearization point
imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
@ -477,48 +508,50 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
deltaTs.push_back(0.01);
for (int i = 1; i < 100; i++) {
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01);
}
bool use2ndOrderIntegration = false;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated = evaluatePreintegratedMeasurements(
bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration);
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs, use2ndOrderIntegration);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0);
const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, body_P_sensor,
Factual, Gactual);
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
newDeltaT, body_P_sensor, Factual, Gactual);
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, newMeasuredAcc,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaPij_old);
// Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, newMeasuredAcc,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaVij_old);
// Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, newMeasuredAcc,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaRij_old);
Matrix FexpectedTop6(6, 9);
@ -526,7 +559,8 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
// Compute expected f_rot wrt angles
Matrix dfr_dangle = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), deltaRij_old);
boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT),
deltaRij_old);
Matrix FexpectedBottom3(3, 9);
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
@ -540,25 +574,26 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute jacobian wrt integration noise
Matrix dgpv_dintNoise(6, 3);
dgpv_dintNoise << I_3x3* newDeltaT, Z_3x3;
dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
// Compute jacobian wrt acc noise
Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, deltaRij_old, _1,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
newMeasuredAcc);
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, deltaRij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
newMeasuredOmega);
Matrix GexpectedTop6(6, 9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
// Compute expected f_rot wrt gyro noise
Matrix dgr_dangle = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), newMeasuredOmega);
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT),
newMeasuredOmega);
Matrix GexpectedBottom3(3, 9);
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
@ -569,12 +604,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
// Check covariance propagation
Matrix9 measurementCovariance;
measurementCovariance << intNoiseVar* I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar* I_3x3, Z_3x3,
Z_3x3, Z_3x3, omegaNoiseVar* I_3x3;
measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar
* I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3;
Matrix newPreintCovarianceExpected =
Factual * oldPreintCovariance * Factual.transpose() +
(1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
* Factual.transpose()
+ (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
@ -583,8 +618,8 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
/* ************************************************************************* */
TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
// Linearization point
imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
@ -597,48 +632,50 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
deltaTs.push_back(0.01);
for (int i = 1; i < 100; i++) {
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01);
}
bool use2ndOrderIntegration = true;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated = evaluatePreintegratedMeasurements(
bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration);
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs, use2ndOrderIntegration);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0);
const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, body_P_sensor,
Factual, Gactual);
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
newDeltaT, body_P_sensor, Factual, Gactual);
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, newMeasuredAcc,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaPij_old);
// Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, newMeasuredAcc,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaVij_old);
// Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, newMeasuredAcc,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaRij_old);
Matrix FexpectedTop6(6, 9);
@ -646,7 +683,8 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
// Compute expected f_rot wrt angles
Matrix dfr_dangle = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), deltaRij_old);
boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT),
deltaRij_old);
Matrix FexpectedBottom3(3, 9);
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
@ -660,25 +698,26 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute jacobian wrt integration noise
Matrix dgpv_dintNoise(6, 3);
dgpv_dintNoise << I_3x3* newDeltaT, Z_3x3;
dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
// Compute jacobian wrt acc noise
Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, deltaRij_old, _1,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
newMeasuredAcc);
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, deltaRij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
newMeasuredOmega);
Matrix GexpectedTop6(6, 9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
// Compute expected f_rot wrt gyro noise
Matrix dgr_dangle = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), newMeasuredOmega);
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT),
newMeasuredOmega);
Matrix GexpectedBottom3(3, 9);
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
@ -689,12 +728,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
// Check covariance propagation
Matrix9 measurementCovariance;
measurementCovariance << intNoiseVar* I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar* I_3x3, Z_3x3,
Z_3x3, Z_3x3, omegaNoiseVar* I_3x3;
measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar
* I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3;
Matrix newPreintCovarianceExpected =
Factual * oldPreintCovariance * Factual.transpose() +
(1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
* Factual.transpose()
+ (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
@ -702,10 +741,11 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
/* ************************************************************************* */
TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 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(Vector3(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));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
Vector3 v2(Vector3(0.5, 0.0, 0.0));
// Measurements
@ -713,19 +753,23 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
nonZeroOmegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector() + Vector3(0.2, 0.0, 0.0);
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), Point3(1, 0, 0));
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0));
ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, nonZeroOmegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
nonZeroOmegaCoriolis);
Values values;
values.insert(X(1), x1);
@ -741,11 +785,11 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
/* ************************************************************************* */
TEST(ImuFactor, PredictPositionAndVelocity) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
Vector3 measuredOmega;
measuredOmega << 0, 0, 0; // M_PI/10.0+0.3;
measuredOmega << 0, 0, 0; // M_PI/10.0+0.3;
Vector3 measuredAcc;
measuredAcc << 0, 1, -9.81;
double deltaT = 0.001;
@ -754,19 +798,22 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
I6x6 = Matrix::Identity(6, 6);
ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true);
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, true);
for (int i = 0; i < 1000; ++i)
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, kZeroOmegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
kZeroOmegaCoriolis);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, kGravity, kZeroOmegaCoriolis);
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, kGravity,
kZeroOmegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity;
expectedVelocity << 0, 1, 0;
@ -776,11 +823,11 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
/* ************************************************************************* */
TEST(ImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10; // M_PI/10.0+0.3;
measuredOmega << 0, 0, M_PI / 10; // M_PI/10.0+0.3;
Vector3 measuredAcc;
measuredAcc << 0, 0, -9.81;
double deltaT = 0.001;
@ -789,21 +836,23 @@ TEST(ImuFactor, PredictRotation) {
I6x6 = Matrix::Identity(6, 6);
ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true);
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, true);
for (int i = 0; i < 1000; ++i)
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, kZeroOmegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
kZeroOmegaCoriolis);
// Predict
Pose3 x1, x2;
Vector3 v1 = Vector3(0, 0.0, 0.0);
Vector3 v2;
ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), kGravity,
kZeroOmegaCoriolis);
ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(),
kGravity, kZeroOmegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
Vector3 expectedVelocity;
expectedVelocity << 0, 0, 0;