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

Small improvements

release/4.3a0
Frank Dellaert 2015-12-22 18:45:38 -08:00
parent 69fa553495
commit 75385d009b
2 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/navigation/ScenarioRunner.h
View File

@ -24,7 +24,7 @@
namespace gtsam { namespace gtsam {
static double intNoiseVar = 0.0001; static double intNoiseVar = 0.0000001;
static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
/// Simple class to test navigation scenarios /// Simple class to test navigation scenarios
@ -33,16 +33,16 @@ class ScenarioRunner {
ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {} ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {}
/// Integrate measurements for T seconds into a PIM /// Integrate measurements for T seconds into a PIM
ImuFactor::PreintegratedMeasurements integrate( ImuFactor::PreintegratedMeasurements integrate(double T,
double T, boost::optional<Sampler&> gyroSampler = boost::none, Sampler* gyroSampler = 0,
boost::optional<Sampler&> accSampler = boost::none) { Sampler* accSampler = 0) {
// TODO(frank): allow non-zero // TODO(frank): allow non-zero
const imuBias::ConstantBias zeroBias; const imuBias::ConstantBias zeroBias;
const bool use2ndOrderCoriolis = true; const bool use2ndOrderIntegration = true;
ImuFactor::PreintegratedMeasurements pim( ImuFactor::PreintegratedMeasurements pim(
zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(), zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(),
kIntegrationErrorCovariance, use2ndOrderCoriolis); kIntegrationErrorCovariance, use2ndOrderIntegration);
const double dt = scenario_.imuSampleTime(); const double dt = scenario_.imuSampleTime();
const double sqrt_dt = std::sqrt(dt); const double sqrt_dt = std::sqrt(dt);
@ -86,14 +86,14 @@ class ScenarioRunner {
Pose3 prediction = predict(integrate(T)).pose; Pose3 prediction = predict(integrate(T)).pose;
// Create two samplers for acceleration and omega noise // Create two samplers for acceleration and omega noise
Sampler gyroSampler(scenario_.gyroNoiseModel(), 29285); Sampler gyroSampler(scenario_.gyroNoiseModel(), 10);
Sampler accSampler(scenario_.accNoiseModel(), 29284); Sampler accSampler(scenario_.accNoiseModel(), 29284);
// Sample ! // Sample !
Matrix samples(9, N); Matrix samples(9, N);
Vector6 sum = Vector6::Zero(); Vector6 sum = Vector6::Zero();
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i < N; i++) {
Pose3 sampled = predict(integrate(T, gyroSampler, accSampler)).pose; Pose3 sampled = predict(integrate(T, &gyroSampler, &accSampler)).pose;
Vector6 xi = sampled.localCoordinates(prediction); Vector6 xi = sampled.localCoordinates(prediction);
samples.col(i) = xi; samples.col(i) = xi;
sum += xi; sum += xi;
@ -106,10 +106,10 @@ class ScenarioRunner {
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i < N; i++) {
Vector6 xi = samples.col(i); Vector6 xi = samples.col(i);
xi -= sampleMean; xi -= sampleMean;
Q += xi * (xi.transpose() / (N - 1)); Q += xi * xi.transpose();
} }
return Q; return Q / (N - 1);
} }
private: private:

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

@ -27,10 +27,11 @@ static const double degree = M_PI / 180.0;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Forward) {
const double v = 2; // m/s const double v = 2; // m/s
Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.1, 0.00001); Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.000001, 1);
ScenarioRunner runner(forward); ScenarioRunner runner(forward);
const double T = 1; // seconds const double T = 1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9)); EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9));
@ -41,8 +42,8 @@ TEST(ScenarioRunner, Forward) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Circle) { TEST(ScenarioRunner, Circle) {
// Forward velocity 2m/s, angular velocity 6 degree/sec // Forward velocity 2m/s, angular velocity 6 degree/sec
const double v = 2, omega = 6 * degree; const double v = 2, w = 6 * degree;
Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0)); Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0));
ScenarioRunner runner(circle); ScenarioRunner runner(circle);
const double T = 15; // seconds const double T = 15; // seconds
@ -60,6 +61,7 @@ TEST(ScenarioRunner, Loop) {
ScenarioRunner runner(loop); ScenarioRunner runner(loop);
const double T = 30; // seconds const double T = 30; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1)); EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1));
} }