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

Increased # MC samples

release/4.3a0
Frank 2015-10-19 15:20:07 -07:00
parent 935801d8e1
commit ee5bd7ac39
1 changed files with 4 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -163,8 +163,6 @@ bool MonteCarlo(const PreintegratedImuMeasurements& pim,
return assert_equal(Matrix(Q), actual, 1e-4); return assert_equal(Matrix(Q), actual, 1e-4);
} }
#if 1
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, StraightLine) { TEST(ImuFactor, StraightLine) {
// Set up IMU measurements // Set up IMU measurements
@ -610,7 +608,6 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
EXPECT( EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega())); assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega()));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& measuredAcc, const Vector3& measuredOmega) { Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& measuredAcc, const Vector3& measuredOmega) {
@ -679,7 +676,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
hist(samples(:,i), 500) hist(samples(:,i), 500)
end end
*/ */
size_t N = 10000; size_t N = 100000;
Matrix samples(9,N); Matrix samples(9,N);
Sampler sampleAccelerationNoise((accNoiseVar2/dt).cwiseSqrt(), 29284); Sampler sampleAccelerationNoise((accNoiseVar2/dt).cwiseSqrt(), 29284);
Sampler sampleOmegaNoise((omegaNoiseVar2/dt).cwiseSqrt(), 10); Sampler sampleOmegaNoise((omegaNoiseVar2/dt).cwiseSqrt(), 10);
@ -698,7 +695,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
#endif #endif
EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, dt, body_P_sensor, EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, dt, body_P_sensor,
measuredAcc, measuredOmega, accNoiseVar2, omegaNoiseVar2, 10000)); measuredAcc, measuredOmega, accNoiseVar2, omegaNoiseVar2, 100000));
// Matrix expected(9,9); // Matrix expected(9,9);
// expected << // expected <<
@ -837,8 +834,8 @@ TEST(ImuFactor, PredictArbitrary) {
Pose3 x1; Pose3 x1;
Vector3 v1 = Vector3(0, 0, 0); Vector3 v1 = Vector3(0, 0, 0);
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega,
measuredAcc, measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar))); Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000));
for (int i = 0; i < 1000; ++i) for (int i = 0; i < 1000; ++i)
pim.integrateMeasurement(measuredAcc, measuredOmega, dt); pim.integrateMeasurement(measuredAcc, measuredOmega, dt);