validate_quaternions.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, Stefan Fabian
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 #ifndef RVIZ_VALIDATE_QUATERNIONS_H
31 #define RVIZ_VALIDATE_QUATERNIONS_H
32 
33 #include <geometry_msgs/PoseStamped.h>
34 #include <OgreQuaternion.h>
35 #include <ros/ros.h>
37 
38 #include <boost/array.hpp>
39 
40 namespace rviz
41 {
42 
43 // Tolerance for acceptable quaternion normalization (same as in tf)
44 static double QUATERNION_NORMALIZATION_TOLERANCE = 10e-3;
45 
46 inline float quaternionNorm2( float w, float x, float y, float z )
47 {
48  return w * w + x * x + y * y + z * z;
49 }
50 
51 inline bool validateQuaternions( float w, float x, float y, float z )
52 {
53  if ( 0.0f == x && 0.0f == y && 0.0f == z && 0.0f == w )
54  {
55  // Allow null quaternions to pass because they are common in uninitialized ROS messages.
56  return true;
57  }
58  float norm2 = quaternionNorm2( w, x, y, z );
59  bool is_normalized = std::abs( norm2 - 1.0f ) < QUATERNION_NORMALIZATION_TOLERANCE;
60  ROS_DEBUG_COND_NAMED( !is_normalized, "quaternions", "Quaternion [x: %.3f, y: %.3f, z: %.3f, w: %.3f] not normalized. "
61  "Magnitude: %.3f", x, y, z, w, std::sqrt(norm2) );
62  return is_normalized;
63 }
64 
65 inline float normalizeQuaternion( float& w, float& x, float& y, float& z )
66 {
67  if ( 0.0f == x && 0.0f == y && 0.0f == z && 0.0f == w )
68  {
69  w = 1.0f;
70  return 0.0f;
71  }
72  float norm2 = quaternionNorm2( w, x, y, z );
73  norm2 = std::sqrt( norm2 );
74  float invnorm = 1.0f / norm2;
75  w *= invnorm;
76  x *= invnorm;
77  y *= invnorm;
78  z *= invnorm;
79  return norm2;
80 }
81 
82 
83 inline double quaternionNorm2( double w, double x, double y, double z )
84 {
85  return w * w + x * x + y * y + z * z;
86 }
87 
88 inline bool validateQuaternions( double w, double x, double y, double z )
89 {
90  if ( 0.0 == x && 0.0 == y && 0.0 == z && 0.0 == w )
91  {
92  // Allow null quaternions to pass because they are common in uninitialized ROS messages.
93  return true;
94  }
95  double norm2 = quaternionNorm2( w, x, y, z );
96  bool is_normalized = std::abs( norm2 - 1.0 ) < QUATERNION_NORMALIZATION_TOLERANCE;
97  ROS_DEBUG_COND_NAMED( !is_normalized, "quaternions", "Quaternion [x: %.3f, y: %.3f, z: %.3f, w: %.3f] not normalized. "
98  "Magnitude: %.3f", x, y, z, w, std::sqrt(norm2) );
99  return is_normalized;
100 }
101 
102 inline double normalizeQuaternion( double& w, double& x, double& y, double& z )
103 {
104  if ( 0.0 == x && 0.0 == y && 0.0 == z && 0.0 == w )
105  {
106  w = 1.0;
107  return 0.0;
108  }
109  double norm2 = quaternionNorm2( w, x, y, z );
110  norm2 = std::sqrt( norm2 );
111  double invnorm = 1.0 / norm2;
112  w *= invnorm;
113  x *= invnorm;
114  y *= invnorm;
115  z *= invnorm;
116  return norm2;
117 }
118 
119 
120 inline bool validateQuaternions( const Ogre::Quaternion& quaternion )
121 {
122  return validateQuaternions( quaternion.w, quaternion.x, quaternion.y, quaternion.z );
123 }
124 
125 inline bool validateQuaternions( const tf::Quaternion& quaternion )
126 {
127  return validateQuaternions( quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z());
128 }
129 
130 inline bool validateQuaternions( const geometry_msgs::Quaternion &msg )
131 {
132  return validateQuaternions( msg.w, msg.x, msg.y, msg.z );
133 }
134 
135 inline bool validateQuaternions( const geometry_msgs::Pose &msg )
136 {
137  return validateQuaternions( msg.orientation );
138 }
139 
140 inline bool validateQuaternions( const geometry_msgs::PoseStamped &msg )
141 {
142  return validateQuaternions( msg.pose );
143 }
144 
145 inline double normalizeQuaternion( Ogre::Quaternion& quaternion )
146 {
147  return normalizeQuaternion( quaternion.w, quaternion.x, quaternion.y, quaternion.z );
148 }
149 
150 inline double normalizeQuaternion( const geometry_msgs::Quaternion &msg, Ogre::Quaternion &q )
151 {
152  q.w = msg.w;
153  q.x = msg.x;
154  q.y = msg.y;
155  q.z = msg.z;
156  return normalizeQuaternion( q );
157 }
158 
159 template<typename T>
160 inline bool validateQuaternions( const std::vector<T> &vec )
161 {
162  typedef std::vector<T> VecType;
163  typename VecType::const_iterator it = vec.begin();
164  typename VecType::const_iterator end = vec.end();
165  for ( ; it != end; ++it )
166  {
167  if ( !validateQuaternions( *it ))
168  {
169  return false;
170  }
171  }
172 
173  return true;
174 }
175 
176 template<typename T, size_t N>
177 inline bool validateQuaternions( const boost::array<T, N> &arr )
178 {
179  typedef boost::array<T, N> ArrType;
180  typename ArrType::const_iterator it = arr.begin();
181  typename ArrType::const_iterator end = arr.end();
182  for ( ; it != end; ++it )
183  {
184  if ( !validateQuaternions( *it ))
185  {
186  return false;
187  }
188  }
189 
190  return true;
191 }
192 
193 } // namespace rviz
194 
195 #endif // RVIZ_VALIDATE_QUATERNIONS_H
f
#define ROS_DEBUG_COND_NAMED(cond, name,...)
float quaternionNorm2(float w, float x, float y, float z)
float normalizeQuaternion(float &w, float &x, float &y, float &z)
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
static double QUATERNION_NORMALIZATION_TOLERANCE


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51