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

Use constants, slight renaming

release/4.3a0
dellaert 2015-07-19 01:52:53 -07:00
parent 7b02a01a44
commit 9382641445
2 changed files with 54 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -88,8 +88,8 @@ CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasur
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) { const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, CombinedImuFactor::CombinedPreintegratedMeasurements result(bias,
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), I_3x3, I_3x3, I_3x3,
Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6, 6), false); I_3x3, I_3x3, I_6x6, false);
list<Vector3>::const_iterator itAcc = measuredAccs.begin(); list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin(); list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -136,13 +136,13 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
double tol = 1e-6; double tol = 1e-6;
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(), ImuFactor::PreintegratedMeasurements expected1(bias, Z_3x3,
Matrix3::Zero(), Matrix3::Zero()); Z_3x3, Z_3x3);
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias,
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Z_3x3, Z_3x3, Z_3x3, Z_3x3,
Matrix3::Zero(), Matrix::Zero(6, 6)); Z_3x3, Z_6x6);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -174,37 +174,28 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
double deltaT = 1.0; double deltaT = 1.0;
double tol = 1e-6; double tol = 1e-6;
Matrix I6x6(6, 6); ImuFactor::PreintegratedMeasurements pim(
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)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); I_3x3, I_3x3, I_3x3);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements combined_pre_int_data( CombinedImuFactor::CombinedPreintegratedMeasurements combined_pim(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), I_3x3, I_3x3, I_3x3, I_3x3, 2 * I_3x3, I_6x6);
Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6);
combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
deltaT);
// Create factor // Create factor
ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim, gravity, omegaCoriolis);
omegaCoriolis);
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::shared_ptr Combinedmodel =
noiseModel::Gaussian::Covariance(combined_pre_int_data.preintMeasCov()); noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov());
CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
combined_pre_int_data, gravity, omegaCoriolis); combined_pim, gravity, omegaCoriolis);
Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias); Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias);
Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2);
Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias,
bias2);
EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
// Expected Jacobians // Expected Jacobians
@ -301,27 +292,23 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) {
measuredAcc << 0, 1.1, -9.81; measuredAcc << 0, 1.1, -9.81;
double deltaT = 0.001; double deltaT = 0.001;
Matrix I6x6(6, 6); CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
I6x6 = Matrix::Identity(6, 6); I_3x3, I_3x3, 2 * I_3x3, I_6x6, true);
CombinedImuFactor::CombinedPreintegratedMeasurements combined_pre_int_data(
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true);
for (int i = 0; i < 1000; ++i) for (int i = 0; i < 1000; ++i)
combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, pim.integrateMeasurement(measuredAcc, measuredOmega,
deltaT); deltaT);
// Create factor // Create factor
noiseModel::Gaussian::shared_ptr combinedmodel = noiseModel::Gaussian::shared_ptr combinedmodel =
noiseModel::Gaussian::Covariance(combined_pre_int_data.preintMeasCov()); noiseModel::Gaussian::Covariance(pim.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
combined_pre_int_data, gravity, omegaCoriolis); pim, gravity, omegaCoriolis);
// Predict // Predict
Pose3 x1; Pose3 x1;
Vector3 v1(0, 0.0, 0.0); Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocityBias = combined_pre_int_data.predict(x1, v1, PoseVelocityBias poseVelocityBias = pim.predict(x1, v1,
bias, gravity, omegaCoriolis); bias, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; Vector3 expectedVelocity;
@ -334,10 +321,8 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(CombinedImuFactor, PredictRotation) { TEST(CombinedImuFactor, 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)
Matrix I6x6(6, 6); CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
CombinedImuFactor::CombinedPreintegratedMeasurements combined_pre_int_data( I_3x3, I_3x3, 2 * I_3x3, I_6x6, true);
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true);
Vector3 measuredAcc; Vector3 measuredAcc;
measuredAcc << 0, 0, -9.81; measuredAcc << 0, 0, -9.81;
Vector3 gravity; Vector3 gravity;
@ -349,16 +334,14 @@ TEST(CombinedImuFactor, PredictRotation) {
double deltaT = 0.001; double deltaT = 0.001;
double tol = 1e-4; double tol = 1e-4;
for (int i = 0; i < 1000; ++i) for (int i = 0; i < 1000; ++i)
combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
deltaT);
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
combined_pre_int_data, gravity, omegaCoriolis); pim, gravity, omegaCoriolis);
// Predict // Predict
Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2; Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2;
Vector3 v(0, 0, 0), v2; Vector3 v(0, 0, 0), v2;
CombinedImuFactor::Predict(x, v, x2, v2, bias, CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis);
Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expectedPose, x2, tol)); EXPECT(assert_equal(expectedPose, x2, tol));
} }
@ -489,7 +472,7 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
// Compute expected G wrt bias random walk noise (15,6) // Compute expected G wrt bias random walk noise (15,6)
Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries
df_rwBias.setZero(); df_rwBias.setZero();
df_rwBias.block<6, 6>(9, 0) = eye(6); df_rwBias.block<6, 6>(9, 0) = I_6x6;
// Compute expected G wrt gyro noise (15,6) // Compute expected G wrt gyro noise (15,6)
Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>( Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(
@ -502,7 +485,7 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old); newDeltaT, use2ndOrderIntegration), bias_old);
df_dinitBias.block<6, 6>(9, 0) = Matrix::Zero(6, 6); // only has to influence first 9 rows df_dinitBias.block<6, 6>(9, 0) = Z_6x6; // only has to influence first 9 rows
Matrix Gexpected(15, 21); Matrix Gexpected(15, 21);
Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias;

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

@ -83,9 +83,9 @@ Rot3 updatePreintegratedRot(const Rot3& deltaRij_old,
double accNoiseVar = 0.01; double accNoiseVar = 0.01;
double omegaNoiseVar = 0.03; double omegaNoiseVar = 0.03;
double intNoiseVar = 0.0001; double intNoiseVar = 0.0001;
const Matrix3 kMeasuredAccCovariance = accNoiseVar * Matrix3::Identity(); const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * Matrix3::Identity(); const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3;
const Matrix3 kIntegrationErrorCovariance = intNoiseVar * Matrix3::Identity(); const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
// Auxiliary functions to test preintegrated Jacobians // Auxiliary functions to test preintegrated Jacobians
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
@ -211,13 +211,13 @@ double deltaT = 1.0;
TEST(ImuFactor, ErrorAndJacobians) { TEST(ImuFactor, ErrorAndJacobians) {
using namespace common; using namespace common;
bool use2ndOrderIntegration = true; bool use2ndOrderIntegration = true;
PreintegratedImuMeasurements pre_int_data(bias, PreintegratedImuMeasurements pim(bias,
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, use2ndOrderIntegration); kIntegrationErrorCovariance, use2ndOrderIntegration);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
kZeroOmegaCoriolis); kZeroOmegaCoriolis);
// Expected error // Expected error
@ -263,7 +263,7 @@ TEST(ImuFactor, ErrorAndJacobians) {
EXPECT(assert_equal(errorExpected, factor.unwhitenedError(values), 1e-6)); EXPECT(assert_equal(errorExpected, factor.unwhitenedError(values), 1e-6));
// Make sure the whitening is done correctly // Make sure the whitening is done correctly
Matrix cov = pre_int_data.preintMeasCov(); Matrix cov = pim.preintMeasCov();
Matrix R = RtR(cov.inverse()); Matrix R = RtR(cov.inverse());
Vector whitened = R * errorExpected; Vector whitened = R * errorExpected;
EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-6)); EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-6));
@ -290,14 +290,14 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
+ Vector3(0.2, 0.0, 0.0); + Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0; double deltaT = 1.0;
PreintegratedImuMeasurements pre_int_data( PreintegratedImuMeasurements pim(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance); kIntegrationErrorCovariance);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
nonZeroOmegaCoriolis); nonZeroOmegaCoriolis);
Values values; Values values;
@ -330,16 +330,16 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
+ Vector3(0.2, 0.0, 0.0); + Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0; double deltaT = 1.0;
PreintegratedImuMeasurements pre_int_data( PreintegratedImuMeasurements pim(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance); kIntegrationErrorCovariance);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
Pose3 bodyPsensor = Pose3(); Pose3 bodyPsensor = Pose3();
bool use2ndOrderCoriolis = true; bool use2ndOrderCoriolis = true;
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
nonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis); nonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
Values values; Values values;
@ -422,7 +422,7 @@ TEST(ImuFactor, fistOrderExponential) {
Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 actualRot = hatRot const Matrix3 actualRot = hatRot
* Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
// hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); // hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
// This is a first order expansion so the equality is only an approximation // This is a first order expansion so the equality is only an approximation
EXPECT(assert_equal(expectedRot, actualRot)); EXPECT(assert_equal(expectedRot, actualRot));
@ -758,15 +758,15 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0)); Point3(1, 0, 0));
PreintegratedImuMeasurements pre_int_data( PreintegratedImuMeasurements pim(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance); kIntegrationErrorCovariance);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
nonZeroOmegaCoriolis); nonZeroOmegaCoriolis);
Values values; Values values;
@ -792,25 +792,22 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
measuredAcc << 0, 1, -9.81; measuredAcc << 0, 1, -9.81;
double deltaT = 0.001; double deltaT = 0.001;
Matrix I6x6(6, 6); PreintegratedImuMeasurements pim(
I6x6 = Matrix::Identity(6, 6);
PreintegratedImuMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, true); kIntegrationErrorCovariance, true);
for (int i = 0; i < 1000; ++i) for (int i = 0; i < 1000; ++i)
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
kZeroOmegaCoriolis); kZeroOmegaCoriolis);
// Predict // Predict
Pose3 x1; Pose3 x1;
Vector3 v1(0, 0.0, 0.0); Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, kGravity, PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, kGravity,
kZeroOmegaCoriolis); kZeroOmegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; Vector3 expectedVelocity;
@ -830,27 +827,22 @@ TEST(ImuFactor, PredictRotation) {
measuredAcc << 0, 0, -9.81; measuredAcc << 0, 0, -9.81;
double deltaT = 0.001; double deltaT = 0.001;
Matrix I6x6(6, 6); PreintegratedImuMeasurements pim(
I6x6 = Matrix::Identity(6, 6);
PreintegratedImuMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, true); kIntegrationErrorCovariance, true);
for (int i = 0; i < 1000; ++i) for (int i = 0; i < 1000; ++i)
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, kZeroOmegaCoriolis);
kZeroOmegaCoriolis);
// Predict // Predict
Pose3 x1, x2; Pose3 x1, x2;
Vector3 v1 = Vector3(0, 0.0, 0.0); Vector3 v1 = Vector3(0, 0.0, 0.0);
Vector3 v2; Vector3 v2;
ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravity, kZeroOmegaCoriolis);
kGravity, kZeroOmegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
Vector3 expectedVelocity; Vector3 expectedVelocity;
expectedVelocity << 0, 0, 0; expectedVelocity << 0, 0, 0;