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

Skeleton compiles/links

release/4.3a0
Frank Dellaert 2015-12-28 07:11:45 -08:00
parent 7834ac08df
commit d0b020b6e8
3 changed files with 56 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/navigation/ScenarioRunner.cpp
View File

@ -21,13 +21,26 @@
namespace gtsam {
////////////////////////////////////////////////////////////////////////////////////////////
void PreintegratedMeasurements2::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {}
NavState PreintegratedMeasurements2::predict(
const NavState& state_i, const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const {
return NavState();
}
////////////////////////////////////////////////////////////////////////////////////////////
static double intNoiseVar = 0.0000001;
static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
PreintegratedMeasurements2 ScenarioRunner::integrate(
double T, const imuBias::ConstantBias& estimatedBias,
bool corrupted) const {
ImuFactor::PreintegratedMeasurements pim(p_, estimatedBias);
PreintegratedMeasurements2 pim(p_, estimatedBias);
const double dt = imuSampleTime();
const size_t nrSteps = T / dt;
@ -43,7 +56,7 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
}
NavState ScenarioRunner::predict(
const ImuFactor::PreintegratedMeasurements& pim,
const PreintegratedMeasurements2& pim,
const imuBias::ConstantBias& estimatedBias) const {
const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0));
return pim.predict(state_i, estimatedBias);

38
gtsam/navigation/ScenarioRunner.h
View File

@ -22,14 +22,41 @@
namespace gtsam {
/**
* Class that integrates on the manifold
*/
class PreintegratedMeasurements2 {
public:
typedef ImuFactor::PreintegratedMeasurements::Params Params;
Matrix9 preintMeasCov() const { return Matrix9::Zero(); }
PreintegratedMeasurements2(
const boost::shared_ptr<Params>& p,
const gtsam::imuBias::ConstantBias& estimatedBias) {}
/**
* Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame)
* @param measuredOmega Measured angular velocity (in body frame)
* @param dt Time interval between this and the last IMU measurement
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt);
/// Predict state at time j
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 6> H2 = boost::none) const;
};
/*
* Simple class to test navigation scenarios.
* Takes a trajectory scenario as input, and can generate IMU measurements
*/
class ScenarioRunner {
public:
typedef boost::shared_ptr<ImuFactor::PreintegratedMeasurements::Params>
SharedParams;
typedef boost::shared_ptr<PreintegratedMeasurements2::Params> SharedParams;
ScenarioRunner(const Scenario* scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0,
const imuBias::ConstantBias& bias = imuBias::ConstantBias())
@ -67,19 +94,18 @@ class ScenarioRunner {
const double& imuSampleTime() const { return imuSampleTime_; }
/// Integrate measurements for T seconds into a PIM
ImuFactor::PreintegratedMeasurements integrate(
PreintegratedMeasurements2 integrate(
double T,
const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(),
bool corrupted = false) const;
/// Predict predict given a PIM
NavState predict(const ImuFactor::PreintegratedMeasurements& pim,
NavState predict(const PreintegratedMeasurements2& pim,
const imuBias::ConstantBias& estimatedBias =
imuBias::ConstantBias()) const;
/// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
Matrix6 poseCovariance(
const ImuFactor::PreintegratedMeasurements& pim) const {
Matrix6 poseCovariance(const PreintegratedMeasurements2& pim) const {
Matrix9 cov = pim.preintMeasCov();
Matrix6 poseCov;
poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), //

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

@ -31,7 +31,7 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
static boost::shared_ptr<PreintegratedMeasurements2::Params> defaultParams() {
auto p = PreintegratedImuMeasurements::Params::MakeSharedD(10);
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
@ -50,7 +50,7 @@ TEST(ScenarioRunner, Forward) {
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@ -63,7 +63,7 @@ TEST(ScenarioRunner, ForwardWithBias) {
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias);
auto pim = runner.integrate(T, kNonZeroBias);
Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
}
@ -77,7 +77,7 @@ TEST(ScenarioRunner, Circle) {
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@ -94,7 +94,7 @@ TEST(ScenarioRunner, Loop) {
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@ -125,7 +125,7 @@ TEST(ScenarioRunner, Accelerating) {
using namespace accelerating;
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@ -139,7 +139,7 @@ TEST(ScenarioRunner, Accelerating) {
// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
// kNonZeroBias);
//
// ImuFactor::PreintegratedMeasurements pim = runner.integrate(T,
// auto pim = runner.integrate(T,
// kNonZeroBias);
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
// kNonZeroBias);
@ -156,7 +156,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
const double T = 3; // seconds
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);