test_orientation_tools.cpp File Reference

Test orientation functions. More...

#include "Math/orientation_tools.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"

Include dependency graph for test_orientation_tools.cpp:

Go to the source code of this file.

Functions
	TEST (Orientation, rad2deg)
	TEST (Orientation, almostEqual)
	TEST (Orientation, coordinateRotation)
	TEST (Orientation, skew)
	TEST (Orientation, quatToRotm)
	TEST (Orientation, quatToRpy)
	TEST (Orientation, quatToRpy2)
	TEST (Orientation, quatToRpy3)
	TEST (Orienation, quaternionDerivative)
	TEST (Orientation, quaternionProduct)
	TEST (Orientation, quaternionProductDirection)
	TEST (Orientation, quaternionIntegration)
	TEST (Orientation, rpyToRotMat)
	TEST (Orientation, allOrientationConversions)

Detailed Description

Test orientation functions.

Test the orientation related functions in orientation_tools.h Does not check any spatial stuff

Definition in file test_orientation_tools.cpp.

Function Documentation

TEST	(	Orientation	,
		rad2deg
	)

Check radians to degrees

Definition at line 17 of file test_orientation_tools.cpp.

References ori::deg2rad(), and ori::rad2deg().

                           {
  EXPECT_EQ(M_PI / 4., deg2rad(45.));
  EXPECT_EQ(45, rad2deg(M_PI / 4.));
}

Here is the call graph for this function:

TEST	(	Orientation	,
		almostEqual
	)

Check the "almostEqual" function for comparing two eigen quantities

Definition at line 25 of file test_orientation_tools.cpp.

References almostEqual().

                               {
  // check matrix "almostEqual" function
  Mat3<double> a, b;
  a << 1, 2, 3, 4, 5, 6, 7, 8, 9.2;
  b << 1, 2, 3, 4, 5, 6, 7, 8, 9.1;
  EXPECT_EQ(true, almostEqual(a, b, .3));
  EXPECT_EQ(false, almostEqual(a, b, .01));

  DVec<double> qdd(4);
  qdd << 1, 2, 3, 4;

  DVec<double> qdd2 = qdd;

  EXPECT_TRUE(almostEqual(qdd, qdd2, 1e-6));
  qdd2(1) += .2;
  EXPECT_FALSE(almostEqual(qdd, qdd2, 1e-6));

  DMat<double> testDynamicMat(3, 3);
  DMat<double> testDynamicMat2(3, 3);
  testDynamicMat << 1, 2, 3, 4, 5, 6, 7, 8, 9.2;
  testDynamicMat2 << 1, 2, 3, 4, 5, 6, 7, 8, 9.1;
  EXPECT_EQ(true, almostEqual(testDynamicMat, testDynamicMat2, .3));
  EXPECT_EQ(false, almostEqual(testDynamicMat, testDynamicMat2, .01));
}

Here is the call graph for this function:

TEST	(	Orientation	,
		coordinateRotation
	)

Check the coordinateRotation algorithm which generates rotaiton matrices for axis rotations

Definition at line 54 of file test_orientation_tools.cpp.

References ori::coordinateRotation(), ori::X, ori::Y, and ori::Z.

                                      {
  // check rotation matrices
  double s = std::sin(.2);
  double c = std::cos(.2);
  Mat3<double> R_ref_x, R_ref_y, R_ref_z;
  R_ref_x << 1, 0, 0, 0, c, s, 0, -s, c;
  R_ref_y << c, 0, -s, 0, 1, 0, s, 0, c;
  R_ref_z << c, s, 0, -s, c, 0, 0, 0, 1;
  EXPECT_EQ(R_ref_x, coordinateRotation(CoordinateAxis::X, .2));
  EXPECT_EQ(R_ref_y, coordinateRotation(CoordinateAxis::Y, .2));
  EXPECT_EQ(R_ref_z, coordinateRotation(CoordinateAxis::Z, .2));
}

Here is the call graph for this function:

TEST	(	Orientation	,
		skew
	)

Check the skew functions, which go between vectors and skew-symmetric matrices

Definition at line 71 of file test_orientation_tools.cpp.

References ori::matToSkewVec(), and ori::vectorToSkewMat().

                        {
  // check skew vec->mat and mat->vec
  Mat3<double> A, B;
  A << 8, 1, 6, 3, 5, 7, 4, 9, 2;
  B << 0, -3, 2, 3, 0, -1, -2, 1, 0;
  Vec3<double> v(1, 1, 1);
  Vec3<double> w(1, 2, 3);

  EXPECT_EQ(matToSkewVec(A), v);
  EXPECT_EQ(B, vectorToSkewMat(w));
  EXPECT_EQ(w, matToSkewVec(vectorToSkewMat(w)));
  EXPECT_EQ(v, matToSkewVec(vectorToSkewMat(v)));
}

Here is the call graph for this function:

TEST	(	Orientation	,
		quatToRotm
	)

Check the quaternion to rotation matrix and rotation matrix to quaternion functions

Definition at line 89 of file test_orientation_tools.cpp.

References almostEqual(), ori::quaternionToRotationMatrix(), and ori::rotationMatrixToQuaternion().

                              {
  // check R -> q and q -> R
  Quat<double> q(.9672, -0.0672, -0.1653, -0.1808);
  Mat3<double> R;
  R << .8799, .3720, -.2955, -.3276, .9256, .1898, .3441, -.0702, .9363;
  Quat<double> q2 = rotationMatrixToQuaternion(R.transpose());
  Mat3<double> R2 = quaternionToRotationMatrix(q).transpose();
  EXPECT_TRUE(almostEqual(q2, q, .0001));
  EXPECT_TRUE(almostEqual(R2, R, .0001));
}

Here is the call graph for this function:

TEST	(	Orientation	,
		quatToRpy
	)

Test the quaternion to roll-pitch-yaw conversion

Definition at line 103 of file test_orientation_tools.cpp.

References almostEqual(), and ori::quatToRPY().

                             {
  // check q -> rpy
  Quat<double> q(0.9672, -0.0672, -0.1653, -0.1808);
  Vec3<double> rpy1(-0.0748, -0.3513, -0.3564);
  EXPECT_TRUE(almostEqual(rpy1, quatToRPY(q), .1));
}

Here is the call graph for this function:

TEST	(	Orientation	,
		quatToRpy2
	)

Test the quaternion to roll-pitch-yaw conversion direction

Definition at line 113 of file test_orientation_tools.cpp.

References almostEqual(), ori::coordinateRotation(), ori::quatToRPY(), ori::rotationMatrixToQuaternion(), and ori::Y.

                              {
  // check q -> rpy
  RotMat<double> R = coordinateRotation(CoordinateAxis::Y, .5);
  Quat<double> q = rotationMatrixToQuaternion(R);
  Vec3<double> rpy = quatToRPY(q);
  Vec3<double> rpy_ref(0, .5, 0);
  EXPECT_TRUE(almostEqual(rpy, rpy_ref, .00001));
}

Here is the call graph for this function:

TEST	(	Orientation	,
		quatToRpy3
	)

Check that rpy is in roll-pitch-yaw order

Definition at line 125 of file test_orientation_tools.cpp.

References almostEqual(), ori::coordinateRotation(), ori::quatToRPY(), ori::rotationMatrixToQuaternion(), and ori::Z.

                              {
  // check q -> rpy
  RotMat<double> R = coordinateRotation(CoordinateAxis::Z, .5);
  Quat<double> q = rotationMatrixToQuaternion(R);
  Vec3<double> rpy = quatToRPY(q);
  Vec3<double> rpy_ref(0, 0, 0.5);
  EXPECT_TRUE(almostEqual(rpy, rpy_ref, .00001));
}

Here is the call graph for this function:

TEST	(	Orienation	,
		quaternionDerivative
	)

Check the quaternion derivative function. This isn't used and probably should be removed

Definition at line 138 of file test_orientation_tools.cpp.

References almostEqual(), and ori::quatDerivative().

                                       {
  Quat<double> ref(-10.8376, -0.6752, -2.5128, -1.3504);
  Quat<double> q =
      quatDerivative(Quat<double>(1, 2, 3, 4), Vec3<double>(1, 2, 3));
  EXPECT_TRUE(almostEqual(q, ref, .0005));
}

Here is the call graph for this function:

TEST	(	Orientation	,
		quaternionProduct
	)

Check the quaternion product

Definition at line 148 of file test_orientation_tools.cpp.

References almostEqual(), and ori::quatProduct().

                                     {
  Quat<double> q1(1, 2, 3, 4);
  Quat<double> q2(5, 6, 7, 8);
  Quat<double> ref(-60, 12, 30, 24);
  EXPECT_TRUE(almostEqual(ref, quatProduct(q1, q2), .0001));
}

Here is the call graph for this function:

TEST	(	Orientation	,
		quaternionProductDirection
	)

Definition at line 155 of file test_orientation_tools.cpp.

References almostEqual(), ori::coordinateRotation(), ori::quaternionToRotationMatrix(), ori::quatProduct(), ori::rotationMatrixToQuaternion(), ori::X, ori::Y, and ori::Z.

                                              {
  RotMat<double> R1 = coordinateRotation(CoordinateAxis::X, 7.23) *
                      coordinateRotation(CoordinateAxis::Y, -2.343) *
                      coordinateRotation(CoordinateAxis::Z, 1.2324);

  RotMat<double> R2 = coordinateRotation(CoordinateAxis::Z, .3231) *
                      coordinateRotation(CoordinateAxis::X, -4.2332) *
                      coordinateRotation(CoordinateAxis::Y, -3.3213);

  RotMat<double> R12_ref = R2 * R1;

  Quat<double> Q1 = rotationMatrixToQuaternion(R1);
  Quat<double> Q2 = rotationMatrixToQuaternion(R2);
  Quat<double> Q12 = quatProduct(Q1, Q2);

  RotMat<double> R12 = quaternionToRotationMatrix(Q12);

  EXPECT_TRUE(almostEqual(R12, R12_ref, .0001));
}

Here is the call graph for this function:

TEST	(	Orientation	,
		quaternionIntegration
	)

Check that the quaternion integration goes in the correct direction

Definition at line 178 of file test_orientation_tools.cpp.

References almostEqual(), ori::coordinateRotation(), ori::integrateQuat(), ori::quaternionToRotationMatrix(), ori::rotationMatrixToQuaternion(), ori::X, ori::Y, and ori::Z.

                                         {
  RotMat<double> eye = RotMat<double>::Identity();

  Vec3<double> omegaX(1, 0, 0);
  Vec3<double> omegaY(0, -1, 0);
  Vec3<double> omegaZ(0, 0, 1);

  RotMat<double> rot_x_ref = coordinateRotation(CoordinateAxis::X, .1);
  RotMat<double> rot_y_ref = coordinateRotation(CoordinateAxis::Y, -.1);
  RotMat<double> rot_z_ref = coordinateRotation(CoordinateAxis::Z, .1);

  Quat<double> eyeQ = rotationMatrixToQuaternion(eye);

  Quat<double> rot_x_quat = integrateQuat(eyeQ, omegaX, .1);
  Quat<double> rot_y_quat = integrateQuat(eyeQ, omegaY, .1);
  Quat<double> rot_z_quat = integrateQuat(eyeQ, omegaZ, .1);

  EXPECT_TRUE(
      almostEqual(quaternionToRotationMatrix(rot_x_quat), rot_x_ref, .0001));
  EXPECT_TRUE(
      almostEqual(quaternionToRotationMatrix(rot_y_quat), rot_y_ref, .0001));
  EXPECT_TRUE(
      almostEqual(quaternionToRotationMatrix(rot_z_quat), rot_z_ref, .0001));
}

Here is the call graph for this function:

TEST	(	Orientation	,
		rpyToRotMat
	)

Check rpy to rotation matrix

Definition at line 206 of file test_orientation_tools.cpp.

References almostEqual(), ori::deg2rad(), ori::quatToRPY(), ori::rotationMatrixToQuaternion(), and ori::rpyToRotMat().

                               {
  Vec3<double> rpy(deg2rad(20.), deg2rad(34.), deg2rad(160.));
  Mat3<double> R = rpyToRotMat(rpy);
  Quat<double> q = rotationMatrixToQuaternion(R);
  Vec3<double> rpyAgain = quatToRPY(q);
  EXPECT_TRUE(almostEqual(rpyAgain, rpy, .001));
  std::cout << "1: " << rpy.transpose() << "\n2: " << rpyAgain.transpose()
            << "\n";
}

Here is the call graph for this function:

TEST	(	Orientation	,
		allOrientationConversions
	)

Check all 6 possible conversions between rotation matrix, quaternion, and rpy

Definition at line 219 of file test_orientation_tools.cpp.

References almostEqual(), ori::deg2rad(), ori::quaternionToRotationMatrix(), ori::quatToRPY(), ori::rotationMatrixToQuaternion(), ori::rotationMatrixToRPY(), ori::rpyToQuat(), and ori::rpyToRotMat().

                                             {
  Vec3<double> refRPY(deg2rad(20.), deg2rad(34.), deg2rad(160.));
  // do all rpy -> conversions
  Quat<double> quatFromRPY = rpyToQuat(refRPY);
  Mat3<double> rFromRPY = rpyToRotMat(refRPY);

  // do all quat -> conversions
  Mat3<double> rFromQuat = quaternionToRotationMatrix(quatFromRPY);
  Vec3<double> rpyFromQuat = quatToRPY(quatFromRPY);

  // do all r -> conversions
  Vec3<double> rpyFromR = rotationMatrixToRPY(rFromRPY);
  Quat<double> quatFromR = rotationMatrixToQuaternion(rFromQuat);

  EXPECT_TRUE(almostEqual(refRPY, rpyFromQuat, .0001));
  EXPECT_TRUE(almostEqual(refRPY, rpyFromR, .0001));

  EXPECT_TRUE(almostEqual(rFromQuat, rFromRPY, .0001));

  EXPECT_TRUE(almostEqual(quatFromR, quatFromRPY, .0001));
}