gtsam 4.1.1
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
22namespace gtsam {
23
25class 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
Navigation state composing of attitude, position, and velocity.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:63
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
Class expmap(const TangentVector &v) const
expmap as required by manifold concept Applies exponential map to v and composes with *this
Definition: Lie.h:78
Definition: Pose3.h:37
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:164
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:310
Definition: Rot3.h:59
Matrix3 matrix() const
return 3*3 rotation matrix
Definition: Rot3M.cpp:219
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
Simple trajectory simulator.
Definition: Scenario.h:25
virtual Pose3 pose(double t) const =0
pose at time t
virtual Vector3 acceleration_n(double t) const =0
acceleration in nav frame
virtual Vector3 velocity_n(double t) const =0
velocity at time t, in nav frame
virtual ~Scenario()
virtual destructor
Definition: Scenario.h:28
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
Scenario with constant twist 3D trajectory.
Definition: Scenario.h:60
ConstantTwistScenario(const Vector3 &w, const Vector3 &v, const Pose3 &nTb0=Pose3())
Construct scenario with constant twist [w,v].
Definition: Scenario.h:63
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition: Scenario.h:74
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition: Scenario.h:70
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:67
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition: Scenario.h:71
Accelerating from an arbitrary initial state, with optional rotation.
Definition: Scenario.h:83
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:92
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition: Scenario.h:95
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
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition: Scenario.h:97
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition: Scenario.h:96