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

Merged in fix/459_scenario_runner (pull request #431) 

deprecated pointer constructor
release/4.3a0
Frank Dellaert 2019-05-17 17:55:25 +00:00
parent ed3228bddd 84eb953ca3
commit 049cf7c96e
3 changed files with 32 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/ScenarioRunner.cpp
View File

@ -49,7 +49,7 @@ PreintegratedImuMeasurements ScenarioRunner::integrate(
NavState ScenarioRunner::predict(const PreintegratedImuMeasurements& pim, NavState ScenarioRunner::predict(const PreintegratedImuMeasurements& pim,
const Bias& estimatedBias) const { const Bias& estimatedBias) const {
const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0)); const NavState state_i(scenario_.pose(0), scenario_.velocity_n(0));
return pim.predict(state_i, estimatedBias); return pim.predict(state_i, estimatedBias);
} }

19
gtsam/navigation/ScenarioRunner.h
View File

@ -42,7 +42,7 @@ class ScenarioRunner {
typedef boost::shared_ptr<PreintegratedImuMeasurements::Params> SharedParams; typedef boost::shared_ptr<PreintegratedImuMeasurements::Params> SharedParams;
private: private:
const Scenario* scenario_; const Scenario& scenario_;
const SharedParams p_; const SharedParams p_;
const double imuSampleTime_, sqrt_dt_; const double imuSampleTime_, sqrt_dt_;
const Bias estimatedBias_; const Bias estimatedBias_;
@ -51,7 +51,7 @@ class ScenarioRunner {
mutable Sampler gyroSampler_, accSampler_; mutable Sampler gyroSampler_, accSampler_;
public: public:
ScenarioRunner(const Scenario* scenario, const SharedParams& p, ScenarioRunner(const Scenario& scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias()) double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias())
: scenario_(scenario), : scenario_(scenario),
p_(p), p_(p),
@ -68,13 +68,13 @@ class ScenarioRunner {
// A gyro simply measures angular velocity in body frame // A gyro simply measures angular velocity in body frame
Vector3 actualAngularVelocity(double t) const { Vector3 actualAngularVelocity(double t) const {
return scenario_->omega_b(t); return scenario_.omega_b(t);
} }
// An accelerometer measures acceleration in body, but not gravity // An accelerometer measures acceleration in body, but not gravity
Vector3 actualSpecificForce(double t) const { Vector3 actualSpecificForce(double t) const {
Rot3 bRn(scenario_->rotation(t).transpose()); Rot3 bRn(scenario_.rotation(t).transpose());
return scenario_->acceleration_b(t) - bRn * gravity_n(); return scenario_.acceleration_b(t) - bRn * gravity_n();
} }
// versions corrupted by bias and noise // versions corrupted by bias and noise
@ -104,6 +104,15 @@ class ScenarioRunner {
/// Estimate covariance of sampled noise for sanity-check /// Estimate covariance of sampled noise for sanity-check
Matrix6 estimateNoiseCovariance(size_t N = 1000) const; Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
ScenarioRunner(const Scenario* scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias())
: ScenarioRunner(*scenario, p, imuSampleTime, bias) {}
/// @}
#endif
}; };
} // namespace gtsam } // namespace gtsam

34
gtsam/navigation/tests/testScenarios.cpp
View File

@ -52,7 +52,7 @@ TEST(ScenarioRunner, Spin) {
const ConstantTwistScenario scenario(W, V); const ConstantTwistScenario scenario(W, V);
auto p = defaultParams(); auto p = defaultParams();
ScenarioRunner runner(&scenario, p, kDt); ScenarioRunner runner(scenario, p, kDt);
const double T = 2 * kDt; // seconds const double T = 2 * kDt; // seconds
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
@ -80,7 +80,7 @@ ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0));
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Forward) {
using namespace forward; using namespace forward;
ScenarioRunner runner(&scenario, defaultParams(), kDt); ScenarioRunner runner(scenario, defaultParams(), kDt);
const double T = 0.1; // seconds const double T = 0.1; // seconds
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
@ -94,7 +94,7 @@ TEST(ScenarioRunner, Forward) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, ForwardWithBias) { TEST(ScenarioRunner, ForwardWithBias) {
using namespace forward; using namespace forward;
ScenarioRunner runner(&scenario, defaultParams(), kDt); ScenarioRunner runner(scenario, defaultParams(), kDt);
const double T = 0.1; // seconds const double T = 0.1; // seconds
auto pim = runner.integrate(T, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias);
@ -108,7 +108,7 @@ TEST(ScenarioRunner, Circle) {
const double v = 2, w = 6 * kDegree; const double v = 2, w = 6 * kDegree;
ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
ScenarioRunner runner(&scenario, defaultParams(), kDt); ScenarioRunner runner(scenario, defaultParams(), kDt);
const double T = 0.1; // seconds const double T = 0.1; // seconds
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
@ -126,7 +126,7 @@ TEST(ScenarioRunner, Loop) {
const double v = 2, w = 6 * kDegree; const double v = 2, w = 6 * kDegree;
ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
ScenarioRunner runner(&scenario, defaultParams(), kDt); ScenarioRunner runner(scenario, defaultParams(), kDt);
const double T = 0.1; // seconds const double T = 0.1; // seconds
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
@ -157,7 +157,7 @@ const double T = 3; // seconds
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Accelerating) { TEST(ScenarioRunner, Accelerating) {
using namespace accelerating; using namespace accelerating;
ScenarioRunner runner(&scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
@ -170,7 +170,7 @@ TEST(ScenarioRunner, Accelerating) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingWithBias) { TEST(ScenarioRunner, AcceleratingWithBias) {
using namespace accelerating; using namespace accelerating;
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
auto pim = runner.integrate(T, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias);
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
@ -185,7 +185,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
const AcceleratingScenario scenario(nRb, P0, V0, A, W); const AcceleratingScenario scenario(nRb, P0, V0, A, W);
const double T = 10; // seconds const double T = 10; // seconds
ScenarioRunner runner(&scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
@ -216,7 +216,7 @@ const double T = 3; // seconds
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Accelerating2) { TEST(ScenarioRunner, Accelerating2) {
using namespace accelerating2; using namespace accelerating2;
ScenarioRunner runner(&scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
@ -229,7 +229,7 @@ TEST(ScenarioRunner, Accelerating2) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingWithBias2) { TEST(ScenarioRunner, AcceleratingWithBias2) {
using namespace accelerating2; using namespace accelerating2;
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
auto pim = runner.integrate(T, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias);
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
@ -244,7 +244,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating2) {
const AcceleratingScenario scenario(nRb, P0, V0, A, W); const AcceleratingScenario scenario(nRb, P0, V0, A, W);
const double T = 10; // seconds const double T = 10; // seconds
ScenarioRunner runner(&scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
@ -276,7 +276,7 @@ const double T = 3; // seconds
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Accelerating3) { TEST(ScenarioRunner, Accelerating3) {
using namespace accelerating3; using namespace accelerating3;
ScenarioRunner runner(&scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
@ -289,7 +289,7 @@ TEST(ScenarioRunner, Accelerating3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingWithBias3) { TEST(ScenarioRunner, AcceleratingWithBias3) {
using namespace accelerating3; using namespace accelerating3;
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
auto pim = runner.integrate(T, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias);
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
@ -304,7 +304,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating3) {
const AcceleratingScenario scenario(nRb, P0, V0, A, W); const AcceleratingScenario scenario(nRb, P0, V0, A, W);
const double T = 10; // seconds const double T = 10; // seconds
ScenarioRunner runner(&scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
@ -337,7 +337,7 @@ const double T = 3; // seconds
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Accelerating4) { TEST(ScenarioRunner, Accelerating4) {
using namespace accelerating4; using namespace accelerating4;
ScenarioRunner runner(&scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
@ -350,7 +350,7 @@ TEST(ScenarioRunner, Accelerating4) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingWithBias4) { TEST(ScenarioRunner, AcceleratingWithBias4) {
using namespace accelerating4; using namespace accelerating4;
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
auto pim = runner.integrate(T, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias);
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
@ -365,7 +365,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) {
const AcceleratingScenario scenario(nRb, P0, V0, A, W); const AcceleratingScenario scenario(nRb, P0, V0, A, W);
const double T = 10; // seconds const double T = 10; // seconds
ScenarioRunner runner(&scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));