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

Split off Scenario abstract base class

release/4.3a0
Frank Dellaert 2015-12-23 09:21:54 -08:00
parent 4129c9651a
commit dfe3f3a348
4 changed files with 65 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
gtsam/navigation/Scenario.h
View File

@ -22,7 +22,7 @@
namespace gtsam {
/**
* Simple IMU simulator with constant twist 3D trajectory.
* Simple IMU simulator.
* 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
@ -31,11 +31,9 @@ namespace gtsam {
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,
Scenario(double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17,
double accSigma = 0.01)
: twist_((Vector6() << w, v).finished()),
imuSampleTime_(imuSampleTime),
: imuSampleTime_(imuSampleTime),
gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {}
@ -56,43 +54,58 @@ class Scenario {
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>(); }
/// Pose of body in nav frame at time t
virtual Pose3 pose(double t) const = 0;
/// Rotation of body in nav frame at time t
Rot3 rotAtTime(double t) const {
return Rot3::Expmap(angularVelocityInBody() * t);
}
virtual Rot3 rotation(double t) const { return pose(t).rotation(); }
/// Pose of body in nav frame at time t
Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
virtual Vector3 angularVelocityInBody(double t) const = 0;
virtual Vector3 linearVelocityInBody(double t) const = 0;
virtual Vector3 accelerationInBody(double t) const = 0;
/// Velocity in nav frame at time t
Vector3 velocity(double t) const {
const Rot3 nRb = rotAtTime(t);
return nRb * linearVelocityInBody();
return rotation(t) * linearVelocityInBody(t);
}
// acceleration in nav frame
// TODO(frank): correct for rotating frames?
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();
return rotation(t) * accelerationInBody(t);
}
private:
Vector6 twist_;
double imuSampleTime_;
noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
};
/**
* Simple IMU simulator with constant twist 3D trajectory.
*/
class ExpmapScenario : public Scenario {
public:
/// Construct scenario with constant twist [w,v]
ExpmapScenario(const Vector3& w, const Vector3& v,
double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17,
double accSigma = 0.01)
: Scenario(imuSampleTime, gyroSigma, accSigma),
twist_((Vector6() << w, v).finished()),
centripetalAcceleration_(twist_.head<3>().cross(twist_.tail<3>())) {}
Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
Vector3 angularVelocityInBody(double t) const { return twist_.head<3>(); }
Vector3 linearVelocityInBody(double t) const { return twist_.tail<3>(); }
Vector3 accelerationInBody(double t) const {
const Rot3 nRb = rotation(t);
return centripetalAcceleration_ - nRb.transpose() * gravity();
}
private:
const Vector6 twist_;
const Vector3 centripetalAcceleration_;
};
} // namespace gtsam

20
gtsam/navigation/ScenarioRunner.h
View File

@ -30,7 +30,7 @@ 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) : scenario_(scenario) {}
/// Integrate measurements for T seconds into a PIM
ImuFactor::PreintegratedMeasurements integrate(
@ -40,17 +40,17 @@ class ScenarioRunner {
const bool use2ndOrderIntegration = true;
ImuFactor::PreintegratedMeasurements pim(
zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(),
zeroBias, scenario_->accCovariance(), scenario_->gyroCovariance(),
kIntegrationErrorCovariance, use2ndOrderIntegration);
const double dt = scenario_.imuSampleTime();
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();
Vector3 measuredOmega = scenario_->angularVelocityInBody(t);
if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
Vector3 measuredAcc = scenario_.accelerationInBody(t);
Vector3 measuredAcc = scenario_->accelerationInBody(t);
if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
}
@ -64,10 +64,10 @@ class ScenarioRunner {
// 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 vel_i = scenario_->velocity(0);
const Vector3 omegaCoriolis = Vector3::Zero();
const bool use2ndOrderCoriolis = true;
return pim.predict(pose_i, vel_i, zeroBias, scenario_.gravity(),
return pim.predict(pose_i, vel_i, zeroBias, scenario_->gravity(),
omegaCoriolis, use2ndOrderCoriolis);
}
@ -87,8 +87,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(scenario_->gyroNoiseModel(), 10);
Sampler accSampler(scenario_->accNoiseModel(), 29284);
// Sample !
Matrix samples(9, N);
@ -114,7 +114,7 @@ class ScenarioRunner {
}
private:
Scenario scenario_;
const Scenario* scenario_;
};
} // namespace gtsam

6
gtsam/navigation/tests/testScenario.cpp
View File

@ -27,7 +27,7 @@ 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));
ExpmapScenario 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));
@ -38,7 +38,7 @@ 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));
ExpmapScenario 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;
@ -52,7 +52,7 @@ 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));
ExpmapScenario 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;

24
gtsam/navigation/tests/testScenarioRunner.cpp
View File

@ -30,14 +30,14 @@ 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), kDeltaT,
kGyroSigma, kAccelerometerSigma);
ScenarioRunner runner(forward);
ScenarioRunner runner(&scenario);
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 +47,14 @@ 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), kDeltaT,
kGyroSigma, kAccelerometerSigma);
ScenarioRunner runner(circle);
ScenarioRunner runner(&scenario);
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,14 +65,14 @@ 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), kDeltaT,
kGyroSigma, kAccelerometerSigma);
ScenarioRunner runner(loop);
ScenarioRunner runner(&scenario);
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));