34 #include <gtest/gtest.h> 44 std::srand(std::time(0));
51 double roll, pitch, yaw;
53 for (
int solution = 1 ; solution < 2 ; ++solution)
55 m.
getRPY(roll, pitch, yaw, solution);
57 q_from_rpy.
setRPY(roll, pitch, yaw);
61 ASSERT_NEAR(0.0, angle1,
epsilon);
65 m2.
setRPY(roll, pitch, yaw);
71 ASSERT_NEAR(0.0, angle2,
epsilon);
78 unsigned int runs = 400;
81 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
82 for (
unsigned int i = 0; i < runs ; i++ )
84 xvalues[i] = 1.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
85 yvalues[i] = 1.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
86 zvalues[i] = 1.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
90 for (
unsigned int i = 0; i < runs ; i++ )
94 q_baseline.
setRPY(zvalues[i],yvalues[i],xvalues[i]);
99 ASSERT_NEAR(0.0, angle,
epsilon);
112 int main(
int argc,
char **argv){
113 testing::InitGoogleTest(&argc, argv);
114 return RUN_ALL_TESTS();
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
tfScalar angle(const Quaternion &q) const
Return the half angle between this quaternion and the other.
Quaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
int main(int argc, char **argv)
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Set the quaternion using fixed axis RPY.
void setRPY(tfScalar roll, tfScalar pitch, tfScalar yaw)
Set the matrix using RPY about XYZ fixed axes.
void testQuatRPY(tf::Quaternion q_baseline)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
Get the matrix represented as roll pitch and yaw about fixed axes XYZ.
tfScalar angleShortestPath(const Quaternion &q) const
Return the angle between this quaternion and the other along the shortest path.