testNavState.cpp File Reference

Unit tests for NavState. More...

#include <gtsam/navigation/NavState.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>

Include dependency graph for testNavState.cpp:

Go to the source code of this file.

Namespaces
	screwNavState

Functions
Point3	screwNavState::expectedP (0.29552, 0.0446635, 1)
Rot3	screwNavState::expectedR (c, -s, 0, s, c, 0, 0, 0, 1)
Point3	screwNavState::expectedV (0.29552, 0.0446635, 1)
static const Vector3	kGravity (0, 0, 9.81)
static const Vector3	kOmegaCoriolis (0.02, 0.03, 0.04)
static const Point3	kPosition (1.0, 2.0, 3.0)
static const Velocity3	kVelocity (0.4, 0.5, 0.6)
int	main ()
static const Point3	P (0.2, 0.7, -2)
static const Point3	P2 (3.5, -8.2, 4.2)
	TEST (NavState, Adjoint_compose_full)
	TEST (NavState, Adjoint_full)
	TEST (NavState, Adjoint_hat)
	TEST (NavState, adjointMap)
	TEST (NavState, Attitude)
	TEST (NavState, Between)
	TEST (NavState, BodyVelocity)
	TEST (NavState, BruteForceExpmap1)
	TEST (NavState, BruteForceExpmap2)
	TEST (NavState, BruteForceExpmap3)
	TEST (NavState, ChartDerivatives)
	TEST (NavState, Compose)
	TEST (NavState, Compose2)
	TEST (NavState, Compose_Inverse)
	TEST (NavState, Concept)
	TEST (NavState, Constructor)
	TEST (NavState, Constructor2)
	TEST (NavState, Coriolis)
	TEST (NavState, Coriolis2)
	TEST (NavState, Coriolis3)
	TEST (NavState, CorrectPIM)
	TEST (NavState, Equals)
	TEST (NavState, Expmap_A_Full)
	TEST (NavState, Expmap_b)
	TEST (NavState, expmap_logmap)
	TEST (NavState, ExpmapDerivative1)
	TEST (NavState, HatAndVee)
	TEST (NavState, interpolate)
	TEST (NavState, Invariants)
	TEST (NavState, Inverse)
	TEST (NavState, InverseDerivatives)
	TEST (NavState, LieGroupDerivatives)
	TEST (NavState, LogmapDerivative)
	TEST (NavState, Manifold)
	TEST (NavState, manifold_expmap)
	TEST (NavState, Position)
	TEST (NavState, Print)
	TEST (NavState, Retract_first_order)
	TEST (NavState, Retract_LocalCoordinates)
	TEST (NavState, retract_localCoordinates)
	TEST (NavState, retract_localCoordinates2)
	TEST (NavState, RetractExpmap)
	TEST (NavState, Stream)
	TEST (NavState, subgroups)
	TEST (NavState, Velocity)
static const Point3	V (3, 0.4, -2.2)
static const Point3	V2 (-6.5, 3.5, 6.2)

Variables
double	screwNavState::a = 0.3
double	screwNavState::c = cos(a)
std::function< Vector9(const NavState &, const bool &)>	coriolis
static const double	dt = 2.0
NavState	screwNavState::expected (expectedR, expectedV, expectedP)
static const Rot3	kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3)
static const NavState	kIdentity
static const Pose3	kPose (kAttitude, kPosition)
static const NavState	kState1 (kAttitude, kPosition, kVelocity)
static const Vector9	kZeroXi = Vector9::Zero()
static const Rot3	R = Rot3::Rodrigues(0.3, 0, 0)
double	screwNavState::s = sin(a)
static const NavState	T (R, P2, V2)
static const NavState	T2 (Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2)
static const NavState	T3 (Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3))
double	screwNavState::w = 0.3
Vector	screwNavState::xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished()

Detailed Description

Unit tests for NavState.

Authors

Frank Dellaert, Varun Agrawal, Fan Jiang

Date

July 2015

Definition in file testNavState.cpp.

Function Documentation

kGravity()

static const Vector3 kGravity ( 0  , 0  , 9.  81  )

static

kOmegaCoriolis()

static const Vector3 kOmegaCoriolis ( 0.  02, 0.  03, 0.  04  )

static

kPosition()

static const Point3 kPosition ( 1.  0, 2.  0, 3.  0  )

static

kVelocity()

static const Velocity3 kVelocity ( 0.  4, 0.  5, 0.  6  )

static

main()

int main

(

)

Definition at line 749 of file testNavState.cpp.

P()

static const Point3 P ( 0.  2, 0.  7, -  2  )

static

P2()

static const Point3 P2 ( 3.  5, -8.  2, 4.  2  )

static

TEST() [1/45]

TEST	(	NavState	,
		Adjoint_compose_full
	)

Definition at line 531 of file testNavState.cpp.

TEST() [2/45]

TEST	(	NavState	,
		Adjoint_full
	)

Definition at line 516 of file testNavState.cpp.

TEST() [3/45]

TEST	(	NavState	,
		Adjoint_hat
	)

Definition at line 585 of file testNavState.cpp.

TEST() [4/45]

TEST	(	NavState	,
		adjointMap
	)

Definition at line 666 of file testNavState.cpp.

TEST() [5/45]

TEST	(	NavState	,
		Attitude
	)

Definition at line 96 of file testNavState.cpp.

TEST() [6/45]

TEST	(	NavState	,
		Between
	)

Definition at line 302 of file testNavState.cpp.

TEST() [7/45]

TEST	(	NavState	,
		BodyVelocity
	)

Definition at line 128 of file testNavState.cpp.

TEST() [8/45]

TEST	(	NavState	,
		BruteForceExpmap1
	)

Definition at line 568 of file testNavState.cpp.

TEST() [9/45]

TEST	(	NavState	,
		BruteForceExpmap2
	)

Definition at line 573 of file testNavState.cpp.

TEST() [10/45]

TEST	(	NavState	,
		BruteForceExpmap3
	)

Definition at line 578 of file testNavState.cpp.

TEST() [11/45]

TEST	(	NavState	,
		ChartDerivatives
	)

Definition at line 738 of file testNavState.cpp.

TEST() [12/45]

TEST	(	NavState	,
		Compose
	)

Definition at line 205 of file testNavState.cpp.

TEST() [13/45]

TEST	(	NavState	,
		Compose2
	)

Definition at line 236 of file testNavState.cpp.

TEST() [14/45]

TEST	(	NavState	,
		Compose_Inverse
	)

Definition at line 295 of file testNavState.cpp.

TEST() [15/45]

TEST	(	NavState	,
		Concept
	)

Definition at line 56 of file testNavState.cpp.

TEST() [16/45]

TEST	(	NavState	,
		Constructor
	)

Definition at line 63 of file testNavState.cpp.

TEST() [17/45]

TEST	(	NavState	,
		Constructor2
	)

Definition at line 83 of file testNavState.cpp.

TEST() [18/45]

TEST	(	NavState	,
		Coriolis
	)

Definition at line 338 of file testNavState.cpp.

TEST() [19/45]

TEST	(	NavState	,
		Coriolis2
	)

Definition at line 349 of file testNavState.cpp.

TEST() [20/45]

TEST	(	NavState	,
		Coriolis3
	)

Consider a massless planet with an attached nav frame at n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously aligned with the nav frame (i.e., nRb != I_3x3). Test that first and second order Coriolis corrections are as expected.

Definition at line 362 of file testNavState.cpp.

TEST() [21/45]

TEST	(	NavState	,
		CorrectPIM
	)

Definition at line 406 of file testNavState.cpp.

TEST() [22/45]

TEST	(	NavState	,
		Equals
	)

Definition at line 196 of file testNavState.cpp.

TEST() [23/45]

TEST	(	NavState	,
		Expmap_A_Full
	)

Definition at line 476 of file testNavState.cpp.

TEST() [24/45]

TEST	(	NavState	,
		Expmap_b
	)

Definition at line 494 of file testNavState.cpp.

TEST() [25/45]

TEST	(	NavState	,
		expmap_logmap
	)

Definition at line 618 of file testNavState.cpp.

TEST() [26/45]

TEST	(	NavState	,
		ExpmapDerivative1
	)

Definition at line 680 of file testNavState.cpp.

TEST() [27/45]

TEST	(	NavState	,
		HatAndVee
	)

Definition at line 544 of file testNavState.cpp.

TEST() [28/45]

TEST	(	NavState	,
		interpolate
	)

Definition at line 327 of file testNavState.cpp.

TEST() [29/45]

TEST	(	NavState	,
		Invariants
	)

Definition at line 713 of file testNavState.cpp.

TEST() [30/45]

TEST	(	NavState	,
		Inverse
	)

Definition at line 256 of file testNavState.cpp.

TEST() [31/45]

TEST	(	NavState	,
		InverseDerivatives
	)

Definition at line 281 of file testNavState.cpp.

TEST() [32/45]

TEST	(	NavState	,
		LieGroupDerivatives
	)

Definition at line 728 of file testNavState.cpp.

TEST() [33/45]

TEST	(	NavState	,
		LogmapDerivative
	)

Definition at line 696 of file testNavState.cpp.

TEST() [34/45]

TEST	(	NavState	,
		Manifold
	)

Definition at line 139 of file testNavState.cpp.

TEST() [35/45]

TEST	(	NavState	,
		manifold_expmap
	)

Definition at line 636 of file testNavState.cpp.

TEST() [36/45]

TEST	(	NavState	,
		Position
	)

Definition at line 106 of file testNavState.cpp.

TEST() [37/45]

TEST	(	NavState	,
		Print
	)

Definition at line 435 of file testNavState.cpp.

TEST() [38/45]

TEST	(	NavState	,
		Retract_first_order
	)

Definition at line 449 of file testNavState.cpp.

TEST() [39/45]

TEST	(	NavState	,
		Retract_LocalCoordinates
	)

Definition at line 601 of file testNavState.cpp.

TEST() [40/45]

TEST	(	NavState	,
		retract_localCoordinates
	)

Definition at line 610 of file testNavState.cpp.

TEST() [41/45]

TEST	(	NavState	,
		retract_localCoordinates2
	)

Definition at line 625 of file testNavState.cpp.

TEST() [42/45]

TEST	(	NavState	,
		RetractExpmap
	)

Definition at line 466 of file testNavState.cpp.

TEST() [43/45]

TEST	(	NavState	,
		Stream
	)

Definition at line 421 of file testNavState.cpp.

TEST() [44/45]

TEST	(	NavState	,
		subgroups
	)

Definition at line 650 of file testNavState.cpp.

TEST() [45/45]

TEST	(	NavState	,
		Velocity
	)

Definition at line 117 of file testNavState.cpp.

V()

static const Point3 V ( 3  , 0.  4, -2.  2  )

static

V2()

static const Point3 V2 ( -6.  5, 3.  5, 6.  2  )

static

Variable Documentation

coriolis

std::function<Vector9(const NavState&, const bool&)> coriolis

Initial value:

=
    std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis,
              std::placeholders::_2, nullptr)

Definition at line 334 of file testNavState.cpp.

dt

const double dt = 2.0

static

Definition at line 333 of file testNavState.cpp.

kAttitude

const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3)

static

Definition at line 35 of file testNavState.cpp.

kIdentity

const NavState kIdentity

static

Definition at line 39 of file testNavState.cpp.

kPose

const Pose3 kPose(kAttitude, kPosition)

static

kState1

const NavState kState1(kAttitude, kPosition, kVelocity)

static

kZeroXi

const Vector9 kZeroXi = Vector9::Zero()

static

Definition at line 43 of file testNavState.cpp.

R

const Rot3 R = Rot3::Rodrigues(0.3, 0, 0)

static

Definition at line 47 of file testNavState.cpp.

T

const NavState T(R, P2, V2)

static

T2

const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2)

static

T3

const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3))

static