quaternion.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <vector>
31 #include <sys/time.h>
32 #include <cstdio>
33 #include <gtest/gtest.h>
34 
36 
37 double epsilon = 10E-6;
38 
39 
40 void seed_rand()
41 {
42  //Seed random number generator with current microseond count
43  timeval temp_time_struct;
44  gettimeofday(&temp_time_struct,NULL);
45  srand(temp_time_struct.tv_usec);
46 }
47 
48 void testQuatRPY(tf::Quaternion q_baseline)
49 {
50  q_baseline.normalize();
51  tf::Matrix3x3 m(q_baseline);
52  double roll, pitch, yaw;
53 
54  for (int solution = 1 ; solution < 2 ; ++solution)
55  {
56  m.getRPY(roll, pitch, yaw, solution);
57  tf::Quaternion q_from_rpy;
58  q_from_rpy.setRPY(roll, pitch, yaw);
59  // use angleShortestPath() because angle() can return PI for equivalent
60  // quaternions
61  double angle1 = q_from_rpy.angleShortestPath(q_baseline);
62  ASSERT_NEAR(0.0, angle1, epsilon);
63  //std::printf("%f, angle between quaternions\n", angle1);
64 
65  tf::Matrix3x3 m2;
66  m2.setRPY(roll, pitch, yaw);
67  tf::Quaternion q_from_m_from_rpy;
68  m2.getRotation(q_from_m_from_rpy);
69  // use angleShortestPath() because angle() can return PI for equivalent
70  // quaternions
71  double angle2 = q_from_m_from_rpy.angleShortestPath(q_baseline);
72  ASSERT_NEAR(0.0, angle2, epsilon);
73  //std::printf("%f, angle between quaternions\n", angle2);
74  }
75 }
76 
77 TEST(tf, Quaternion)
78 {
79  unsigned int runs = 400;
80  seed_rand();
81 
82  std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
83  for ( unsigned int i = 0; i < runs ; i++ )
84  {
85  xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
86  yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
87  zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
88  }
89 
90 
91  for ( unsigned int i = 0; i < runs ; i++ )
92  {
93  tf::Matrix3x3 mat;
94  tf::Quaternion q_baseline;
95  q_baseline.setRPY(zvalues[i],yvalues[i],xvalues[i]);
96  mat.setRotation(q_baseline);
97  tf::Quaternion q_from_m;
98  mat.getRotation(q_from_m);
99  double angle = q_from_m.angle(q_baseline);
100  ASSERT_NEAR(0.0, angle, epsilon);
101  testQuatRPY(q_baseline);
102  }
103 
104  // test some corner cases
105  testQuatRPY(tf::Quaternion( 0.5, 0.5, 0.5, -0.5));
106  testQuatRPY(tf::Quaternion( 0.5, 0.5, 0.5, 0.5));
107  testQuatRPY(tf::Quaternion( 0.5, -0.5, 0.5, 0.5));
108  testQuatRPY(tf::Quaternion( 0.5, 0.5, -0.5, 0.5));
109  testQuatRPY(tf::Quaternion(-0.5, 0.5, 0.5, 0.5));
110 }
111 
112 
113 int main(int argc, char **argv){
114  testing::InitGoogleTest(&argc, argv);
115  return RUN_ALL_TESTS();
116 }
tf::Matrix3x3
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
Definition: Matrix3x3.h:33
tf::Matrix3x3::setRPY
void setRPY(tfScalar roll, tfScalar pitch, tfScalar yaw)
Set the matrix using RPY about XYZ fixed axes.
Definition: Matrix3x3.h:205
tf::Quaternion::normalize
Quaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: Quaternion.h:176
main
int main(int argc, char **argv)
Definition: quaternion.cpp:113
tf::Matrix3x3::getRPY
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.
Definition: Matrix3x3.h:367
tf::Quaternion::setRPY
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Set the quaternion using fixed axis RPY.
Definition: Quaternion.h:96
Transform.h
tf::Matrix3x3::getRotation
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
Definition: Matrix3x3.h:245
tf::Quaternion::angleShortestPath
tfScalar angleShortestPath(const Quaternion &q) const
Return the angle between this quaternion and the other along the shortest path.
Definition: Quaternion.h:221
testQuatRPY
void testQuatRPY(tf::Quaternion q_baseline)
Definition: quaternion.cpp:48
epsilon
double epsilon
Definition: quaternion.cpp:37
seed_rand
void seed_rand()
Definition: quaternion.cpp:40
angle
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
tf::Quaternion::angle
tfScalar angle(const Quaternion &q) const
Return the half angle between this quaternion and the other.
Definition: Quaternion.h:213
tf
Definition: exceptions.h:38
TEST
TEST(tf, Quaternion)
Definition: quaternion.cpp:77
tf::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
tf::Matrix3x3::setRotation
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
Definition: Matrix3x3.h:148


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sat Aug 19 2023 02:38:07