testCombinedImuFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
22 #include <CppUnitLite/TestHarness.h>
23 #include <gtsam/base/TestableAssertions.h>
24 #include <gtsam/base/numericalDerivative.h>
25 #include <gtsam/geometry/Pose3.h>
26 #include <gtsam/inference/Symbol.h>
27 #include <gtsam/navigation/CombinedImuFactor.h>
28 #include <gtsam/navigation/ImuBias.h>
29 #include <gtsam/navigation/ImuFactor.h>
30 #include <gtsam/navigation/ScenarioRunner.h>
31 #include <gtsam/nonlinear/Values.h>
32 
33 #include <list>
34 
35 #include "imuFactorTesting.h"
36 
37 namespace testing {
38 // Create default parameters with Z-down and above noise parameters
39 static std::shared_ptr<PreintegratedCombinedMeasurements::Params> Params(
40  const Matrix3& biasAccCovariance = Matrix3::Zero(),
41  const Matrix3& biasOmegaCovariance = Matrix3::Zero(),
42  const Matrix6& biasAccOmegaInt = Matrix6::Zero()) {
43  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity);
44  p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
45  p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
46  p->integrationCovariance = 0.0001 * I_3x3;
47  p->biasAccCovariance = biasAccCovariance;
48  p->biasOmegaCovariance = biasOmegaCovariance;
49  p->biasAccOmegaInt = biasAccOmegaInt;
50  return p;
51 }
52 } // namespace testing
53 
54 /* ************************************************************************* */
55 TEST(CombinedImuFactor, PreintegratedMeasurements ) {
56  // Linearization point
57  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
58 
59  // Measurements
60  Vector3 measuredAcc(0.1, 0.0, 0.0);
61  Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
62  double deltaT = 0.5;
63  double tol = 1e-6;
64 
65  auto p = testing::Params();
66 
67  // Actual preintegrated values
68  PreintegratedImuMeasurements expected1(p, bias);
69  expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
70 
71  PreintegratedCombinedMeasurements actual1(p, bias);
72 
73  actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
74 
75  EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol));
76  EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol));
77  EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol));
78  DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
79 }
80 
81 
82 /* ************************************************************************* */
83 TEST(CombinedImuFactor, ErrorWithBiases ) {
84  Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
85  Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
86  Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
87  Vector3 v1(0.5, 0.0, 0.0);
88  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
89  Point3(5.5, 1.0, -50.0));
90  Vector3 v2(0.5, 0.0, 0.0);
91 
92  auto p = testing::Params();
93  p->omegaCoriolis = Vector3(0,0.1,0.1);
94  PreintegratedImuMeasurements pim(
95  p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
96 
97  // Measurements
98  Vector3 measuredOmega;
99  measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
100  Vector3 measuredAcc =
101  x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0);
102  double deltaT = 1.0;
103  double tol = 1e-6;
104 
105  pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
106 
107  PreintegratedCombinedMeasurements combined_pim(p,
108  Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
109 
110  combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
111 
112  // Create factor
113  ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim);
114 
115  noiseModel::Gaussian::shared_ptr Combinedmodel =
116  noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov());
117  CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
118  combined_pim);
119 
120  Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias);
121  Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias,
122  bias2);
123  EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
124 
125  // Expected Jacobians
126  Matrix H1e, H2e, H3e, H4e, H5e;
127  (void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
128 
129  // Actual Jacobians
130  Matrix H1a, H2a, H3a, H4a, H5a, H6a;
131  (void) combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a,
132  H3a, H4a, H5a, H6a);
133 
134  EXPECT(assert_equal(H1e, H1a.topRows(9)));
135  EXPECT(assert_equal(H2e, H2a.topRows(9)));
136  EXPECT(assert_equal(H3e, H3a.topRows(9)));
137  EXPECT(assert_equal(H4e, H4a.topRows(9)));
138  EXPECT(assert_equal(H5e, H5a.topRows(9)));
139 }
140 
141 /* ************************************************************************* */
142 #ifdef GTSAM_TANGENT_PREINTEGRATION
143 TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
144  auto p = testing::Params();
145  testing::SomeMeasurements measurements;
146 
147  auto preintegrated = [=](const Vector3& a, const Vector3& w) {
148  PreintegratedImuMeasurements pim(p, Bias(a, w));
149  testing::integrateMeasurements(measurements, &pim);
150  return pim.preintegrated();
151  };
152 
153  // Actual pre-integrated values
154  PreintegratedCombinedMeasurements pim(p);
155  testing::integrateMeasurements(measurements, &pim);
156 
157  EXPECT(assert_equal(numericalDerivative21<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
158  pim.preintegrated_H_biasAcc()));
159  EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
160  pim.preintegrated_H_biasOmega(), 1e-3));
161 }
162 #endif
163 
164 /* ************************************************************************* */
165 TEST(CombinedImuFactor, PredictPositionAndVelocity) {
166  const Bias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
167 
168  auto p = testing::Params();
169 
170  // Measurements
171  const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3;
172  const Vector3 measuredAcc(0, 1.1, -kGravity);
173  const double deltaT = 0.01;
174 
175  PreintegratedCombinedMeasurements pim(p, bias);
176 
177  for (int i = 0; i < 100; ++i)
178  pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
179 
180  // Create factor
181  const noiseModel::Gaussian::shared_ptr combinedmodel =
182  noiseModel::Gaussian::Covariance(pim.preintMeasCov());
183  const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
184 
185  // Predict
186  const NavState actual = pim.predict(NavState(), bias);
187  const Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
188  const Vector3 expectedVelocity(0, 1, 0);
189  EXPECT(assert_equal(expectedPose, actual.pose()));
190  EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity())));
191 }
192 
193 /* ************************************************************************* */
194 TEST(CombinedImuFactor, PredictRotation) {
195  const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
196  auto p = testing::Params();
197  PreintegratedCombinedMeasurements pim(p, bias);
198  const Vector3 measuredAcc = - kGravityAlongNavZDown;
199  const Vector3 measuredOmega(0, 0, M_PI / 10.0);
200  const double deltaT = 0.01;
201  const double tol = 1e-4;
202  for (int i = 0; i < 100; ++i)
203  pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
204  const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
205 
206  // Predict
207  const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2;
208  const Vector3 v(0, 0, 0), v2;
209  const NavState actual = pim.predict(NavState(x, v), bias);
210  const Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
211  EXPECT(assert_equal(expectedPose, actual.pose(), tol));
212 }
213 
214 /* ************************************************************************* */
215 // Testing covariance to check if all the jacobians are accounted for.
216 TEST(CombinedImuFactor, CheckCovariance) {
217  auto params = PreintegrationCombinedParams::MakeSharedU(9.81);
218 
219  params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
220  params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
221  params->setIntegrationCovariance(pow(0.0, 2) * I_3x3);
222  params->setOmegaCoriolis(Vector3::Zero());
223 
224  imuBias::ConstantBias currentBias;
225 
226  PreintegratedCombinedMeasurements actual(params, currentBias);
227 
228  // Measurements
229  Vector3 measuredAcc(0.1577, -0.8251, 9.6111);
230  Vector3 measuredOmega(-0.0210, 0.0311, 0.0145);
231  double deltaT = 0.01;
232 
233  actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
234 
235  Eigen::Matrix<double, 15, 15> expected;
236  expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
237  0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
238  0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
239  0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, 0, //
240  0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, //
241  0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, //
242  0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, //
243  0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, //
244  0, 0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, //
245  0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, //
246  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, //
247  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, //
248  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, //
249  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, //
250  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
251 
252  // regression
253  EXPECT(assert_equal(expected, actual.preintMeasCov()));
254 }
255 
256 /* ************************************************************************* */
257 // Test that the covariance values for the ImuFactor and the CombinedImuFactor
258 // (top-left 9x9) are the same
259 TEST(CombinedImuFactor, SameCovariance) {
260  // IMU measurements and time delta
261  Vector3 accMeas(0.1577, -0.8251, 9.6111);
262  Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
263  double deltaT = 0.01;
264 
265  // Assume zero bias
266  imuBias::ConstantBias currentBias;
267 
268  // Define params for ImuFactor
269  auto params = PreintegrationParams::MakeSharedU();
270  params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
271  params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
272  params->setIntegrationCovariance(pow(0, 2) * I_3x3);
273  params->setOmegaCoriolis(Vector3::Zero());
274 
275  // The IMU preintegration object for ImuFactor
276  PreintegratedImuMeasurements pim(params, currentBias);
277  pim.integrateMeasurement(accMeas, omegaMeas, deltaT);
278 
279  // Define params for CombinedImuFactor
280  auto combined_params = PreintegrationCombinedParams::MakeSharedU();
281  combined_params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
282  combined_params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
283  // Set bias integration covariance explicitly to zero
284  combined_params->setIntegrationCovariance(Z_3x3);
285  combined_params->setOmegaCoriolis(Z_3x1);
286  // Set bias initial covariance explicitly to zero
287  combined_params->setBiasAccOmegaInit(Z_6x6);
288 
289  // The IMU preintegration object for CombinedImuFactor
290  PreintegratedCombinedMeasurements cpim(combined_params, currentBias);
291  cpim.integrateMeasurement(accMeas, omegaMeas, deltaT);
292 
293  // Assert if the noise covariance
294  EXPECT(assert_equal(pim.preintMeasCov(),
295  cpim.preintMeasCov().block(0, 0, 9, 9)));
296 }
297 
298 /* ************************************************************************* */
299 TEST(CombinedImuFactor, Accelerating) {
300  const double a = 0.2, v = 50;
301 
302  // Set up body pointing towards y axis, and start at 10,20,0 with velocity
303  // going in X The body itself has Z axis pointing down
304  const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
305  const Point3 initial_position(10, 20, 0);
306  const Vector3 initial_velocity(v, 0, 0);
307 
308  const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
309  Vector3(a, 0, 0));
310 
311  const double T = 3.0; // seconds
312 
313  CombinedScenarioRunner runner(scenario, testing::Params(), T / 10);
314 
315  PreintegratedCombinedMeasurements pim = runner.integrate(T);
316  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
317 
318  auto estimatedCov = runner.estimateCovariance(T, 100);
319  Eigen::Matrix<double, 15, 15> expected = pim.preintMeasCov();
320  EXPECT(assert_equal(estimatedCov, expected, 0.1));
321 }
322 
323 /* ************************************************************************* */
324 TEST(CombinedImuFactor, ResetIntegration) {
325  const double a = 0.2, v = 50;
326 
327  // Set up body pointing towards y axis, and start at 10,20,0 with velocity
328  // going in X The body itself has Z axis pointing down
329  const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
330  const Point3 initial_position(10, 20, 0);
331  const Vector3 initial_velocity(v, 0, 0);
332 
333  const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
334  Vector3(a, 0, 0));
335 
336  const double T = 3.0; // seconds
337 
338  auto preinMeasCov = 0.001 * Eigen::Matrix<double, 15, 15>::Identity();
339  CombinedScenarioRunner runner(
340  scenario,
341  testing::Params(Matrix3::Zero(), Matrix3::Zero(),
342  0.1 * Matrix6::Identity()),
343  T / 10, imuBias::ConstantBias(), preinMeasCov);
344 
345  PreintegratedCombinedMeasurements pim = runner.integrate(T);
346  // Make copy for testing different conditions
347  PreintegratedCombinedMeasurements pim2 = pim;
348 
349  // Test default method
350  pim.resetIntegration();
351  Matrix6 expected = 0.1 * I_6x6;
352  EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt, 1e-9));
353 
354  // Test method where Q_init is provided
355  Matrix6 expected_Q_init = I_6x6 * 0.001;
356  pim2.resetIntegration(expected_Q_init);
357  EXPECT(assert_equal(expected_Q_init, pim.p().biasAccOmegaInt, 1e-9));
358 }
359 
360 /* ************************************************************************* */
361 int main() {
362  TestResult tr;
363  return TestRegistry::runAllTests(tr);
364 }
365 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
testing
Definition: benchmark.h:20
kGyroSigma
static const double kGyroSigma
Definition: testScenarioRunner.cpp:32
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
kAccelSigma
static const double kAccelSigma
Definition: testScenarioRunner.cpp:33
ScenarioRunner.h
Simple class to test navigation scenarios.
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::PreintegratedImuMeasurements::integrateMeasurement
void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
Definition: ImuFactor.cpp:54
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:47
TestHarness.h
gtsam::PreintegrationCombinedParams::biasAccOmegaInt
Matrix6 biasAccOmegaInt
covariance of bias used as initial estimate.
Definition: PreintegrationCombinedParams.h:41
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::CombinedScenarioRunner::estimateCovariance
Eigen::Matrix< double, 15, 15 > estimateCovariance(double T, size_t N=1000, const Bias &estimatedBias=Bias()) const
Compute a Monte Carlo estimate of the predict covariance using N samples.
Definition: ScenarioRunner.cpp:132
gtsam::ManifoldPreintegration::deltaRij
Rot3 deltaRij() const override
Definition: ManifoldPreintegration.h:77
biased_x_rotation::bias
const Vector3 bias(1, 2, 3)
gtsam::NavState
Definition: NavState.h:38
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
X
#define X
Definition: icosphere.cpp:20
initial::nRb
const Rot3 nRb
Definition: testScenarioRunner.cpp:149
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
bias2
Bias bias2(biasAcc2, biasGyro2)
gtsam::CombinedImuFactor::evaluateError
Vector evaluateError(const Pose3 &pose_i, const Vector3 &vel_i, const Pose3 &pose_j, const Vector3 &vel_j, const imuBias::ConstantBias &bias_i, const imuBias::ConstantBias &bias_j, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5, OptionalMatrixType H6) const override
vector of errors
Definition: CombinedImuFactor.cpp:234
gtsam::PreintegratedCombinedMeasurements::resetIntegration
void resetIntegration() override
Re-initialize PreintegratedCombinedMeasurements.
Definition: CombinedImuFactor.cpp:76
gtsam::ConstantTwistScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:67
testing::integrateMeasurements
void integrateMeasurements(const vector< ImuMeasurement > &measurements, PIM *pim)
Definition: imuFactorTesting.h:59
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::noiseModel::Gaussian::shared_ptr
std::shared_ptr< Gaussian > shared_ptr
Definition: NoiseModel.h:191
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
Definition: Rot3.cpp:136
gtsam::PreintegratedCombinedMeasurements::integrateMeasurement
void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
Definition: CombinedImuFactor.cpp:106
TestableAssertions.h
Provides additional testing facilities for common data structures.
common::deltaT
static const double deltaT
Definition: testImuFactor.cpp:183
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
numericalDerivative.h
Some functions to compute numerical derivatives.
main
int main()
Definition: testCombinedImuFactor.cpp:361
biased_x_rotation::measuredOmega
const Vector3 measuredOmega
Definition: testPreintegratedRotation.cpp:35
gtsam::ImuFactor::evaluateError
Vector evaluateError(const Pose3 &pose_i, const Vector3 &vel_i, const Pose3 &pose_j, const Vector3 &vel_j, const imuBias::ConstantBias &bias_i, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) const override
vector of errors
Definition: ImuFactor.cpp:153
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::CombinedScenarioRunner
Definition: ScenarioRunner.h:114
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::PreintegratedImuMeasurements::preintMeasCov
Matrix preintMeasCov() const
Return pre-integrated measurement covariance.
Definition: ImuFactor.h:137
gtsam::PreintegratedImuMeasurements
Definition: ImuFactor.h:68
gtsam::PreintegrationBase::predict
NavState predict(const NavState &state_i, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 6 > H2={}) const
Predict state at time j.
Definition: PreintegrationBase.cpp:117
gtsam::ManifoldPreintegration::deltaVij
Vector3 deltaVij() const override
Definition: ManifoldPreintegration.h:79
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
TEST
TEST(CombinedImuFactor, PreintegratedMeasurements)
Definition: testCombinedImuFactor.cpp:55
kGravity
const double kGravity
Definition: ImuFactorsExample2.cpp:36
forward::scenario
ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0))
Symbol.h
gtsam::PreintegratedCombinedMeasurements::p
Params & p() const
const reference to params, shadows definition in base class
Definition: CombinedImuFactor.h:133
gtsam::ImuFactor
Definition: ImuFactor.h:169
Eigen::Triplet< double >
TestResult
Definition: TestResult.h:26
gtsam::AcceleratingScenario
Accelerating from an arbitrary initial state, with optional rotation.
Definition: Scenario.h:83
imuFactorTesting.h
Common testing infrastructure.
ImuBias.h
gtsam::CombinedImuFactor
Definition: CombinedImuFactor.h:205
Eigen::ArrayBase::pow
const Eigen::CwiseBinaryOp< Eigen::internal::scalar_pow_op< typename Derived::Scalar, typename ExponentDerived::Scalar >, const Derived, const ExponentDerived > pow(const Eigen::ArrayBase< Derived > &x, const Eigen::ArrayBase< ExponentDerived > &exponents)
Definition: GlobalFunctions.h:143
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:316
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
Bias
imuBias::ConstantBias Bias
Definition: testImuBias.cpp:27
p
float * p
Definition: Tutorial_Map_using.cpp:9
v2
Vector v2
Definition: testSerializationBase.cpp:39
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::PreintegratedCombinedMeasurements::preintMeasCov
Matrix preintMeasCov() const
Definition: CombinedImuFactor.h:139
gtsam::NavState::velocity
const Velocity3 & velocity(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:62
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
common::measuredAcc
static const Vector3 measuredAcc
Definition: testImuFactor.cpp:181
Eigen::Matrix< double, 15, 15 >
gtsam::ManifoldPreintegration::deltaPij
Vector3 deltaPij() const override
Definition: ManifoldPreintegration.h:78
CombinedImuFactor.h
gtsam::CombinedScenarioRunner::predict
NavState predict(const PreintegratedCombinedMeasurements &pim, const Bias &estimatedBias=Bias()) const
Predict predict given a PIM.
Definition: ScenarioRunner.cpp:125
M_PI
#define M_PI
Definition: mconf.h:117
gtsam::PreintegratedCombinedMeasurements
Definition: CombinedImuFactor.h:66
gtsam::NavState::pose
const Pose3 pose() const
Definition: NavState.h:90
testing::Params
static std::shared_ptr< PreintegratedCombinedMeasurements::Params > Params(const Matrix3 &biasAccCovariance=Matrix3::Zero(), const Matrix3 &biasOmegaCovariance=Matrix3::Zero(), const Matrix6 &biasAccOmegaInt=Matrix6::Zero())
Definition: testCombinedImuFactor.cpp:39
gtsam::PreintegrationBase::deltaTij
double deltaTij() const
Definition: PreintegrationBase.h:105
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::CombinedScenarioRunner::integrate
PreintegratedCombinedMeasurements integrate(double T, const Bias &estimatedBias=Bias(), bool corrupted=false) const
Integrate measurements for T seconds into a PIM.
Definition: ScenarioRunner.cpp:106
Values.h
A non-templated config holding any types of Manifold-group elements.
Pose3.h
3D Pose manifold SO(3) x R^3 and group SE(3)
v1
Vector v1
Definition: testSerializationBase.cpp:38
ImuFactor.h
testing::SomeMeasurements
Definition: imuFactorTesting.h:65

gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:16:11