Merge branch 'RSS_ImuFactor' into feature/scenarios
Conflicts: gtsam_unstable/slam/Mechanization_bRn2.cpprelease/4.3a0
Frank Dellaert
commit
cebd9ed1d3
|
|
@ -1 +1,3 @@
|
||||||
/html/
|
/html/
|
||||||
|
*.lyx~
|
||||||
|
*.bib~
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,98 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 Scenario.h
|
||||||
|
* @brief Simple class to test navigation scenarios
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
#include <gtsam/linear/NoiseModel.h>
|
||||||
|
#include <gtsam/geometry/Pose3.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Simple IMU simulator with constant twist 3D trajectory.
|
||||||
|
* It is also assumed that gravity is magically counteracted and has no effect
|
||||||
|
* on trajectory. Hence, a simulated IMU yields the actual body angular
|
||||||
|
* velocity, and negative G acceleration plus the acceleration created by the
|
||||||
|
* rotating body frame.
|
||||||
|
*/
|
||||||
|
class Scenario {
|
||||||
|
public:
|
||||||
|
/// Construct scenario with constant twist [w,v]
|
||||||
|
Scenario(const Vector3& w, const Vector3& v,
|
||||||
|
double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17,
|
||||||
|
double accSigma = 0.01)
|
||||||
|
: twist_((Vector6() << w, v).finished()),
|
||||||
|
imuSampleTime_(imuSampleTime),
|
||||||
|
gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
|
||||||
|
accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {}
|
||||||
|
|
||||||
|
const double& imuSampleTime() const { return imuSampleTime_; }
|
||||||
|
|
||||||
|
// NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
|
||||||
|
// also, uses g=10 for easy debugging
|
||||||
|
Vector3 gravity() const { return Vector3(0, 0, -10.0); }
|
||||||
|
|
||||||
|
const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const {
|
||||||
|
return gyroNoiseModel_;
|
||||||
|
}
|
||||||
|
|
||||||
|
const noiseModel::Diagonal::shared_ptr& accNoiseModel() const {
|
||||||
|
return accNoiseModel_;
|
||||||
|
}
|
||||||
|
|
||||||
|
Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); }
|
||||||
|
Matrix3 accCovariance() const { return accNoiseModel_->covariance(); }
|
||||||
|
|
||||||
|
Vector3 angularVelocityInBody() const { return twist_.head<3>(); }
|
||||||
|
Vector3 linearVelocityInBody() const { return twist_.tail<3>(); }
|
||||||
|
|
||||||
|
/// Rotation of body in nav frame at time t
|
||||||
|
Rot3 rotAtTime(double t) const {
|
||||||
|
return Rot3::Expmap(angularVelocityInBody() * t);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Pose of body in nav frame at time t
|
||||||
|
Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
|
||||||
|
|
||||||
|
/// Velocity in nav frame at time t
|
||||||
|
Vector3 velocity(double t) const {
|
||||||
|
const Rot3 nRb = rotAtTime(t);
|
||||||
|
return nRb * linearVelocityInBody();
|
||||||
|
}
|
||||||
|
|
||||||
|
// acceleration in nav frame
|
||||||
|
Vector3 acceleration(double t) const {
|
||||||
|
const Vector3 centripetalAcceleration =
|
||||||
|
angularVelocityInBody().cross(linearVelocityInBody());
|
||||||
|
const Rot3 nRb = rotAtTime(t);
|
||||||
|
return nRb * centripetalAcceleration - gravity();
|
||||||
|
}
|
||||||
|
|
||||||
|
// acceleration in body frame frame
|
||||||
|
Vector3 accelerationInBody(double t) const {
|
||||||
|
const Vector3 centripetalAcceleration =
|
||||||
|
angularVelocityInBody().cross(linearVelocityInBody());
|
||||||
|
const Rot3 nRb = rotAtTime(t);
|
||||||
|
return centripetalAcceleration - nRb.transpose() * gravity();
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
Vector6 twist_;
|
||||||
|
double imuSampleTime_;
|
||||||
|
noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
|
@ -0,0 +1,120 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 ScenarioRunner.h
|
||||||
|
* @brief Simple class to test navigation scenarios
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
#include <gtsam/linear/Sampler.h>
|
||||||
|
#include <gtsam/navigation/ImuFactor.h>
|
||||||
|
#include <gtsam/navigation/Scenario.h>
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
static double intNoiseVar = 0.0000001;
|
||||||
|
static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
|
||||||
|
|
||||||
|
/// Simple class to test navigation scenarios
|
||||||
|
class ScenarioRunner {
|
||||||
|
public:
|
||||||
|
ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {}
|
||||||
|
|
||||||
|
/// Integrate measurements for T seconds into a PIM
|
||||||
|
ImuFactor::PreintegratedMeasurements integrate(
|
||||||
|
double T, Sampler* gyroSampler = 0, Sampler* accSampler = 0) const {
|
||||||
|
// TODO(frank): allow non-zero
|
||||||
|
const imuBias::ConstantBias zeroBias;
|
||||||
|
const bool use2ndOrderIntegration = true;
|
||||||
|
|
||||||
|
ImuFactor::PreintegratedMeasurements pim(
|
||||||
|
zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(),
|
||||||
|
kIntegrationErrorCovariance, use2ndOrderIntegration);
|
||||||
|
|
||||||
|
const double dt = scenario_.imuSampleTime();
|
||||||
|
const double sqrt_dt = std::sqrt(dt);
|
||||||
|
const size_t nrSteps = T / dt;
|
||||||
|
double t = 0;
|
||||||
|
for (size_t k = 0; k < nrSteps; k++, t += dt) {
|
||||||
|
Vector3 measuredOmega = scenario_.angularVelocityInBody();
|
||||||
|
if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
|
||||||
|
Vector3 measuredAcc = scenario_.accelerationInBody(t);
|
||||||
|
if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
|
||||||
|
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
|
||||||
|
}
|
||||||
|
|
||||||
|
return pim;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Predict predict given a PIM
|
||||||
|
PoseVelocityBias predict(
|
||||||
|
const ImuFactor::PreintegratedMeasurements& pim) const {
|
||||||
|
// TODO(frank): allow non-zero bias, omegaCoriolis
|
||||||
|
const imuBias::ConstantBias zeroBias;
|
||||||
|
const Pose3 pose_i = Pose3::identity();
|
||||||
|
const Vector3 vel_i = scenario_.velocity(0);
|
||||||
|
const Vector3 omegaCoriolis = Vector3::Zero();
|
||||||
|
const bool use2ndOrderCoriolis = true;
|
||||||
|
return pim.predict(pose_i, vel_i, zeroBias, scenario_.gravity(),
|
||||||
|
omegaCoriolis, use2ndOrderCoriolis);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
|
||||||
|
Matrix6 poseCovariance(
|
||||||
|
const ImuFactor::PreintegratedMeasurements& pim) const {
|
||||||
|
Matrix9 cov = pim.preintMeasCov(); // _ position rotation
|
||||||
|
Matrix6 poseCov;
|
||||||
|
poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 3), //
|
||||||
|
cov.block<3, 3>(3, 6), cov.block<3, 3>(3, 3);
|
||||||
|
return poseCov;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute a Monte Carlo estimate of the PIM pose covariance using N samples
|
||||||
|
Matrix6 estimatePoseCovariance(double T, size_t N = 1000) const {
|
||||||
|
// Get predict prediction from ground truth measurements
|
||||||
|
Pose3 prediction = predict(integrate(T)).pose;
|
||||||
|
|
||||||
|
// Create two samplers for acceleration and omega noise
|
||||||
|
Sampler gyroSampler(scenario_.gyroNoiseModel(), 10);
|
||||||
|
Sampler accSampler(scenario_.accNoiseModel(), 29284);
|
||||||
|
|
||||||
|
// Sample !
|
||||||
|
Matrix samples(9, N);
|
||||||
|
Vector6 sum = Vector6::Zero();
|
||||||
|
for (size_t i = 0; i < N; i++) {
|
||||||
|
Pose3 sampled = predict(integrate(T, &gyroSampler, &accSampler)).pose;
|
||||||
|
Vector6 xi = sampled.localCoordinates(prediction);
|
||||||
|
samples.col(i) = xi;
|
||||||
|
sum += xi;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute MC covariance
|
||||||
|
Vector6 sampleMean = sum / N;
|
||||||
|
Matrix6 Q;
|
||||||
|
Q.setZero();
|
||||||
|
for (size_t i = 0; i < N; i++) {
|
||||||
|
Vector6 xi = samples.col(i);
|
||||||
|
xi -= sampleMean;
|
||||||
|
Q += xi * xi.transpose();
|
||||||
|
}
|
||||||
|
|
||||||
|
return Q / (N - 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
Scenario scenario_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
|
@ -0,0 +1,69 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 testScenario.cpp
|
||||||
|
* @brief Unit test Scenario class
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/navigation/Scenario.h>
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
static const double degree = M_PI / 180.0;
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Scenario, Forward) {
|
||||||
|
const double v = 2; // m/s
|
||||||
|
Scenario forward(Vector3::Zero(), Vector3(v, 0, 0));
|
||||||
|
|
||||||
|
const Pose3 T15 = forward.pose(15);
|
||||||
|
EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
|
||||||
|
EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Scenario, Circle) {
|
||||||
|
// Forward velocity 2m/s, angular velocity 6 degree/sec around Z
|
||||||
|
const double v = 2, w = 6 * degree;
|
||||||
|
Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0));
|
||||||
|
|
||||||
|
// R = v/w, so test if circle is of right size
|
||||||
|
const double R = v / w;
|
||||||
|
const Pose3 T15 = circle.pose(15);
|
||||||
|
EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9));
|
||||||
|
EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Scenario, Loop) {
|
||||||
|
// Forward velocity 2m/s
|
||||||
|
// Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||||
|
const double v = 2, w = 6 * degree;
|
||||||
|
Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0));
|
||||||
|
|
||||||
|
// R = v/w, so test if loop crests at 2*R
|
||||||
|
const double R = v / w;
|
||||||
|
const Pose3 T30 = loop.pose(30);
|
||||||
|
EXPECT(assert_equal(Vector3(-M_PI, 0, -M_PI), T30.rotation().xyz(), 1e-9));
|
||||||
|
EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
@ -0,0 +1,74 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 degree = M_PI / 180.0;
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, Forward) {
|
||||||
|
const double v = 2; // m/s
|
||||||
|
Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.000001, 1);
|
||||||
|
|
||||||
|
ScenarioRunner runner(forward);
|
||||||
|
const double T = 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-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, Circle) {
|
||||||
|
// Forward velocity 2m/s, angular velocity 6 degree/sec
|
||||||
|
const double v = 2, w = 6 * degree;
|
||||||
|
Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0));
|
||||||
|
|
||||||
|
ScenarioRunner runner(circle);
|
||||||
|
const double T = 15; // seconds
|
||||||
|
|
||||||
|
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, Loop) {
|
||||||
|
// Forward velocity 2m/s
|
||||||
|
// Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||||
|
const double v = 2, w = 6 * degree;
|
||||||
|
Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0));
|
||||||
|
|
||||||
|
ScenarioRunner runner(loop);
|
||||||
|
const double T = 30; // seconds
|
||||||
|
|
||||||
|
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
Loading…
Reference in New Issue