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

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

@ -27,10 +27,11 @@ 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.1, 0.00001);
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));
@ -41,8 +42,8 @@ TEST(ScenarioRunner, Forward) {
/* ************************************************************************* */
TEST(ScenarioRunner, Circle) {
// Forward velocity 2m/s, angular velocity 6 degree/sec
const double v = 2, omega = 6 * degree;
Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0));
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
@ -60,6 +61,7 @@ TEST(ScenarioRunner, Loop) {
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));
}