Merge remote-tracking branch 'origin/RSS_ImuFactor' into feature/scenarios
Conflicts: gtsam/navigation/ScenarioRunner.h gtsam/navigation/tests/testScenarioRunner.cpprelease/4.3a0
Frank Dellaert
commit
e06ae41c86
|
|
@ -21,78 +21,68 @@
|
|||
|
||||
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.
|
||||
*/
|
||||
/// Simple trajectory simulator.
|
||||
class Scenario {
|
||||
public:
|
||||
// Quantities a Scenario needs to specify:
|
||||
|
||||
virtual Pose3 pose(double t) const = 0;
|
||||
virtual Vector3 omega_b(double t) const = 0;
|
||||
virtual Vector3 velocity_n(double t) const = 0;
|
||||
virtual Vector3 acceleration_n(double t) const = 0;
|
||||
|
||||
// Derived quantities:
|
||||
|
||||
virtual Rot3 rotation(double t) const { return pose(t).rotation(); }
|
||||
|
||||
Vector3 velocity_b(double t) const {
|
||||
const Rot3 nRb = rotation(t);
|
||||
return nRb.transpose() * velocity_n(t);
|
||||
}
|
||||
|
||||
Vector3 acceleration_b(double t) const {
|
||||
const Rot3 nRb = rotation(t);
|
||||
return nRb.transpose() * acceleration_n(t);
|
||||
}
|
||||
};
|
||||
|
||||
/// Scenario with constant twist 3D trajectory.
|
||||
class ExpmapScenario : public 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)
|
||||
ExpmapScenario(const Vector3& w, const Vector3& v)
|
||||
: twist_((Vector6() << w, v).finished()),
|
||||
imuSampleTime_(imuSampleTime),
|
||||
gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
|
||||
accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {}
|
||||
a_b_(twist_.head<3>().cross(twist_.tail<3>())) {}
|
||||
|
||||
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();
|
||||
}
|
||||
Rot3 rotation(double t) const { return Rot3::Expmap(twist_.head<3>() * t); }
|
||||
Vector3 omega_b(double t) const { return twist_.head<3>(); }
|
||||
Vector3 velocity_n(double t) const { return rotation(t) * twist_.tail<3>(); }
|
||||
Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; }
|
||||
|
||||
private:
|
||||
Vector6 twist_;
|
||||
double imuSampleTime_;
|
||||
noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
|
||||
const Vector6 twist_;
|
||||
const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
|
||||
};
|
||||
|
||||
/// Accelerating from an arbitrary initial state
|
||||
class AcceleratingScenario : public Scenario {
|
||||
public:
|
||||
/// Construct scenario with constant twist [w,v]
|
||||
AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
|
||||
const Vector3& accelerationInBody)
|
||||
: nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(nRb_ * accelerationInBody) {}
|
||||
|
||||
Pose3 pose(double t) const {
|
||||
return Pose3(nRb_, p0_ + v0_ * t + a_n_ * t * t / 2.0);
|
||||
}
|
||||
Vector3 omega_b(double t) const { return Vector3::Zero(); }
|
||||
Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; }
|
||||
Vector3 acceleration_n(double t) const { return a_n_; }
|
||||
|
||||
private:
|
||||
const Rot3 nRb_;
|
||||
const Vector3 p0_, v0_, a_n_;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -30,7 +30,29 @@ static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
|
|||
/// Simple class to test navigation scenarios
|
||||
class ScenarioRunner {
|
||||
public:
|
||||
ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {}
|
||||
ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0,
|
||||
double gyroSigma = 0.17, double accSigma = 0.01)
|
||||
: scenario_(scenario),
|
||||
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_n() 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(); }
|
||||
|
||||
/// Integrate measurements for T seconds into a PIM
|
||||
ImuFactor::PreintegratedMeasurements integrate(
|
||||
|
|
@ -40,17 +62,18 @@ class ScenarioRunner {
|
|||
const bool use2ndOrderIntegration = true;
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim(
|
||||
zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(),
|
||||
zeroBias, accCovariance(), gyroCovariance(),
|
||||
kIntegrationErrorCovariance, use2ndOrderIntegration);
|
||||
|
||||
const double dt = scenario_.imuSampleTime();
|
||||
const double dt = 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();
|
||||
Rot3 bRn = scenario_->rotation(t).transpose();
|
||||
Vector3 measuredOmega = scenario_->omega_b(t);
|
||||
if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
|
||||
Vector3 measuredAcc = scenario_.accelerationInBody(t);
|
||||
Vector3 measuredAcc = scenario_->acceleration_b(t) - bRn * gravity_n();
|
||||
if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
|
||||
}
|
||||
|
|
@ -63,12 +86,10 @@ class ScenarioRunner {
|
|||
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 pim.predict(scenario_->pose(0), scenario_->velocity_n(0), zeroBias,
|
||||
gravity_n(), omegaCoriolis, use2ndOrderCoriolis);
|
||||
}
|
||||
|
||||
/// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
|
||||
|
|
@ -76,8 +97,8 @@ class ScenarioRunner {
|
|||
const ImuFactor::PreintegratedMeasurements& pim) const {
|
||||
Matrix9 cov = pim.preintMeasCov();
|
||||
Matrix6 poseCov;
|
||||
poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), //
|
||||
cov.block<3, 3>(3, 0), cov.block<3, 3>(3, 3);
|
||||
poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 0), //
|
||||
cov.block<3, 3>(0, 6), cov.block<3, 3>(0, 0);
|
||||
return poseCov;
|
||||
}
|
||||
|
||||
|
|
@ -87,8 +108,8 @@ class ScenarioRunner {
|
|||
Pose3 prediction = predict(integrate(T)).pose;
|
||||
|
||||
// Create two samplers for acceleration and omega noise
|
||||
Sampler gyroSampler(scenario_.gyroNoiseModel(), 10);
|
||||
Sampler accSampler(scenario_.accNoiseModel(), 29284);
|
||||
Sampler gyroSampler(gyroNoiseModel(), 10);
|
||||
Sampler accSampler(accNoiseModel(), 29284);
|
||||
|
||||
// Sample !
|
||||
Matrix samples(9, N);
|
||||
|
|
@ -114,7 +135,9 @@ class ScenarioRunner {
|
|||
}
|
||||
|
||||
private:
|
||||
Scenario scenario_;
|
||||
const Scenario* scenario_;
|
||||
double imuSampleTime_;
|
||||
noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -27,9 +27,15 @@ 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 Vector3 W(0, 0, 0), V(v, 0, 0);
|
||||
const ExpmapScenario scenario(W, V);
|
||||
|
||||
const Pose3 T15 = forward.pose(15);
|
||||
const double T = 15;
|
||||
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
|
||||
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
|
||||
|
||||
const Pose3 T15 = scenario.pose(T);
|
||||
EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
|
||||
EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9));
|
||||
}
|
||||
|
|
@ -38,11 +44,17 @@ TEST(Scenario, Forward) {
|
|||
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));
|
||||
const Vector3 W(0, 0, w), V(v, 0, 0);
|
||||
const ExpmapScenario scenario(W, V);
|
||||
|
||||
const double T = 15;
|
||||
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
|
||||
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
|
||||
|
||||
// R = v/w, so test if circle is of right size
|
||||
const double R = v / w;
|
||||
const Pose3 T15 = circle.pose(15);
|
||||
const Pose3 T15 = scenario.pose(T);
|
||||
EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9));
|
||||
EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9));
|
||||
}
|
||||
|
|
@ -52,15 +64,44 @@ 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));
|
||||
const Vector3 W(0, -w, 0), V(v, 0, 0);
|
||||
const ExpmapScenario scenario(W, V);
|
||||
|
||||
const double T = 30;
|
||||
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
|
||||
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
|
||||
|
||||
// R = v/w, so test if loop crests at 2*R
|
||||
const double R = v / w;
|
||||
const Pose3 T30 = loop.pose(30);
|
||||
const Pose3 T30 = scenario.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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Scenario, Accelerating) {
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
|
||||
// going in X. The body itself has Z axis pointing down
|
||||
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
const Point3 P0(10, 20, 0);
|
||||
const Vector3 V0(50, 0, 0);
|
||||
|
||||
const double a_b = 0.2; // m/s^2
|
||||
const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0));
|
||||
|
||||
const double T = 3;
|
||||
const Vector3 A = nRb * Vector3(a_b, 0, 0);
|
||||
EXPECT(assert_equal(Vector3(0, 0, 0), scenario.omega_b(T), 1e-9));
|
||||
EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9));
|
||||
EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9));
|
||||
|
||||
const Pose3 T3 = scenario.pose(3);
|
||||
EXPECT(assert_equal(nRb, T3.rotation(), 1e-9));
|
||||
EXPECT(assert_equal(Point3(10 + T * 50, 20 + a_b * T * T / 2, 0),
|
||||
T3.translation(), 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -30,14 +30,13 @@ 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);
|
||||
ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
|
||||
|
||||
ScenarioRunner runner(forward);
|
||||
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9));
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
|
||||
|
||||
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
|
|
@ -47,14 +46,13 @@ TEST(ScenarioRunner, Forward) {
|
|||
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);
|
||||
ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
|
||||
|
||||
ScenarioRunner runner(circle);
|
||||
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1));
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1));
|
||||
|
||||
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
|
|
@ -65,19 +63,39 @@ 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);
|
||||
ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
|
||||
|
||||
ScenarioRunner runner(loop);
|
||||
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1));
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1));
|
||||
|
||||
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Accelerating) {
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
|
||||
// going in X. The body itself has Z axis pointing down
|
||||
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
const Point3 P0(10, 20, 0);
|
||||
const Vector3 V0(50, 0, 0);
|
||||
|
||||
const double a_b = 0.2; // m/s^2
|
||||
const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0));
|
||||
|
||||
const double T = 3; // seconds
|
||||
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
|
||||
|
||||
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue