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, adjointMap)

	TEST (NavState, Attitude)

	TEST (NavState, Between)

	TEST (NavState, BodyVelocity)

	TEST (NavState, ChartDerivatives)

	TEST (NavState, Compose)

	TEST (NavState, Compose2)

	TEST (NavState, Compose_Inverse)

	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, 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 685 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/39]

TEST	(	NavState	,
		Adjoint_compose_full
	)

Definition at line 524 of file testNavState.cpp.

◆ TEST() [2/39]

TEST	(	NavState	,
		Adjoint_full
	)

Definition at line 509 of file testNavState.cpp.

◆ TEST() [3/39]

TEST	(	NavState	,
		adjointMap
	)

Definition at line 602 of file testNavState.cpp.

◆ TEST() [4/39]

TEST	(	NavState	,
		Attitude
	)

Definition at line 89 of file testNavState.cpp.

◆ TEST() [5/39]

TEST	(	NavState	,
		Between
	)

Definition at line 295 of file testNavState.cpp.

◆ TEST() [6/39]

TEST	(	NavState	,
		BodyVelocity
	)

Definition at line 121 of file testNavState.cpp.

◆ TEST() [7/39]

TEST	(	NavState	,
		ChartDerivatives
	)

Definition at line 674 of file testNavState.cpp.

◆ TEST() [8/39]

TEST	(	NavState	,
		Compose
	)

Definition at line 198 of file testNavState.cpp.

◆ TEST() [9/39]

TEST	(	NavState	,
		Compose2
	)

Definition at line 229 of file testNavState.cpp.

◆ TEST() [10/39]

TEST	(	NavState	,
		Compose_Inverse
	)

Definition at line 288 of file testNavState.cpp.

◆ TEST() [11/39]

TEST	(	NavState	,
		Constructor
	)

Definition at line 56 of file testNavState.cpp.

◆ TEST() [12/39]

TEST	(	NavState	,
		Constructor2
	)

Definition at line 76 of file testNavState.cpp.

◆ TEST() [13/39]

TEST	(	NavState	,
		Coriolis
	)

Definition at line 331 of file testNavState.cpp.

◆ TEST() [14/39]

TEST	(	NavState	,
		Coriolis2
	)

Definition at line 342 of file testNavState.cpp.

◆ TEST() [15/39]

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 355 of file testNavState.cpp.

◆ TEST() [16/39]

TEST	(	NavState	,
		CorrectPIM
	)

Definition at line 399 of file testNavState.cpp.

◆ TEST() [17/39]

TEST	(	NavState	,
		Equals
	)

Definition at line 189 of file testNavState.cpp.

◆ TEST() [18/39]

TEST	(	NavState	,
		Expmap_A_Full
	)

Definition at line 469 of file testNavState.cpp.

◆ TEST() [19/39]

TEST	(	NavState	,
		Expmap_b
	)

Definition at line 487 of file testNavState.cpp.

◆ TEST() [20/39]

TEST	(	NavState	,
		expmap_logmap
	)

Definition at line 554 of file testNavState.cpp.

◆ TEST() [21/39]

TEST	(	NavState	,
		ExpmapDerivative1
	)

Definition at line 616 of file testNavState.cpp.

◆ TEST() [22/39]

TEST	(	NavState	,
		interpolate
	)

Definition at line 320 of file testNavState.cpp.

◆ TEST() [23/39]

TEST	(	NavState	,
		Invariants
	)

Definition at line 649 of file testNavState.cpp.

◆ TEST() [24/39]

TEST	(	NavState	,
		Inverse
	)

Definition at line 249 of file testNavState.cpp.

◆ TEST() [25/39]

TEST	(	NavState	,
		InverseDerivatives
	)

Definition at line 274 of file testNavState.cpp.

◆ TEST() [26/39]

TEST	(	NavState	,
		LieGroupDerivatives
	)

Definition at line 664 of file testNavState.cpp.

◆ TEST() [27/39]

TEST	(	NavState	,
		LogmapDerivative
	)

Definition at line 632 of file testNavState.cpp.

◆ TEST() [28/39]

TEST	(	NavState	,
		Manifold
	)

Definition at line 132 of file testNavState.cpp.

◆ TEST() [29/39]

TEST	(	NavState	,
		manifold_expmap
	)

Definition at line 572 of file testNavState.cpp.

◆ TEST() [30/39]

TEST	(	NavState	,
		Position
	)

Definition at line 99 of file testNavState.cpp.

◆ TEST() [31/39]

TEST	(	NavState	,
		Print
	)

Definition at line 428 of file testNavState.cpp.

◆ TEST() [32/39]

TEST	(	NavState	,
		Retract_first_order
	)

Definition at line 442 of file testNavState.cpp.

◆ TEST() [33/39]

TEST	(	NavState	,
		Retract_LocalCoordinates
	)

Definition at line 537 of file testNavState.cpp.

◆ TEST() [34/39]

TEST	(	NavState	,
		retract_localCoordinates
	)

Definition at line 546 of file testNavState.cpp.

◆ TEST() [35/39]

TEST	(	NavState	,
		retract_localCoordinates2
	)

Definition at line 561 of file testNavState.cpp.

◆ TEST() [36/39]

TEST	(	NavState	,
		RetractExpmap
	)

Definition at line 459 of file testNavState.cpp.

◆ TEST() [37/39]

TEST	(	NavState	,
		Stream
	)

Definition at line 414 of file testNavState.cpp.

◆ TEST() [38/39]

TEST	(	NavState	,
		subgroups
	)

Definition at line 586 of file testNavState.cpp.

◆ TEST() [39/39]

TEST	(	NavState	,
		Velocity
	)

Definition at line 110 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 327 of file testNavState.cpp.

◆ dt

const double dt = 2.0

static

Definition at line 326 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

Namespaces

Functions

Variables

Detailed Description

Function Documentation

◆ kGravity()

◆ kOmegaCoriolis()

◆ kPosition()

◆ kVelocity()

◆ main()

◆ P()

◆ P2()

◆ TEST() [1/39]

◆ TEST() [2/39]

◆ TEST() [3/39]

◆ TEST() [4/39]

◆ TEST() [5/39]

◆ TEST() [6/39]

◆ TEST() [7/39]

◆ TEST() [8/39]

◆ TEST() [9/39]

◆ TEST() [10/39]

◆ TEST() [11/39]

◆ TEST() [12/39]

◆ TEST() [13/39]

◆ TEST() [14/39]

◆ TEST() [15/39]

◆ TEST() [16/39]

◆ TEST() [17/39]

◆ TEST() [18/39]

◆ TEST() [19/39]

◆ TEST() [20/39]

◆ TEST() [21/39]

◆ TEST() [22/39]

◆ TEST() [23/39]

◆ TEST() [24/39]

◆ TEST() [25/39]

◆ TEST() [26/39]

◆ TEST() [27/39]

◆ TEST() [28/39]

◆ TEST() [29/39]

◆ TEST() [30/39]

◆ TEST() [31/39]

◆ TEST() [32/39]

◆ TEST() [33/39]

◆ TEST() [34/39]

◆ TEST() [35/39]

◆ TEST() [36/39]

◆ TEST() [37/39]

◆ TEST() [38/39]

◆ TEST() [39/39]

◆ V()

◆ V2()

Variable Documentation

◆ coriolis

◆ dt

◆ kAttitude

◆ kIdentity

◆ kPose

◆ kState1

◆ kZeroXi

◆ R

◆ T

◆ T2

◆ T3