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

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:48:08