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:
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
static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi={})
Exponential map at identity - create a rotation from canonical coordinates .
Definition: Pose3.cpp:169
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
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition: Scenario.h:95
ConstantTwistScenario(const Vector3 &w, const Vector3 &v, const Pose3 &nTb0=Pose3())
Construct scenario with constant twist [w,v].
Definition: Scenario.h:63
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition: Scenario.h:96
AcceleratingScenario(const Rot3 &nRb, const Point3 &p0, const Vector3 &v0, const Vector3 &a_n, const Vector3 &omega_b=Vector3::Zero())
Definition: Scenario.h:87
NavState navState(double t) const
Definition: Scenario.h:40
Vector3f p0
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition: Scenario.h:70
Rot3 rotation(double t) const
Definition: Scenario.h:39
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
const Rot3 nRb
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:67
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition: Scenario.h:97
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:64
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition: Scenario.h:71
Array< int, Dynamic, 1 > v
RowVector3d w
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:92
Simple trajectory simulator.
Definition: Scenario.h:25
traits
Definition: chartTesting.h:28
virtual Vector3 acceleration_n(double t) const =0
acceleration in nav frame
static const double v0
Matrix3 matrix() const
Definition: Rot3M.cpp:218
Vector3 velocity_b(double t) const
Definition: Scenario.h:42
virtual ~Scenario()
virtual destructor
Definition: Scenario.h:28
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
Vector3 Point3
Definition: Point3.h:38
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition: Scenario.h:74
Matrix3 transpose() const
Definition: Rot3M.cpp:143
Vector3 acceleration_b(double t) const
Definition: Scenario.h:47
Accelerating from an arbitrary initial state, with optional rotation.
Definition: Scenario.h:83
Point2 t(10, 10)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:36