relax normalization tolerance. #196 was too strict for some use cases.

ros · Apr 1, 2017 · 634fd0c · 634fd0c
1 parent 120a532
commit 634fd0c
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp
@@ -41,6 +41,9 @@
 namespace tf2
 {
 
+// Tolerance for acceptable quaternion normalization
+static double QUATERNION_NORMALIZATION_TOLERANCE = 10e-3;
+
 /** \brief convert Transform msg to Transform */
 void transformMsgToTF2(const geometry_msgs::Transform& msg, tf2::Transform& tf2)
 {tf2 = tf2::Transform(tf2::Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), tf2::Vector3(msg.translation.x, msg.translation.y, msg.translation.z));}
@@ -244,7 +247,7 @@ bool BufferCore::setTransform(const geometry_msgs::TransformStamped& transform_i
   bool valid = std::abs((stripped.transform.rotation.w * stripped.transform.rotation.w
                         + stripped.transform.rotation.x * stripped.transform.rotation.x
                         + stripped.transform.rotation.y * stripped.transform.rotation.y
-                        + stripped.transform.rotation.z * stripped.transform.rotation.z) - 1.0f) < 10e-6;
+                        + stripped.transform.rotation.z * stripped.transform.rotation.z) - 1.0f) < QUATERNION_NORMALIZATION_TOLERANCE;
 
   if (!valid) 
   {