87 lines
3.0 KiB
C++
87 lines
3.0 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file testScenarioRunner.cpp
|
|
* @brief test ImuFacor with ScenarioRunner class
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <gtsam/navigation/ScenarioRunner.h>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include <cmath>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
static const double kDegree = M_PI / 180.0;
|
|
static const double kDeltaT = 1e-2;
|
|
static const double kGyroSigma = 0.02;
|
|
static const double kAccelerometerSigma = 0.1;
|
|
|
|
/* ************************************************************************* */
|
|
TEST(ScenarioRunner, Forward) {
|
|
const double v = 2; // m/s
|
|
Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT, kGyroSigma,
|
|
kAccelerometerSigma);
|
|
|
|
ScenarioRunner runner(forward);
|
|
const double T = 0.1; // seconds
|
|
|
|
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
|
EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9));
|
|
|
|
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
|
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(ScenarioRunner, Circle) {
|
|
// Forward velocity 2m/s, angular velocity 6 kDegree/sec
|
|
const double v = 2, w = 6 * kDegree;
|
|
Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0), kDeltaT, kGyroSigma,
|
|
kAccelerometerSigma);
|
|
|
|
ScenarioRunner runner(circle);
|
|
const double T = 0.1; // seconds
|
|
|
|
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
|
EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1));
|
|
|
|
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
|
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(ScenarioRunner, Loop) {
|
|
// Forward velocity 2m/s
|
|
// Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
|
|
const double v = 2, w = 6 * kDegree;
|
|
Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0), kDeltaT, kGyroSigma,
|
|
kAccelerometerSigma);
|
|
|
|
ScenarioRunner runner(loop);
|
|
const double T = 0.1; // seconds
|
|
|
|
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
|
EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1));
|
|
|
|
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
|
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|