testNavState.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 
19 #include <gtsam/navigation/NavState.h>
20 #include <gtsam/base/TestableAssertions.h>
21 #include <gtsam/base/numericalDerivative.h>
22 
23 #include <CppUnitLite/TestHarness.h>
24 
25 using namespace std::placeholders;
26 using namespace std;
27 using namespace gtsam;
28 
29 static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3);
30 static const Point3 kPosition(1.0, 2.0, 3.0);
31 static const Pose3 kPose(kAttitude, kPosition);
32 static const Velocity3 kVelocity(0.4, 0.5, 0.6);
33 static const NavState kIdentity;
34 static const NavState kState1(kAttitude, kPosition, kVelocity);
35 static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04);
36 static const Vector3 kGravity(0, 0, 9.81);
37 static const Vector9 kZeroXi = Vector9::Zero();
38 
39 /* ************************************************************************* */
40 TEST(NavState, Constructor) {
41  std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
42  std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2,
43  std::placeholders::_3, nullptr, nullptr, nullptr);
44  Matrix aH1, aH2, aH3;
45  EXPECT(
46  assert_equal(kState1,
47  NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3)));
48  EXPECT(
49  assert_equal(
50  numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
51  EXPECT(
52  assert_equal(
53  numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
54  EXPECT(
55  assert_equal(
56  numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
57 }
58 
59 /* ************************************************************************* */
60 TEST(NavState, Constructor2) {
61  std::function<NavState(const Pose3&, const Vector3&)> construct =
62  std::bind(&NavState::FromPoseVelocity, std::placeholders::_1,
63  std::placeholders::_2, nullptr, nullptr);
64  Matrix aH1, aH2;
65  EXPECT(
66  assert_equal(kState1,
67  NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
68  EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1));
69  EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2));
70 }
71 
72 /* ************************************************************************* */
73 TEST( NavState, Attitude) {
74  Matrix39 aH, eH;
75  Rot3 actual = kState1.attitude(aH);
76  EXPECT(assert_equal(actual, kAttitude));
77  eH = numericalDerivative11<Rot3, NavState>(
78  std::bind(&NavState::attitude, std::placeholders::_1, nullptr), kState1);
79  EXPECT(assert_equal((Matrix )eH, aH));
80 }
81 
82 /* ************************************************************************* */
83 TEST( NavState, Position) {
84  Matrix39 aH, eH;
85  Point3 actual = kState1.position(aH);
86  EXPECT(assert_equal(actual, kPosition));
87  eH = numericalDerivative11<Point3, NavState>(
88  std::bind(&NavState::position, std::placeholders::_1, nullptr),
89  kState1);
90  EXPECT(assert_equal((Matrix )eH, aH));
91 }
92 
93 /* ************************************************************************* */
94 TEST( NavState, Velocity) {
95  Matrix39 aH, eH;
96  Velocity3 actual = kState1.velocity(aH);
97  EXPECT(assert_equal(actual, kVelocity));
98  eH = numericalDerivative11<Velocity3, NavState>(
99  std::bind(&NavState::velocity, std::placeholders::_1, nullptr),
100  kState1);
101  EXPECT(assert_equal((Matrix )eH, aH));
102 }
103 
104 /* ************************************************************************* */
105 TEST( NavState, BodyVelocity) {
106  Matrix39 aH, eH;
107  Velocity3 actual = kState1.bodyVelocity(aH);
108  EXPECT(assert_equal<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
109  eH = numericalDerivative11<Velocity3, NavState>(
110  std::bind(&NavState::bodyVelocity, std::placeholders::_1, nullptr),
111  kState1);
112  EXPECT(assert_equal((Matrix )eH, aH));
113 }
114 
115 /* ************************************************************************* */
116 TEST( NavState, Manifold ) {
117  // Check zero xi
118  EXPECT(assert_equal(kIdentity, kIdentity.retract(kZeroXi)));
119  EXPECT(assert_equal(kZeroXi, kIdentity.localCoordinates(kIdentity)));
120  EXPECT(assert_equal(kState1, kState1.retract(kZeroXi)));
121  EXPECT(assert_equal(kZeroXi, kState1.localCoordinates(kState1)));
122 
123  // Check definition of retract as operating on components separately
124  Vector9 xi;
125  xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
126  Rot3 drot = Rot3::Expmap(xi.head<3>());
127  Point3 dt = Point3(xi.segment<3>(3));
128  Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
129  NavState state2 = NavState(kState1.attitude() * drot,
130  kState1.position() + kState1.attitude() * dt,
131  kState1.velocity() + kState1.attitude() * dvel);
132  EXPECT(assert_equal(state2, kState1.retract(xi)));
133  EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
134 
135  // roundtrip from state2 to state3 and back
136  NavState state3 = state2.retract(xi);
137  EXPECT(assert_equal(xi, state2.localCoordinates(state3)));
138 
139  // Check retract derivatives
140  Matrix9 aH1, aH2;
141  kState1.retract(xi, aH1, aH2);
142  std::function<NavState(const NavState&, const Vector9&)> retract =
143  std::bind(&NavState::retract, std::placeholders::_1,
144  std::placeholders::_2, nullptr, nullptr);
145  EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
146  EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
147 
148  // Check retract derivatives on state 2
149  const Vector9 xi2 = -3.0*xi;
150  state2.retract(xi2, aH1, aH2);
151  EXPECT(assert_equal(numericalDerivative21(retract, state2, xi2), aH1));
152  EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2));
153 
154  // Check localCoordinates derivatives
155  std::function<Vector9(const NavState&, const NavState&)> local =
156  std::bind(&NavState::localCoordinates, std::placeholders::_1,
157  std::placeholders::_2, nullptr, nullptr);
158  // from state1 to state2
159  kState1.localCoordinates(state2, aH1, aH2);
160  EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
161  EXPECT(assert_equal(numericalDerivative22(local, kState1, state2), aH2));
162  // from identity to state2
163  kIdentity.localCoordinates(state2, aH1, aH2);
164  EXPECT(assert_equal(numericalDerivative21(local, kIdentity, state2), aH1));
165  EXPECT(assert_equal(numericalDerivative22(local, kIdentity, state2), aH2));
166  // from state2 to identity
167  state2.localCoordinates(kIdentity, aH1, aH2);
168  EXPECT(assert_equal(numericalDerivative21(local, state2, kIdentity), aH1));
169  EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
170 }
171 
172 /* ************************************************************************* */
173 static const double dt = 2.0;
174 std::function<Vector9(const NavState&, const bool&)> coriolis =
175  std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis,
176  std::placeholders::_2, nullptr);
177 
178 TEST(NavState, Coriolis) {
179  Matrix9 aH;
180 
181  // first-order
182  kState1.coriolis(dt, kOmegaCoriolis, false, aH);
183  EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), aH));
184  // second-order
185  kState1.coriolis(dt, kOmegaCoriolis, true, aH);
186  EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), aH));
187 }
188 
189 TEST(NavState, Coriolis2) {
190  Matrix9 aH;
191  NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
192  Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
193 
194  // first-order
195  state2.coriolis(dt, kOmegaCoriolis, false, aH);
196  EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), aH));
197  // second-order
198  state2.coriolis(dt, kOmegaCoriolis, true, aH);
199  EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH));
200 }
201 
202 TEST(NavState, Coriolis3) {
210  // Get true first and second order coriolis accelerations
211  double dt = 2.0, dt2 = dt * dt;
212  Vector3 n_omega(0.0, 0.0, 1.0), n_t(1.0, 0.0, 0.0), n_v(0.0, 1.0, 0.0);
213  Vector3 n_aCorr1 = -2.0 * n_omega.cross(n_v),
214  n_aCorr2 = -n_omega.cross(n_omega.cross(n_t));
215  Rot3 nRb = Rot3(-1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0),
216  bRn = nRb.inverse();
217 
218  // Get expected first and second order corrections in the nav frame
219  Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1,
220  n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2),
221  n_dV1e = dt * n_aCorr1,
222  n_dV2e = dt * (n_aCorr1 + n_aCorr2);
223 
224  // Get expected first and second order corrections in the body frame
225  Vector3 dRe = -dt * (bRn * n_omega),
226  b_dP1e = bRn * n_dP1e, b_dP2e = bRn * n_dP2e,
227  b_dV1e = bRn * n_dV1e, b_dV2e = bRn * n_dV2e;
228 
229  // Get actual first and scond order corrections in body frame
230  NavState kState2(nRb, n_t, n_v);
231  Vector9 dXi1a = kState2.coriolis(dt, n_omega, false),
232  dXi2a = kState2.coriolis(dt, n_omega, true);
233  Vector3 dRa = NavState::dR(dXi1a),
234  b_dP1a = NavState::dP(dXi1a), b_dV1a = NavState::dV(dXi1a),
235  b_dP2a = NavState::dP(dXi2a), b_dV2a = NavState::dV(dXi2a);
236 
237  EXPECT(assert_equal(dRe, dRa));
238  EXPECT(assert_equal(b_dP1e, b_dP1a));
239  EXPECT(assert_equal(b_dV1e, b_dV1a));
240  EXPECT(assert_equal(b_dP2e, b_dP2a));
241  EXPECT(assert_equal(b_dV2e, b_dV2a));
242 
243 }
244 
245 /* ************************************************************************* */
246 TEST(NavState, CorrectPIM) {
247  Vector9 xi;
248  xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
249  double dt = 0.5;
250  Matrix9 aH1, aH2;
251  std::function<Vector9(const NavState&, const Vector9&)> correctPIM =
252  std::bind(&NavState::correctPIM, std::placeholders::_1,
253  std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false,
254  nullptr, nullptr);
255  kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2);
256  EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1));
257  EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));
258 }
259 
260 /* ************************************************************************* */
261 TEST(NavState, Stream)
262 {
263  NavState state;
264 
265  std::ostringstream os;
266  os << state;
267 
268  string expected;
269  expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0 0 0\nv: 0 0 0";
270 
271  EXPECT(os.str() == expected);
272 }
273 
274 
275 /* ************************************************************************* */
276 int main() {
277  TestResult tr;
278  return TestRegistry::runAllTests(tr);
279 }
280 /* ************************************************************************* */
TestableAssertions.h
Provides additional testing facilities for common data structures.
kZeroXi
static const Vector9 kZeroXi
Definition: testNavState.cpp:37
kPosition
static const Point3 kPosition(1.0, 2.0, 3.0)
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
make_changelog.state
state
Definition: make_changelog.py:28
construct
void construct(...)
Definition: init.h:104
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::NavState::bodyVelocity
Velocity3 bodyVelocity(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:69
kPose
static const Pose3 kPose(kAttitude, kPosition)
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef&#39;d to Vector3.
Definition: NavState.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
M_PI
#define M_PI
Definition: main.h:106
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
kVelocity
static const Velocity3 kVelocity(0.4, 0.5, 0.6)
std
Definition: BFloat16.h:88
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::NavState::correctPIM
Vector9 correctPIM(const Vector9 &pim, double dt, const Vector3 &n_gravity, const std::optional< Vector3 > &omegaCoriolis, bool use2ndOrderCoriolis=false, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
Definition: NavState.cpp:257
dt
static const double dt
Definition: testNavState.cpp:173
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:166
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
TEST
TEST(NavState, Constructor)
Definition: testNavState.cpp:40
initial::nRb
const Rot3 nRb
Definition: testScenarioRunner.cpp:149
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
Definition: Rot3.cpp:135
kState1
static const NavState kState1(kAttitude, kPosition, kVelocity)
common::state2
static const NavState state2(x2, v2)
gtsam::numericalDerivative32
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:259
kIdentity
static const NavState kIdentity
Definition: testNavState.cpp:33
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::numericalDerivative31
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:226
gtsam::NavState::attitude
const Rot3 & attitude(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:48
drot
int BLASFUNC() drot(int *, double *, int *, double *, int *, double *, double *)
kGravity
static const Vector3 kGravity(0, 0, 9.81)
gtsam::internal::velocity
Velocity3 velocity(const NavState &X, OptionalJacobian< 3, 9 > H)
Definition: navigation/expressions.h:28
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
main
int main()
Definition: testNavState.cpp:276
gtsam::NavState::velocity
const Velocity3 & velocity(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:62
gtsam
traits
Definition: chartTesting.h:28
gtsam::NavState::retract
NavState retract(const Vector9 &v, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
retract with optional derivatives
Definition: NavState.cpp:107
kAttitude
static const Rot3 kAttitude
Definition: testNavState.cpp:29
gtsam::internal::attitude
Rot3 attitude(const NavState &X, OptionalJacobian< 3, 9 > H)
Definition: navigation/expressions.h:22
gtsam::NavState::position
const Point3 & position(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:55
kOmegaCoriolis
static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04)
create
ADT create(const Signature &signature)
Definition: testAlgebraicDecisionTree.cpp:130
coriolis
std::function< Vector9(const NavState &, const bool &)> coriolis
Definition: testNavState.cpp:174
bodyVelocity
Vector3 bodyVelocity(const Pose3 &w_t_b, const Vector3 &vec_w, OptionalJacobian< 3, 6 > Hpose={}, OptionalJacobian< 3, 3 > Hvel={})
Definition: testFactorTesting.cpp:29
os
ofstream os("timeSchurFactors.csv")
gtsam::NavState::coriolis
Vector9 coriolis(double dt, const Vector3 &omega, bool secondOrder=false, OptionalJacobian< 9, 9 > H={}) const
Compute tangent space contribution due to Coriolis forces.
Definition: NavState.cpp:213
gtsam::NavState::localCoordinates
Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
localCoordinates with optional derivatives
Definition: NavState.cpp:133
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::internal::position
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
Definition: navigation/expressions.h:25

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:46