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

Merge pull request #874 from borglab/fix/368 

CombinedImuFactor: Add bias effect on position jacobian
release/4.3a0
Varun Agrawal 2022-08-21 16:33:53 -04:00 committed by GitHub
parent fb8d6abb21 103c78b0de
commit 587678e0b7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 1198 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

872
doc/ImuFactor.lyx
View File

File diff suppressed because it is too large Load Diff

BIN
doc/ImuFactor.pdf
View File

Binary file not shown.

BIN
doc/PreintegratedIMUJacobians.pdf Normal file
View File

Binary file not shown.

88
doc/refs.bib
View File

@ -1,26 +1,72 @@
%% This BibTeX bibliography file was created using BibDesk.
%% https://bibdesk.sourceforge.io/
%% Created for Varun Agrawal at 2021-09-27 17:39:09 -0400
%% Saved with string encoding Unicode (UTF-8)
@article{Lupton12tro,
author = {Lupton, Todd and Sukkarieh, Salah},
date-added = {2021-09-27 17:38:56 -0400},
date-modified = {2021-09-27 17:39:09 -0400},
doi = {10.1109/TRO.2011.2170332},
journal = {IEEE Transactions on Robotics},
number = {1},
pages = {61-76},
title = {Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions},
volume = {28},
year = {2012},
Bdsk-Url-1 = {https://doi.org/10.1109/TRO.2011.2170332}}
@inproceedings{Forster15rss,
author = {Christian Forster and Luca Carlone and Frank Dellaert and Davide Scaramuzza},
booktitle = {Robotics: Science and Systems},
date-added = {2021-09-26 20:44:41 -0400},
date-modified = {2021-09-26 20:45:03 -0400},
title = {IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation},
year = {2015}}
@article{Iserles00an,
title = {Lie-group methods},
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and
N{\o}rsett, Syvert P and Zanna, Antonella},
journal = {Acta Numerica 2000},
volume = {9},
pages = {215--365},
year = {2000},
publisher = {Cambridge Univ Press}
}
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella},
journal = {Acta Numerica 2000},
pages = {215--365},
publisher = {Cambridge Univ Press},
title = {Lie-group methods},
volume = {9},
year = {2000}}
@book{Murray94book,
title = {A mathematical introduction to robotic manipulation},
author = {Murray, Richard M and Li, Zexiang and Sastry, S
Shankar and Sastry, S Shankara},
year = {1994},
publisher = {CRC press}
}
author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara},
publisher = {CRC press},
title = {A mathematical introduction to robotic manipulation},
year = {1994}}
@book{Spivak65book,
title = {Calculus on manifolds},
author = {Spivak, Michael},
volume = {1},
year = {1965},
publisher = {WA Benjamin New York}
}
author = {Spivak, Michael},
publisher = {WA Benjamin New York},
title = {Calculus on manifolds},
volume = {1},
year = {1965}}
@phdthesis{Nikolic16thesis,
title={Characterisation, calibration, and design of visual-inertial sensor systems for robot navigation},
author={Nikolic, Janosch},
year={2016},
school={ETH Zurich}
}
@book{Simon06book,
title={Optimal state estimation: Kalman, H infinity, and nonlinear approaches},
author={Simon, Dan},
year={2006},
publisher={John Wiley \& Sons}
}
@inproceedings{Trawny05report_IndirectKF,
title={Indirect Kalman Filter for 3 D Attitude Estimation},
author={Nikolas Trawny and Stergios I. Roumeliotis},
year={2005}
}

19
examples/CombinedImuFactorsExample.cpp
View File

@ -60,13 +60,14 @@ namespace po = boost::program_options;
po::variables_map parseOptions(int argc, char* argv[]) {
po::options_description desc;
desc.add_options()("help,h", "produce help message")(
"data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
"path to the CSV file with the IMU data")(
"output_filename",
po::value<string>()->default_value("imuFactorExampleResults.csv"),
"path to the result file to use")("use_isam", po::bool_switch(),
"use ISAM as the optimizer");
desc.add_options()("help,h", "produce help message") // help message
("data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
"path to the CSV file with the IMU data") // path to the data file
("output_filename",
po::value<string>()->default_value("imuFactorExampleResults.csv"),
"path to the result file to use") // filename to save results to
("use_isam", po::bool_switch(),
"use ISAM as the optimizer"); // flag for ISAM optimizer
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
@ -106,7 +107,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
I_3x3 * 1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
Matrix66 bias_acc_omega_int =
Matrix66 bias_acc_omega_init =
I_6x6 * 1e-5; // error in the bias used for preintegration
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@ -122,7 +123,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int;
p->biasAccOmegaInt = bias_acc_omega_init;
return p;
}

4
examples/ImuFactorsExample.cpp
View File

@ -94,7 +94,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
I_3x3 * 1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
Matrix66 bias_acc_omega_int =
Matrix66 bias_acc_omega_init =
I_6x6 * 1e-5; // error in the bias used for preintegration
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@ -110,7 +110,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int;
p->biasAccOmegaInt = bias_acc_omega_init;
return p;
}

1
gtsam/base/Vector.h
View File

@ -60,6 +60,7 @@ GTSAM_MAKE_VECTOR_DEFS(9)
GTSAM_MAKE_VECTOR_DEFS(10)
GTSAM_MAKE_VECTOR_DEFS(11)
GTSAM_MAKE_VECTOR_DEFS(12)
GTSAM_MAKE_VECTOR_DEFS(15)
typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector;

77
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -93,9 +93,14 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
if (dt <= 0) {
throw std::runtime_error(
"PreintegratedCombinedMeasurements::integrateMeasurement: dt <=0");
}
// Update preintegrated measurements.
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
Matrix9 A; // Jacobian wrt preintegrated measurements without bias (df/dx)
Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively.
PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
// Update preintegrated measurements covariance: as in [2] we consider a first
@ -105,47 +110,78 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// and preintegrated measurements
// Single Jacobians to propagate covariance
// TODO(frank): should we not also account for bias on position?
Matrix3 theta_H_biasOmega = -C.topRows<3>();
Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
Matrix3 theta_H_biasOmega = C.topRows<3>();
Matrix3 pos_H_biasAcc = B.middleRows<3>(3);
Matrix3 vel_H_biasAcc = B.bottomRows<3>();
Matrix3 theta_H_biasOmegaInit = -theta_H_biasOmega;
Matrix3 pos_H_biasAccInit = -pos_H_biasAcc;
Matrix3 vel_H_biasAccInit = -vel_H_biasAcc;
// overall Jacobian wrt preintegrated measurements (df/dx)
Eigen::Matrix<double, 15, 15> F;
F.setZero();
F.block<9, 9>(0, 0) = A;
F.block<3, 3>(0, 12) = theta_H_biasOmega;
F.block<3, 3>(3, 9) = pos_H_biasAcc;
F.block<3, 3>(6, 9) = vel_H_biasAcc;
F.block<6, 6>(9, 9) = I_6x6;
// Update the uncertainty on the state (matrix F in [4]).
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
// propagate uncertainty
// TODO(frank): use noiseModel routine so we can have arbitrary noise models.
const Matrix3& aCov = p().accelerometerCovariance;
const Matrix3& wCov = p().gyroscopeCovariance;
const Matrix3& iCov = p().integrationCovariance;
const Matrix6& bInitCov = p().biasAccOmegaInt;
// first order uncertainty propagation
// Optimized matrix multiplication (1/dt) * G * measurementCovariance *
// G.transpose()
// Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
Eigen::Matrix<double, 15, 15> G_measCov_Gt;
G_measCov_Gt.setZero(15, 15);
const Matrix3& bInitCov11 = bInitCov.block<3, 3>(0, 0) / dt;
const Matrix3& bInitCov12 = bInitCov.block<3, 3>(0, 3) / dt;
const Matrix3& bInitCov21 = bInitCov.block<3, 3>(3, 0) / dt;
const Matrix3& bInitCov22 = bInitCov.block<3, 3>(3, 3) / dt;
// BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = dt * iCov;
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
* (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
* (vel_H_biasAcc.transpose());
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
* (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
* (theta_H_biasOmega.transpose());
D_R_R(&G_measCov_Gt) =
(theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose()) //
+
(theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
D_t_t(&G_measCov_Gt) =
(pos_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) //
+ (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()) //
+ (dt * iCov);
D_v_v(&G_measCov_Gt) =
(vel_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) //
+ (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
* theta_H_biasOmega.transpose();
D_v_R(&G_measCov_Gt) = temp;
D_R_v(&G_measCov_Gt) = temp.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
D_R_t(&G_measCov_Gt) =
theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose();
D_R_v(&G_measCov_Gt) =
theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose();
D_t_R(&G_measCov_Gt) =
pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
D_t_v(&G_measCov_Gt) =
(pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) +
(pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
D_v_R(&G_measCov_Gt) =
vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
D_v_t(&G_measCov_Gt) =
(vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) +
(vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
preintMeasCov_.noalias() += G_measCov_Gt;
}
//------------------------------------------------------------------------------
@ -253,6 +289,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
return os;
}
}
/// namespace gtsam
} // namespace gtsam

7
gtsam/navigation/CombinedImuFactor.h
View File

@ -51,6 +51,7 @@ typedef ManifoldPreintegration PreintegrationType;
* TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
* Computation of the Jacobian Matrices", Tech. Report, 2013.
* Available in this repo as "PreintegratedIMUJacobians.pdf".
* [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
* Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
* Robotics: Science and Systems (RSS), 2015.
@ -61,7 +62,7 @@ typedef ManifoldPreintegration PreintegrationType;
struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration
Matrix6 biasAccOmegaInt; ///< covariance of bias used as initial estimate.
/// Default constructor makes uninitialized params struct.
/// Used for serialization.
@ -92,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; }
const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
private:

6
gtsam/navigation/ImuFactor.cpp
View File

@ -59,7 +59,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
// Update preintegrated measurements (also get Jacobian)
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively.
PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
// first order covariance propagation:
@ -73,11 +73,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
const Matrix3& iCov = p().integrationCovariance;
// (1/dt) allows to pass from continuous time noise to discrete time noise
// Update the uncertainty on the state (matrix A in [4]).
preintMeasCov_ = A * preintMeasCov_ * A.transpose();
// These 2 updates account for uncertainty on the IMU measurement (matrix B in [4]).
preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose();
preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 (9x3 matrix)
preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt;
}

1
gtsam/navigation/ImuFactor.h
View File

@ -53,6 +53,7 @@ typedef ManifoldPreintegration PreintegrationType;
* TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
* Computation of the Jacobian Matrices", Tech. Report, 2013.
* Available in this repo as "PreintegratedIMUJacobians.pdf".
* [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
* Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
* Robotics: Science and Systems (RSS), 2015.

4
gtsam/navigation/PreintegrationBase.cpp
View File

@ -157,9 +157,9 @@ Vector9 PreintegrationBase::computeError(const NavState& state_i,
state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0,
H1 || H3 ? &D_error_predict : 0);
if (H1) *H1 << D_error_predict* D_predict_state_i;
if (H1) *H1 << D_error_predict * D_predict_state_i;
if (H2) *H2 << D_error_state_j;
if (H3) *H3 << D_error_predict* D_predict_bias_i;
if (H3) *H3 << D_error_predict * D_predict_bias_i;
return error;
}

60
gtsam/navigation/ScenarioRunner.cpp
View File

@ -15,8 +15,8 @@
* @author Frank Dellaert
*/
#include <gtsam/navigation/ScenarioRunner.h>
#include <gtsam/base/timing.h>
#include <gtsam/navigation/ScenarioRunner.h>
#include <boost/assign.hpp>
#include <cmath>
@ -105,4 +105,62 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
return Q / (N - 1);
}
PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate(
double T, const Bias& estimatedBias, bool corrupted) const {
gttic_(integrate);
PreintegratedCombinedMeasurements pim(p_, estimatedBias);
const double dt = imuSampleTime();
const size_t nrSteps = T / dt;
double t = 0;
for (size_t k = 0; k < nrSteps; k++, t += dt) {
Vector3 measuredOmega =
corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t);
Vector3 measuredAcc =
corrupted ? measuredSpecificForce(t) : actualSpecificForce(t);
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
}
return pim;
}
NavState CombinedScenarioRunner::predict(
const PreintegratedCombinedMeasurements& pim,
const Bias& estimatedBias) const {
const NavState state_i(scenario().pose(0), scenario().velocity_n(0));
return pim.predict(state_i, estimatedBias);
}
Eigen::Matrix<double, 15, 15> CombinedScenarioRunner::estimateCovariance(
double T, size_t N, const Bias& estimatedBias) const {
gttic_(estimateCovariance);
// Get predict prediction from ground truth measurements
NavState prediction = predict(integrate(T));
// Sample !
Matrix samples(15, N);
Vector15 sum = Vector15::Zero();
for (size_t i = 0; i < N; i++) {
auto pim = integrate(T, estimatedBias, true);
NavState sampled = predict(pim);
Vector15 xi = Vector15::Zero();
xi << sampled.localCoordinates(prediction),
(estimatedBias_.vector() - estimatedBias.vector());
samples.col(i) = xi;
sum += xi;
}
// Compute MC covariance
Vector15 sampleMean = sum / N;
Eigen::Matrix<double, 15, 15> Q;
Q.setZero();
for (size_t i = 0; i < N; i++) {
Vector15 xi = samples.col(i) - sampleMean;
Q += xi * xi.transpose();
}
return Q / (N - 1);
}
} // namespace gtsam

44
gtsam/navigation/ScenarioRunner.h
View File

@ -16,9 +16,10 @@
*/
#pragma once
#include <gtsam/linear/Sampler.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/Scenario.h>
#include <gtsam/linear/Sampler.h>
namespace gtsam {
@ -66,10 +67,10 @@ class GTSAM_EXPORT ScenarioRunner {
// also, uses g=10 for easy debugging
const Vector3& gravity_n() const { return p_->n_gravity; }
const Scenario& scenario() const { return scenario_; }
// A gyro simply measures angular velocity in body frame
Vector3 actualAngularVelocity(double t) const {
return scenario_.omega_b(t);
}
Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); }
// An accelerometer measures acceleration in body, but not gravity
Vector3 actualSpecificForce(double t) const {
@ -106,4 +107,39 @@ class GTSAM_EXPORT ScenarioRunner {
Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
};
/*
* Simple class to test navigation scenarios with CombinedImuMeasurements.
* Takes a trajectory scenario as input, and can generate IMU measurements
*/
class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
public:
typedef boost::shared_ptr<PreintegrationCombinedParams> SharedParams;
private:
const SharedParams p_;
const Bias estimatedBias_;
public:
CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0,
const Bias& bias = Bias())
: ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
imuSampleTime, bias),
p_(p),
estimatedBias_(bias) {}
/// Integrate measurements for T seconds into a PIM
PreintegratedCombinedMeasurements integrate(
double T, const Bias& estimatedBias = Bias(),
bool corrupted = false) const;
/// Predict predict given a PIM
NavState predict(const PreintegratedCombinedMeasurements& pim,
const Bias& estimatedBias = Bias()) const;
/// Compute a Monte Carlo estimate of the predict covariance using N samples
Eigen::Matrix<double, 15, 15> estimateCovariance(
double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
};
} // namespace gtsam

4
gtsam/navigation/navigation.i
View File

@ -165,11 +165,11 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
void setBiasAccCovariance(Matrix cov);
void setBiasOmegaCovariance(Matrix cov);
void setBiasAccOmegaInt(Matrix cov);
void setBiasAccOmegaInit(Matrix cov);
Matrix getBiasAccCovariance() const ;
Matrix getBiasOmegaCovariance() const ;
Matrix getBiasAccOmegaInt() const;
Matrix getBiasAccOmegaInit() const;
};

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

@ -16,18 +16,19 @@
* @author Frank Dellaert
* @author Richard Roberts
* @author Stephen Williams
* @author Varun Agrawal
*/
#include <gtsam/navigation/ImuFactor.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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Pose3.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>
@ -40,12 +41,15 @@ static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
p->integrationCovariance = 0.0001 * I_3x3;
p->biasAccCovariance = Z_3x3;
p->biasOmegaCovariance = Z_3x3;
p->biasAccOmegaInt = Z_6x6;
return p;
}
}
} // namespace testing
/* ************************************************************************* */
TEST( CombinedImuFactor, PreintegratedMeasurements ) {
TEST(CombinedImuFactor, PreintegratedMeasurements ) {
// Linearization point
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
@ -71,8 +75,9 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
}
/* ************************************************************************* */
TEST( CombinedImuFactor, ErrorWithBiases ) {
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));
@ -203,6 +208,114 @@ TEST(CombinedImuFactor, PredictRotation) {
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, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 5.0005e-05, 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 that the covariance values for the ImuFactor and the CombinedImuFactor
// (top-left 9x9) are the same
TEST(CombinedImuFactor, SameCovariance) {
// IMU measurements and time delta
Vector3 accMeas(0.1577, -0.8251, 9.6111);
Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
double deltaT = 0.01;
// Assume zero bias
imuBias::ConstantBias currentBias;
// Define params for ImuFactor
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());
// The IMU preintegration object for ImuFactor
PreintegratedImuMeasurements pim(params, currentBias);
pim.integrateMeasurement(accMeas, omegaMeas, deltaT);
// Define params for CombinedImuFactor
auto combined_params = PreintegrationCombinedParams::MakeSharedU();
combined_params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
combined_params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
// Set bias integration covariance explicitly to zero
combined_params->setIntegrationCovariance(Z_3x3);
combined_params->setOmegaCoriolis(Z_3x1);
// Set bias initial covariance explicitly to zero
combined_params->setBiasAccOmegaInit(Z_6x6);
// The IMU preintegration object for CombinedImuFactor
PreintegratedCombinedMeasurements cpim(combined_params, currentBias);
cpim.integrateMeasurement(accMeas, omegaMeas, deltaT);
// Assert if the noise covariance
EXPECT(assert_equal(pim.preintMeasCov(),
cpim.preintMeasCov().block(0, 0, 9, 9)));
}
/* ************************************************************************* */
TEST(CombinedImuFactor, Accelerating) {
const double a = 0.2, v = 50;
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
// going in X The body itself has Z axis pointing down
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
const Point3 initial_position(10, 20, 0);
const Vector3 initial_velocity(v, 0, 0);
const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
Vector3(a, 0, 0));
const double T = 3.0; // seconds
CombinedScenarioRunner runner(scenario, testing::Params(), T / 10);
PreintegratedCombinedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
auto estimatedCov = runner.estimateCovariance(T, 100);
Eigen::Matrix<double, 15, 15> expected = pim.preintMeasCov();
EXPECT(assert_equal(estimatedCov, expected, 0.1));
}
/* ************************************************************************* */
int main() {
TestResult tr;