testScenario.cpp
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 
20 
22 #include <cmath>
23 
24 using namespace std::placeholders;
25 using namespace std;
26 using namespace gtsam;
27 
28 static const double kDegree = M_PI / 180.0;
29 
30 /* ************************************************************************* */
31 TEST(Scenario, Spin) {
32  // angular velocity 6 kDegree/sec
33  const double w = 6 * kDegree;
34  const Vector3 W(0, 0, w), V(0, 0, 0);
36 
37  const double T = 10;
38  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
39  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
40  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
41 
42  const Pose3 T10 = scenario.pose(T);
43  EXPECT(assert_equal(Vector3(0, 0, 60 * kDegree), T10.rotation().xyz(), 1e-9));
44  EXPECT(assert_equal(Point3(0, 0, 0), T10.translation(), 1e-9));
45 }
46 
47 /* ************************************************************************* */
49  const double v = 2; // m/s
50  const Vector3 W(0, 0, 0), V(v, 0, 0);
52 
53  const double T = 15;
54  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
55  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
56  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
57 
58  const Pose3 T15 = scenario.pose(T);
59  EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
60  EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9));
61 }
62 
63 /* ************************************************************************* */
64 TEST(Scenario, Circle) {
65  // Forward velocity 2m/s, angular velocity 6 kDegree/sec around Z
66  const double v = 2, w = 6 * kDegree;
67  const Vector3 W(0, 0, w), V(v, 0, 0);
69 
70  const double T = 15;
71  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
72  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
73  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
74 
75  // R = v/w, so test if circle is of right size
76  const double R = v / w;
77  const Pose3 T15 = scenario.pose(T);
78  EXPECT(assert_equal(Vector3(0, 0, 90 * kDegree), T15.rotation().xyz(), 1e-9));
79  EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9));
80 }
81 
82 /* ************************************************************************* */
83 TEST(Scenario, Loop) {
84  // Forward velocity 2m/s
85  // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
86  const double v = 2, w = 6 * kDegree;
87  const Vector3 W(0, -w, 0), V(v, 0, 0);
89 
90  const double T = 30;
91  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
92  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
93  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
94 
95  // R = v/w, so test if loop crests at 2*R
96  const double R = v / w;
97  const Pose3 T30 = scenario.pose(30);
98  EXPECT(assert_equal(Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
99 #ifdef GTSAM_USE_QUATERNIONS
100  EXPECT(assert_equal(Vector3(-M_PI, 0, -M_PI), T30.rotation().xyz()));
101 #else
102  EXPECT(assert_equal(Vector3(M_PI, 0, M_PI), T30.rotation().xyz()));
103 #endif
104  EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
105 }
106 
107 /* ************************************************************************* */
108 TEST(Scenario, LoopWithInitialPose) {
109  // Forward velocity 2m/s
110  // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
111  const double v = 2, w = 6 * kDegree;
112  const Vector3 W(0, -w, 0), V(v, 0, 0);
113  const Rot3 nRb0 = Rot3::Yaw(M_PI);
114  const Pose3 nTb0(nRb0, Point3(1, 2, 3));
115  const ConstantTwistScenario scenario(W, V, nTb0);
116 
117  const double T = 30;
118  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
119  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
120  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
121 
122  // R = v/w, so test if loop crests at 2*R
123  const double R = v / w;
124  const Pose3 T30 = scenario.pose(30);
125  EXPECT(
126  assert_equal(nRb0 * Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
127  EXPECT(assert_equal(Point3(1, 2, 3 + 2 * R), T30.translation(), 1e-9));
128 }
129 
130 /* ************************************************************************* */
131 TEST(Scenario, Accelerating) {
132  // Set up body pointing towards y axis, and start at 10,20,0 with velocity
133  // going in X. The body itself has Z axis pointing down
134  const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
135  const Point3 P0(10, 20, 0);
136  const Vector3 V0(50, 0, 0);
137 
138  const double a = 0.2; // m/s^2
139  const Vector3 A(0, a, 0), W(0.1, 0.2, 0.3);
140  const AcceleratingScenario scenario(nRb, P0, V0, A, W);
141 
142  const double T = 3;
143  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
144  EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9));
145  EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9));
146 
147  {
148  // Check acceleration in nav
149  Matrix expected = numericalDerivative11<Vector3, double>(
150  std::bind(&Scenario::velocity_n, scenario, std::placeholders::_1), T);
151  EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9));
152  }
153 
154  const Pose3 T3 = scenario.pose(3);
155  EXPECT(assert_equal(nRb.expmap(T * W), T3.rotation(), 1e-9));
156  EXPECT(assert_equal(Point3(10 + T * 50, 20 + a * T * T / 2, 0),
157  T3.translation(), 1e-9));
158 }
159 
160 /* ************************************************************************* */
161 int main() {
162  TestResult tr;
163  return TestRegistry::runAllTests(tr);
164 }
165 /* ************************************************************************* */
Simple class to test navigation scenarios.
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
const Vector3 V0(0, 0, 0)
static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3))
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition: Scenario.h:95
TEST(Scenario, Spin)
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition: Scenario.h:96
Matrix expected
Definition: testMatrix.cpp:971
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
#define M_PI
Definition: main.h:106
Rot2 R(Rot2::fromAngle(0.1))
Definition: BFloat16.h:88
Key W(std::uint64_t j)
Some functions to compute numerical derivatives.
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition: Scenario.h:70
int main()
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 xyz(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:160
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition: Scenario.h:97
#define EXPECT(condition)
Definition: Test.h:150
Array< int, Dynamic, 1 > v
Eigen::Triplet< double > T
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0))
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
Vector3 velocity_b(double t) const
Definition: Scenario.h:42
Class expmap(const TangentVector &v) const
Definition: Lie.h:78
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
const Point3 P0(0, 0, 0)
Vector3 Point3
Definition: Point3.h:38
static const double kDegree
Vector3 acceleration_b(double t) const
Definition: Scenario.h:47
Accelerating from an arbitrary initial state, with optional rotation.
Definition: Scenario.h:83


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:18