105 lines
3.5 KiB
C++
105 lines
3.5 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 Scenario.h
|
|
* @brief Simple class to test navigation scenarios
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#pragma once
|
|
#include <gtsam/linear/NoiseModel.h>
|
|
#include <gtsam/navigation/NavState.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/// Simple trajectory simulator.
|
|
class Scenario {
|
|
public:
|
|
/// virtual destructor
|
|
virtual ~Scenario() {}
|
|
|
|
// Quantities a Scenario needs to specify:
|
|
|
|
virtual Pose3 pose(double t) const = 0; ///< pose at time t
|
|
virtual Vector3 omega_b(double t) const = 0; ///< angular velocity in body frame
|
|
virtual Vector3 velocity_n(double t) const = 0; ///< velocity at time t, in nav frame
|
|
virtual Vector3 acceleration_n(double t) const = 0; ///< acceleration in nav frame
|
|
|
|
// Derived quantities:
|
|
|
|
Rot3 rotation(double t) const { return pose(t).rotation(); }
|
|
NavState navState(double t) const { return NavState(pose(t), velocity_n(t)); }
|
|
|
|
Vector3 velocity_b(double t) const {
|
|
const Rot3 nRb = rotation(t);
|
|
return nRb.transpose() * velocity_n(t);
|
|
}
|
|
|
|
Vector3 acceleration_b(double t) const {
|
|
const Rot3 nRb = rotation(t);
|
|
return nRb.transpose() * acceleration_n(t);
|
|
}
|
|
};
|
|
|
|
/**
|
|
* Scenario with constant twist 3D trajectory.
|
|
* Note that a plane flying level does not feel any acceleration: gravity is
|
|
* being perfectly compensated by the lift generated by the wings, and drag is
|
|
* compensated by thrust. The accelerometer will add the gravity component back
|
|
* in, as modeled by the specificForce method in ScenarioRunner.
|
|
*/
|
|
class ConstantTwistScenario : public Scenario {
|
|
public:
|
|
/// Construct scenario with constant twist [w,v]
|
|
ConstantTwistScenario(const Vector3& w, const Vector3& v,
|
|
const Pose3& nTb0 = Pose3())
|
|
: twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {}
|
|
|
|
Pose3 pose(double t) const override {
|
|
return nTb0_ * Pose3::Expmap(twist_ * t);
|
|
}
|
|
Vector3 omega_b(double t) const override { return twist_.head<3>(); }
|
|
Vector3 velocity_n(double t) const override {
|
|
return rotation(t).matrix() * twist_.tail<3>();
|
|
}
|
|
Vector3 acceleration_n(double t) const override { return rotation(t) * a_b_; }
|
|
|
|
private:
|
|
const Vector6 twist_;
|
|
const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
|
|
const Pose3 nTb0_;
|
|
};
|
|
|
|
/// Accelerating from an arbitrary initial state, with optional rotation
|
|
class AcceleratingScenario : public Scenario {
|
|
public:
|
|
/// Construct scenario with constant acceleration in navigation frame and
|
|
/// optional angular velocity in body: rotating while translating...
|
|
AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
|
|
const Vector3& a_n,
|
|
const Vector3& omega_b = Vector3::Zero())
|
|
: nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
|
|
|
|
Pose3 pose(double t) const override {
|
|
return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
|
|
}
|
|
Vector3 omega_b(double t) const override { return omega_b_; }
|
|
Vector3 velocity_n(double t) const override { return v0_ + a_n_ * t; }
|
|
Vector3 acceleration_n(double t) const override { return a_n_; }
|
|
|
|
private:
|
|
const Rot3 nRb_;
|
|
const Vector3 p0_, v0_, a_n_, omega_b_;
|
|
};
|
|
|
|
} // namespace gtsam
|