normalize invalid quaternions instead of rejecting them

reverts most changes introduced in b329145

Normalization of quaternions usually is done in context_->getFrameManager()->transform()
which transforms a ROS pose into an Ogre position + orientation.
Only in very rare cases, a ROS quaternion was directly used as an
Ogre::Quaternion, which then indeed requires normalization.

src/rviz/default_plugin/covariance_visual.cpp

@@ -30,6 +30,7 @@
 #include "covariance_visual.h"
 
 #include "rviz/ogre_helpers/shape.h"
+#include "rviz/validate_quaternions.h"
 
 #include <OgreSceneManager.h>
 #include <OgreSceneNode.h>
@@ -299,7 +300,10 @@ void CovarianceVisual::setCovariance( const geometry_msgs::PoseWithCovariance& p
   updateOrientationVisibility();
 
   // store orientation in Ogre structure
-  Ogre::Quaternion ori(pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z);
+  Ogre::Quaternion ori;
+  if (!normalizeQuaternion(pose.pose.orientation, ori))
+    ROS_WARN("invalid quaternion (zero length)");
+
   // Set the orientation of the fixed node. Since this node is attached to the root node, it's orientation will be the
   // inverse of pose's orientation.
   fixed_orientation_node_->setOrientation(ori.Inverse());

src/rviz/default_plugin/interactive_marker_display.cpp

@@ -34,7 +34,6 @@
 #include "rviz/properties/ros_topic_property.h"
 #include "rviz/selection/selection_manager.h"
 #include "rviz/validate_floats.h"
-#include "rviz/validate_quaternions.h"
 #include "rviz/display_context.h"
 
 #include "rviz/default_plugin/interactive_marker_display.h"
@@ -62,20 +61,6 @@ bool validateFloats(const visualization_msgs::InteractiveMarker& msg)
   }
   return valid;
 }
-
-bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
-{
-  if ( !validateQuaternions( marker.pose.orientation )) return false;
-  for ( int c = 0; c < marker.controls.size(); ++c )
-  {
-    if ( !validateQuaternions( marker.controls[c].orientation )) return false;
-    for ( int m = 0; m < marker.controls[c].markers.size(); ++m )
-    {
-      if ( !validateQuaternions( marker.controls[c].markers[m].pose.orientation )) return false;
-    }
-  }
-  return true;
-}
 /////////////
 
 
@@ -229,13 +214,6 @@ void InteractiveMarkerDisplay::updateMarkers(
       //setStatusStd( StatusProperty::Error, "General", "Marker " + marker.name + " contains invalid floats!" );
       continue;
     }
-
-    if( !validateQuaternions( marker ))
-    {
-      setStatusStd( StatusProperty::Error, marker.name,
-                    "Marker contains invalid quaternions (length not equal to 1)!" );
-      continue;
-    }
     ROS_DEBUG("Processing interactive marker '%s'. %d", marker.name.c_str(), (int)marker.controls.size() );
 
     std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker.name );
@@ -296,13 +274,6 @@ void InteractiveMarkerDisplay::updatePoses(
       return;
     }
 
-    if( !validateQuaternions( marker_pose.pose ))
-    {
-      setStatusStd( StatusProperty::Error, marker_pose.name,
-                    "Pose message contains invalid quaternions (length not equal to 1)!" );
-      return;
-    }
-
     std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker_pose.name );
 
     if ( int_marker_entry != im_map.end() )

src/rviz/default_plugin/interactive_markers/interactive_marker.cpp

@@ -48,6 +48,7 @@
 #include "rviz/frame_manager.h"
 #include "rviz/render_panel.h"
 #include "rviz/geometry.h"
+#include "rviz/validate_quaternions.h"
 
 #include "rviz/default_plugin/interactive_markers/integer_action.h"
 #include "rviz/default_plugin/interactive_markers/interactive_marker.h"
@@ -119,11 +120,8 @@ bool InteractiveMarker::processMessage( const visualization_msgs::InteractiveMar
       message.pose.position.y,
       message.pose.position.z );
 
-  orientation_ = Ogre::Quaternion(
-      message.pose.orientation.w,
-      message.pose.orientation.x,
-      message.pose.orientation.y,
-      message.pose.orientation.z );
+  if (!normalizeQuaternion(message.pose.orientation, orientation_))
+     ROS_WARN("invalid quaternion (zero length)");
 
   pose_changed_ =false;
   time_since_last_feedback_ = 0;

src/rviz/default_plugin/map_display.cpp

@@ -656,9 +656,10 @@ void MapDisplay::showMap()
     return;
   }
 
-  if( !validateQuaternions( current_map_.info.origin ))
+  Ogre::Quaternion orientation;
+  if( !normalizeQuaternion(current_map_.info.origin.orientation, orientation))
   {
-    setStatus( StatusProperty::Error, "Map", "Message contained invalid quaternions (length not equal to 1)!" );
+    setStatus( StatusProperty::Error, "Map", "Message has invalid quaternion (length close to zero)!" );
     return;
   }
 
@@ -692,10 +693,6 @@ void MapDisplay::showMap()
   Ogre::Vector3 position( current_map_.info.origin.position.x,
                           current_map_.info.origin.position.y,
                           current_map_.info.origin.position.z );
-  Ogre::Quaternion orientation( current_map_.info.origin.orientation.w,
-                                current_map_.info.origin.orientation.x,
-                                current_map_.info.origin.orientation.y,
-                                current_map_.info.origin.orientation.z );
   frame_ = current_map_.header.frame_id;
   if (frame_.empty())
   {

src/rviz/default_plugin/marker_display.cpp

@@ -49,7 +49,6 @@
 #include "rviz/properties/ros_topic_property.h"
 #include "rviz/selection/selection_manager.h"
 #include "rviz/validate_floats.h"
-#include "rviz/validate_quaternions.h"
 
 #include "rviz/default_plugin/marker_display.h"
 
@@ -301,13 +300,6 @@ void MarkerDisplay::processMessage( const visualization_msgs::Marker::ConstPtr&
     return;
   }
 
-  if( !validateQuaternions( message->pose ))
-  {
-    setMarkerStatus( MarkerID( message->ns, message->id ), StatusProperty::Error,
-                     "Contains invalid quaternions (length not equal to 1)!" );
-    return;
-  }
-
   switch ( message->action )
   {
   case visualization_msgs::Marker::ADD:

src/rviz/default_plugin/odometry_display.cpp

@@ -34,7 +34,6 @@
 #include "rviz/properties/float_property.h"
 #include "rviz/properties/int_property.h"
 #include "rviz/validate_floats.h"
-#include "rviz/validate_quaternions.h"
 
 #include <OgreSceneManager.h>
 #include <OgreSceneNode.h>
@@ -264,12 +263,6 @@ void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& messag
     return;
   }
 
-  if( !validateQuaternions( message->pose.pose ))
-  {
-    setStatus( StatusProperty::Error, "Topic", "Message contained unnormalized quaternion (squares of values don't add to 1)" );
-    return;
-  }
-
   if( last_used_message_ )
   {
     Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, last_used_message_->pose.pose.position.y, last_used_message_->pose.pose.position.z);

src/rviz/default_plugin/path_display.cpp

@@ -45,7 +45,6 @@
 #include "rviz/properties/int_property.h"
 #include "rviz/properties/vector_property.h"
 #include "rviz/validate_floats.h"
-#include "rviz/validate_quaternions.h"
 
 #include "rviz/ogre_helpers/billboard_line.h"
 #include "rviz/default_plugin/path_display.h"
@@ -424,12 +423,6 @@ void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )
     return;
   }
 
-  if( !validateQuaternions( msg->poses ))
-  {
-    setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
-    return;
-  }
-
   // Lookup transform into fixed frame
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;

src/rviz/default_plugin/pose_array_display.cpp

@@ -65,7 +65,10 @@ namespace
 
   Ogre::Quaternion quaternionRosToOgre( geometry_msgs::Quaternion const & quaternion )
   {
-    return Ogre::Quaternion( quaternion.w, quaternion.x, quaternion.y, quaternion.z );
+    Ogre::Quaternion q;
+    if (!normalizeQuaternion(quaternion, q))
+      ROS_WARN("invalid quaternion (zero length)");
+    return q;
   }
 }
 
@@ -142,12 +145,6 @@ void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr&
     return;
   }
 
-  if( !validateQuaternions( msg->poses ))
-  {
-    setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
-    return;
-  }
-
   if( !setTransform( msg->header ) )
   {
     setStatus( StatusProperty::Error, "Topic", "Failed to look up transform" );

src/rviz/default_plugin/pose_display.cpp

@@ -43,7 +43,6 @@
 #include "rviz/properties/vector_property.h"
 #include "rviz/selection/selection_manager.h"
 #include "rviz/validate_floats.h"
-#include "rviz/validate_quaternions.h"
 
 #include "rviz/default_plugin/pose_display.h"
 
@@ -262,12 +261,6 @@ void PoseDisplay::processMessage( const geometry_msgs::PoseStamped::ConstPtr& me
     return;
   }
 
-  if( !validateQuaternions( *message ))
-  {
-    setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
-    return;
-  }
-
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
   if( !context_->getFrameManager()->transform( message->header, message->pose, position, orientation ))

src/rviz/default_plugin/pose_with_covariance_display.cpp

@@ -44,7 +44,6 @@
 #include "rviz/properties/vector_property.h"
 #include "rviz/selection/selection_manager.h"
 #include "rviz/validate_floats.h"
-#include "rviz/validate_quaternions.h"
 
 #include "pose_with_covariance_display.h"
 #include "covariance_visual.h"
@@ -306,12 +305,6 @@ void PoseWithCovarianceDisplay::processMessage( const geometry_msgs::PoseWithCov
     return;
   }
 
-  if( !validateQuaternions( message->pose.pose ))
-  {
-    setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
-    return;
-  }
-
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
   if( !context_->getFrameManager()->transform( message->header, message->pose.pose, position, orientation ))

src/rviz/validate_quaternions.h

@@ -39,65 +39,104 @@
 namespace rviz
 {
 
-inline bool validateQuaternions( float w, float x, float y, float z )
+inline float quaternionNorm2(float w, float x, float y, float z)
 {
-  return std::abs( w * w + x * x + y * y + z * z - 1.0f ) < 10e-3f;
+  return w * w + x * x + y * y + z * z;
 }
 
-inline bool validateQuaternions( double w, double x, double y, double z )
+inline bool validateQuaternion(float w, float x, float y, float z)
 {
-  return std::abs( w * w + x * x + y * y + z * z - 1.0 ) < 10e-3;
+  return std::abs(quaternionNorm2(w, x, y, z) - 1.0f) < 10e-3f;
 }
 
-inline bool validateQuaternions( Ogre::Quaternion quaternion )
+inline bool normalizeQuaternion(float& w, float& x, float& y, float& z)
 {
-  return validateQuaternions( quaternion.w, quaternion.x, quaternion.y, quaternion.z );
+  float norm2 = quaternionNorm2(w, x, y, z);
+  if (norm2 < 10e-3f)
+    return false;
+  norm2 = std::sqrt(norm2);
+  w /= norm2;
+  x /= norm2;
+  y /= norm2;
+  z /= norm2;
+  return true;
 }
 
-inline bool validateQuaternions( tf::Quaternion quaternion )
+
+inline double quaternionNorm2(double w, double x, double y, double z)
 {
-  return validateQuaternions( quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z());
+  return w * w + x * x + y * y + z * z;
 }
 
-inline bool validateQuaternions( const geometry_msgs::Quaternion &msg )
+inline bool validateQuaternion(double w, double x, double y, double z)
 {
-  return validateQuaternions( msg.w, msg.x, msg.y, msg.z );
+  return std::abs(quaternionNorm2(w, x, y, z) - 1.0) < 10e-3;
 }
 
-inline bool validateQuaternions( const geometry_msgs::Pose &msg )
+inline bool normalizeQuaternion(double& w, double& x, double& y, double& z)
 {
-  return validateQuaternions( msg.orientation );
+  float norm2 = quaternionNorm2(w, x, y, z);
+  if (norm2 < 10e-3f)
+    return false;
+  norm2 = std::sqrt(norm2);
+  w /= norm2;
+  x /= norm2;
+  y /= norm2;
+  z /= norm2;
+  return true;
 }
 
-inline bool validateQuaternions( const geometry_msgs::PoseStamped &msg )
+
+inline bool validateQuaternion(const Ogre::Quaternion& quaternion)
 {
-  return validateQuaternions( msg.pose );
+  return validateQuaternion(quaternion.w, quaternion.x, quaternion.y, quaternion.z);
 }
 
-template<typename T>
-inline bool validateQuaternions( const std::vector<T> &vec )
+inline bool validateQuaternion(const tf::Quaternion& quaternion)
 {
-  typedef std::vector<T> VecType;
-  typename VecType::const_iterator it = vec.begin();
-  typename VecType::const_iterator end = vec.end();
-  for ( ; it != end; ++it )
-  {
-    if ( !validateQuaternions( *it ))
-    {
-      return false;
-    }
-  }
+  return validateQuaternion(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z());
+}
 
-  return true;
+inline bool validateQuaternion(const geometry_msgs::Quaternion &msg)
+{
+  return validateQuaternion(msg.w, msg.x, msg.y, msg.z);
 }
 
-template<typename T, size_t N>
-inline bool validateQuaternions( const boost::array<T, N> &arr )
+inline bool validateQuaternion(const geometry_msgs::Pose &msg)
 {
-  typedef boost::array<T, N> ArrType;
-  typename ArrType::const_iterator it = arr.begin();
-  typename ArrType::const_iterator end = arr.end();
-  for ( ; it != end; ++it )
+  return validateQuaternion(msg.orientation);
+}
+
+inline bool validateQuaternion(const geometry_msgs::PoseStamped &msg)
+{
+  return validateQuaternion(msg.pose.orientation);
+}
+
+
+inline bool normalizeQuaternion(Ogre::Quaternion& quaternion)
+{
+  if (normalizeQuaternion(quaternion.w, quaternion.x, quaternion.y, quaternion.z))
+    return true;
+
+  quaternion.w = 1.0;
+  quaternion.x = quaternion.y = quaternion.z = 0.0;
+  return false;
+}
+
+inline bool normalizeQuaternion(const geometry_msgs::Quaternion &msg, Ogre::Quaternion &q)
+{
+  q.w = msg.w;
+  q.x = msg.x;
+  q.y = msg.y;
+  q.z = msg.z;
+  return normalizeQuaternion(q);
+}
+
+
+template<typename T>
+inline bool validateQuaternions(const T &vec)
+{
+  for (typename T::const_iterator it = vec.begin(), end = vec.end() ; it != end; ++it)
   {
     if ( !validateQuaternions( *it ))
     {
@@ -107,6 +146,7 @@ inline bool validateQuaternions( const boost::array<T, N> &arr )
 
   return true;
 }
+
 } // namespace rviz
 
 #endif // RVIZ_VALIDATE_QUATERNIONS_H