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

Reduced time for ImuFactor tests

release/4.3a0
Frank Dellaert 2019-06-12 14:39:04 -04:00
parent 6f5e332a98
commit d102a223a5
2 changed files with 19 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -163,11 +163,11 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) {
// Measurements
const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3;
const Vector3 measuredAcc(0, 1.1, -kGravity);
const double deltaT = 0.001;
const double deltaT = 0.01;
PreintegratedCombinedMeasurements pim(p, bias);
for (int i = 0; i < 1000; ++i)
for (int i = 0; i < 100; ++i)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
@ -190,9 +190,9 @@ TEST(CombinedImuFactor, PredictRotation) {
PreintegratedCombinedMeasurements pim(p, bias);
const Vector3 measuredAcc = - kGravityAlongNavZDown;
const Vector3 measuredOmega(0, 0, M_PI / 10.0);
const double deltaT = 0.001;
const double deltaT = 0.01;
const double tol = 1e-4;
for (int i = 0; i < 1000; ++i)
for (int i = 0; i < 100; ++i)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);

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

@ -18,6 +18,8 @@
* @author Stephen Williams
*/
#define ENABLE_TIMING // uncomment for timing results
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/ScenarioRunner.h>
#include <gtsam/geometry/Pose3.h>
@ -111,7 +113,7 @@ TEST(ImuFactor, Accelerating) {
PreintegratedImuMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix9 estimatedCov = runner.estimateCovariance(T);
Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
}
@ -569,6 +571,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
/* ************************************************************************* */
TEST(ImuFactor, PredictPositionAndVelocity) {
gttic(PredictPositionAndVelocity);
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
@ -597,6 +600,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
/* ************************************************************************* */
TEST(ImuFactor, PredictRotation) {
gttic(PredictRotation);
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
@ -623,6 +627,7 @@ TEST(ImuFactor, PredictRotation) {
/* ************************************************************************* */
TEST(ImuFactor, PredictArbitrary) {
gttic(PredictArbitrary);
Pose3 x1;
const Vector3 v1(0, 0, 0);
@ -675,6 +680,7 @@ TEST(ImuFactor, PredictArbitrary) {
/* ************************************************************************* */
TEST(ImuFactor, bodyPSensorNoBias) {
gttic(bodyPSensorNoBias);
Bias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
// Rotate sensor (z-down) to body (same as navigation) i.e. z-up
@ -716,10 +722,11 @@ TEST(ImuFactor, bodyPSensorNoBias) {
#include <gtsam/nonlinear/Marginals.h>
TEST(ImuFactor, bodyPSensorWithBias) {
gttic(bodyPSensorWithBias);
using noiseModel::Diagonal;
typedef Bias Bias;
int numFactors = 80;
int numFactors = 10;
Vector6 noiseBetweenBiasSigma;
noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6,
3.0e-6, 3.0e-6);
@ -899,6 +906,7 @@ TEST(ImuFactor, MergeWithCoriolis) {
/* ************************************************************************* */
// Same values as pre-integration test but now testing covariance
TEST(ImuFactor, CheckCovariance) {
gttic(CheckCovariance);
// Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
@ -922,6 +930,10 @@ TEST(ImuFactor, CheckCovariance) {
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
auto result = TestRegistry::runAllTests(tr);
#ifdef ENABLE_TIMING
tictoc_print_();
#endif
return result;
}
/* ************************************************************************* */