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

Moved to header file

release/4.3a0
Frank 2015-12-22 11:49:14 -08:00
parent 40bc3149ad
commit 95745015e0
2 changed files with 81 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
gtsam/navigation/ScenarioRunner.h Normal file
View File

@ -0,0 +1,79 @@
/* ----------------------------------------------------------------------------
* 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/navigation/ImuFactor.h>
#include <gtsam/navigation/Scenario.h>
#include <iostream>
namespace gtsam {
double accNoiseVar = 0.01;
double omegaNoiseVar = 0.03;
double intNoiseVar = 0.0001;
const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3;
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
ImuFactor::PreintegratedMeasurements integrate(double T) {
// TODO(frank): allow non-zero
const imuBias::ConstantBias zeroBias;
const bool use2ndOrderCoriolis = true;
ImuFactor::PreintegratedMeasurements result(
zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, use2ndOrderCoriolis);
const Vector3 measuredOmega = scenario_.angularVelocityInBody();
const double deltaT = scenario_.imuSampleTime();
const size_t nrSteps = T / deltaT;
double t = 0;
for (size_t k = 0; k < nrSteps; k++, t += deltaT) {
const Vector3 measuredAcc = scenario_.accelerationInBody(t);
result.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
}
return result;
}
// Predict mean
Pose3 mean(const ImuFactor::PreintegratedMeasurements& integrated) {
// TODO(frank): allow non-standard
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;
const PoseVelocityBias prediction =
integrated.predict(pose_i, vel_i, zeroBias, scenario_.gravity(),
omegaCoriolis, use2ndOrderCoriolis);
return prediction.pose;
}
private:
Scenario scenario_;
};
} // namespace gtsam

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

@ -10,79 +10,12 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file ScenarioRunner.h * @file testScenarioRunner.cpp
* @brief Simple class to test navigation scenarios
* @author Frank Dellaert
*/
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/Scenario.h>
#include <iostream>
namespace gtsam {
double accNoiseVar = 0.01;
double omegaNoiseVar = 0.03;
double intNoiseVar = 0.0001;
const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3;
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
ImuFactor::PreintegratedMeasurements integrate(double T) {
// TODO(frank): allow non-zero
const imuBias::ConstantBias zeroBias;
const bool use2ndOrderCoriolis = true;
ImuFactor::PreintegratedMeasurements result(
zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, use2ndOrderCoriolis);
const Vector3 measuredOmega = scenario_.angularVelocityInBody();
const double deltaT = scenario_.imuSampleTime();
const size_t nrSteps = T / deltaT;
double t = 0;
for (size_t k = 0; k < nrSteps; k++, t += deltaT) {
const Vector3 measuredAcc = scenario_.accelerationInBody(t);
result.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
}
return result;
}
// Predict mean
Pose3 mean(const ImuFactor::PreintegratedMeasurements& integrated) {
// TODO(frank): allow non-standard
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;
const PoseVelocityBias prediction =
integrated.predict(pose_i, vel_i, zeroBias, scenario_.gravity(),
omegaCoriolis, use2ndOrderCoriolis);
return prediction.pose;
}
private:
Scenario scenario_;
};
} // namespace gtsam
/**
* @file testScenario.cpp
* @brief test ImuFacor with ScenarioRunner class * @brief test ImuFacor with ScenarioRunner class
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <gtsam/navigation/Scenario.h> #include <gtsam/navigation/ScenarioRunner.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <cmath> #include <cmath>