From 1d30875132284022ec535155f84453281b1b27dd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 30 Dec 2017 00:37:10 +0100 Subject: [PATCH] normalize invalid quaternions instead of rejecting them reverts most changes introduced in b3291453c75af65a4ef5606f5ae5ebe424b17f97 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 | 6 +- .../interactive_marker_display.cpp | 29 ----- .../interactive_marker.cpp | 8 +- src/rviz/default_plugin/map_display.cpp | 9 +- src/rviz/default_plugin/marker_display.cpp | 8 -- src/rviz/default_plugin/odometry_display.cpp | 7 -- src/rviz/default_plugin/path_display.cpp | 7 -- .../default_plugin/pose_array_display.cpp | 11 +- src/rviz/default_plugin/pose_display.cpp | 7 -- .../pose_with_covariance_display.cpp | 7 -- src/rviz/validate_quaternions.h | 106 ++++++++++++------ 11 files changed, 88 insertions(+), 117 deletions(-) diff --git a/src/rviz/default_plugin/covariance_visual.cpp b/src/rviz/default_plugin/covariance_visual.cpp index cb0cee3678..6caa5e16ee 100644 --- a/src/rviz/default_plugin/covariance_visual.cpp +++ b/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 #include @@ -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()); diff --git a/src/rviz/default_plugin/interactive_marker_display.cpp b/src/rviz/default_plugin/interactive_marker_display.cpp index ca5ca25c17..cd94cea7da 100644 --- a/src/rviz/default_plugin/interactive_marker_display.cpp +++ b/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() ) diff --git a/src/rviz/default_plugin/interactive_markers/interactive_marker.cpp b/src/rviz/default_plugin/interactive_markers/interactive_marker.cpp index c436a2ec1d..68c0c5bf4d 100644 --- a/src/rviz/default_plugin/interactive_markers/interactive_marker.cpp +++ b/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; diff --git a/src/rviz/default_plugin/map_display.cpp b/src/rviz/default_plugin/map_display.cpp index 6e883886c9..c1299609ca 100644 --- a/src/rviz/default_plugin/map_display.cpp +++ b/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()) { diff --git a/src/rviz/default_plugin/marker_display.cpp b/src/rviz/default_plugin/marker_display.cpp index c4ca0deec3..a972e5abf3 100644 --- a/src/rviz/default_plugin/marker_display.cpp +++ b/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: diff --git a/src/rviz/default_plugin/odometry_display.cpp b/src/rviz/default_plugin/odometry_display.cpp index d6feb72bf2..3c86b85f99 100644 --- a/src/rviz/default_plugin/odometry_display.cpp +++ b/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 #include @@ -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); diff --git a/src/rviz/default_plugin/path_display.cpp b/src/rviz/default_plugin/path_display.cpp index 9722c77374..89fa2591d7 100644 --- a/src/rviz/default_plugin/path_display.cpp +++ b/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; diff --git a/src/rviz/default_plugin/pose_array_display.cpp b/src/rviz/default_plugin/pose_array_display.cpp index 95e82abf2c..755280f504 100644 --- a/src/rviz/default_plugin/pose_array_display.cpp +++ b/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" ); diff --git a/src/rviz/default_plugin/pose_display.cpp b/src/rviz/default_plugin/pose_display.cpp index 82c9376b57..46b2c92a48 100644 --- a/src/rviz/default_plugin/pose_display.cpp +++ b/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 )) diff --git a/src/rviz/default_plugin/pose_with_covariance_display.cpp b/src/rviz/default_plugin/pose_with_covariance_display.cpp index 42dae0f649..c8ab2ff615 100644 --- a/src/rviz/default_plugin/pose_with_covariance_display.cpp +++ b/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 )) diff --git a/src/rviz/validate_quaternions.h b/src/rviz/validate_quaternions.h index 005b273556..8a7640060d 100644 --- a/src/rviz/validate_quaternions.h +++ b/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 -inline bool validateQuaternions( const std::vector &vec ) +inline bool validateQuaternion(const tf::Quaternion& quaternion) { - typedef std::vector 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 -inline bool validateQuaternions( const boost::array &arr ) +inline bool validateQuaternion(const geometry_msgs::Pose &msg) { - typedef boost::array 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 +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 &arr ) return true; } + } // namespace rviz #endif // RVIZ_VALIDATE_QUATERNIONS_H