gtsam  4.1.0
gtsam
Scenario.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #pragma once
21 
22 namespace gtsam {
23 
25 class Scenario {
26  public:
28  virtual ~Scenario() {}
29 
30  // Quantities a Scenario needs to specify:
31 
32  virtual Pose3 pose(double t) const = 0;
33  virtual Vector3 omega_b(double t) const = 0;
34  virtual Vector3 velocity_n(double t) const = 0;
35  virtual Vector3 acceleration_n(double t) const = 0;
36 
37  // Derived quantities:
38 
39  Rot3 rotation(double t) const { return pose(t).rotation(); }
40  NavState navState(double t) const { return NavState(pose(t), velocity_n(t)); }
41 
42  Vector3 velocity_b(double t) const {
43  const Rot3 nRb = rotation(t);
44  return nRb.transpose() * velocity_n(t);
45  }
46 
47  Vector3 acceleration_b(double t) const {
48  const Rot3 nRb = rotation(t);
49  return nRb.transpose() * acceleration_n(t);
50  }
51 };
52 
61  public:
63  ConstantTwistScenario(const Vector3& w, const Vector3& v,
64  const Pose3& nTb0 = Pose3())
65  : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {}
66 
67  Pose3 pose(double t) const override {
68  return nTb0_ * Pose3::Expmap(twist_ * t);
69  }
70  Vector3 omega_b(double t) const override { return twist_.head<3>(); }
71  Vector3 velocity_n(double t) const override {
72  return rotation(t).matrix() * twist_.tail<3>();
73  }
74  Vector3 acceleration_n(double t) const override { return rotation(t) * a_b_; }
75 
76  private:
77  const Vector6 twist_;
78  const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
79  const Pose3 nTb0_;
80 };
81 
84  public:
87  AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
88  const Vector3& a_n,
89  const Vector3& omega_b = Vector3::Zero())
90  : nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
91 
92  Pose3 pose(double t) const override {
93  return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
94  }
95  Vector3 omega_b(double t) const override { return omega_b_; }
96  Vector3 velocity_n(double t) const override { return v0_ + a_n_ * t; }
97  Vector3 acceleration_n(double t) const override { return a_n_; }
98 
99  private:
100  const Rot3 nRb_;
101  const Vector3 p0_, v0_, a_n_, omega_b_;
102 };
103 
104 } // namespace gtsam
gtsam::Scenario::omega_b
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
gtsam::AcceleratingScenario::acceleration_n
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition: Scenario.h:97
gtsam::AcceleratingScenario::AcceleratingScenario
AcceleratingScenario(const Rot3 &nRb, const Point3 &p0, const Vector3 &v0, const Vector3 &a_n, const Vector3 &omega_b=Vector3::Zero())
Construct scenario with constant acceleration in navigation frame and optional angular velocity in bo...
Definition: Scenario.h:87
gtsam::ConstantTwistScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:67
gtsam::Scenario::pose
virtual Pose3 pose(double t) const =0
pose at time t
NavState.h
Navigation state composing of attitude, position, and velocity.
gtsam::Rot3::matrix
Matrix3 matrix() const
return 3*3 rotation matrix
Definition: Rot3M.cpp:219
gtsam::Pose3::Expmap
static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi=boost::none)
Exponential map at identity - create a rotation from canonical coordinates .
Definition: Pose3.cpp:120
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
gtsam::Scenario
Simple trajectory simulator.
Definition: Scenario.h:25
gtsam::AcceleratingScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:92
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::AcceleratingScenario
Accelerating from an arbitrary initial state, with optional rotation.
Definition: Scenario.h:83
gtsam::ConstantTwistScenario::ConstantTwistScenario
ConstantTwistScenario(const Vector3 &w, const Vector3 &v, const Pose3 &nTb0=Pose3())
Construct scenario with constant twist [w,v].
Definition: Scenario.h:63
gtsam::ConstantTwistScenario::velocity_n
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition: Scenario.h:71
gtsam::AcceleratingScenario::velocity_n
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition: Scenario.h:96
gtsam::Scenario::~Scenario
virtual ~Scenario()
virtual destructor
Definition: Scenario.h:28
gtsam::Scenario::acceleration_n
virtual Vector3 acceleration_n(double t) const =0
acceleration in nav frame
gtsam::Rot3
Definition: Rot3.h:59
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v) const
expmap as required by manifold concept Applies exponential map to v and composes with *this
Definition: Lie.h:77
gtsam::Pose3
Definition: Pose3.h:37
gtsam::ConstantTwistScenario
Scenario with constant twist 3D trajectory.
Definition: Scenario.h:60
gtsam::ConstantTwistScenario::acceleration_n
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition: Scenario.h:74
gtsam::AcceleratingScenario::omega_b
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition: Scenario.h:95
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:63
gtsam::Scenario::velocity_n
virtual Vector3 velocity_n(double t) const =0
velocity at time t, in nav frame
NoiseModel.h
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:266
gtsam::ConstantTwistScenario::omega_b
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition: Scenario.h:70