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