65 : twist_((Vector6() << w, v).finished()), a_b_(w.
cross(v)), nTb0_(nTb0) {}
90 : nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(
omega_b) {}
93 return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi={})
Exponential map at identity - create a rotation from canonical coordinates .
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
Vector3 omega_b(double t) const override
angular velocity in body frame
ConstantTwistScenario(const Vector3 &w, const Vector3 &v, const Pose3 &nTb0=Pose3())
Construct scenario with constant twist [w,v].
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
AcceleratingScenario(const Rot3 &nRb, const Point3 &p0, const Vector3 &v0, const Vector3 &a_n, const Vector3 &omega_b=Vector3::Zero())
NavState navState(double t) const
Vector3 omega_b(double t) const override
angular velocity in body frame
Rot3 rotation(double t) const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Pose3 pose(double t) const override
pose at time t
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Array< int, Dynamic, 1 > v
Navigation state composing of attitude, position, and velocity.
Pose3 pose(double t) const override
pose at time t
Simple trajectory simulator.
virtual Vector3 acceleration_n(double t) const =0
acceleration in nav frame
Vector3 velocity_b(double t) const
virtual ~Scenario()
virtual destructor
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Matrix3 transpose() const
Vector3 acceleration_b(double t) const
Accelerating from an arbitrary initial state, with optional rotation.