Rotate image with transformation from image frame to target one

CMakeLists.txt

@@ -106,6 +106,7 @@ endif()
 find_package(catkin REQUIRED
   COMPONENTS
   angles
+  cv_bridge
   cmake_modules
   geometry_msgs
   image_geometry
@@ -127,6 +128,8 @@ find_package(catkin REQUIRED
   std_msgs
   std_srvs
   tf
+  tf2
+  tf2_geometry_msgs
   urdf
   visualization_msgs
 )

package.xml

@@ -20,6 +20,7 @@
 
   <build_depend>assimp-dev</build_depend>
   <build_depend>cmake_modules</build_depend>
+  <build_depend>cv_bridge</build_depend>
   <build_depend>eigen</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>image_geometry</build_depend>
@@ -44,13 +45,16 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_depend>tf</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
   <build_depend>tinyxml</build_depend>
   <build_depend>urdf</build_depend>
   <build_depend>visualization_msgs</build_depend>
   <build_depend>yaml-cpp</build_depend>
   <build_depend>opengl</build_depend>
 
   <run_depend>assimp</run_depend>
+  <run_depend>cv_bridge</run_depend>
   <run_depend>eigen</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>image_geometry</run_depend>
@@ -76,6 +80,8 @@
   <run_depend>std_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
   <run_depend>tinyxml</run_depend>
   <run_depend>urdf</run_depend>
   <run_depend>visualization_msgs</run_depend>

src/rviz/CMakeLists.txt

@@ -4,9 +4,15 @@ if(NEW_YAMLCPP_FOUND)
   add_definitions(-DRVIZ_HAVE_YAMLCPP_05)
 endif(NEW_YAMLCPP_FOUND)
 
+if(UNIX AND NOT APPLE)
+  find_package(X11 REQUIRED)
+endif()
+
+find_package(OpenCV REQUIRED core imgproc)
+
 add_subdirectory(default_plugin)
 
-include_directories(.)
+include_directories(. ${OpenCV_INCLUDE_DIRS})
 
 # Build the version number and other build-time constants into the
 # source for access at run-time.
@@ -128,10 +134,12 @@ add_library( ${PROJECT_NAME}
 target_link_libraries(${PROJECT_NAME}
   ${Boost_LIBRARIES}
   ${catkin_LIBRARIES}
+  ${QT_LIBRARIES}
   ${OGRE_OV_LIBRARIES_ABS}
   ${OPENGL_LIBRARIES}
-  ${QT_LIBRARIES}
   ${rviz_ADDITIONAL_LIBRARIES}
+  ${X11_X11_LIB}
+  ${OpenCV_LIBRARIES}
   assimp
   yaml-cpp
 )

src/rviz/default_plugin/image_display.cpp

@@ -71,6 +71,8 @@ ImageDisplay::ImageDisplay()
   median_buffer_size_property_ = new IntProperty( "Median window", 5, "Window size for median filter used for computin min/max.",
                                                   this, SLOT( updateNormalizeOptions() ) );
 
+  target_frame_property_ = new StringProperty( "Target Frame", "", "Target frame to rotate the image", this, SLOT( updateTargetFrame() ));
+
   got_float_image_ = false;
 }
 
@@ -154,6 +156,15 @@ void ImageDisplay::onDisable()
   clear();
 }
 
+void ImageDisplay::updateTargetFrame()
+{
+  std::string target_frame = target_frame_property_->getStdString();
+  if (!target_frame.empty())
+  {
+    texture_.setRotateImageFrame(target_frame);
+  }
+}
+
 void ImageDisplay::updateNormalizeOptions()
 {
   if (got_float_image_)
@@ -242,6 +253,12 @@ void ImageDisplay::processMessage(const sensor_msgs::Image::ConstPtr& msg)
     got_float_image_ = got_float_image;
     updateNormalizeOptions();
   }
+
+  if (target_frame_property_->getStdString().empty())
+  {
+    target_frame_property_->setValue(QString(msg->header.frame_id.c_str()));
+  }
+
   texture_.addMessage(msg);
 }
 

src/rviz/default_plugin/image_display.h

@@ -74,6 +74,7 @@ Q_OBJECT
 
 public Q_SLOTS:
   virtual void updateNormalizeOptions();
+  virtual void updateTargetFrame();
 
 protected:
   // overrides from Display
@@ -100,6 +101,7 @@ public Q_SLOTS:
   FloatProperty* min_property_;
   FloatProperty* max_property_;
   IntProperty* median_buffer_size_property_;
+  StringProperty* target_frame_property_;
   bool got_float_image_;
 };
 

src/rviz/image/ros_image_texture.cpp

@@ -36,8 +36,14 @@
 #include <boost/foreach.hpp>
 
 #include <OgreTextureManager.h>
+#include <opencv2/imgproc/imgproc.hpp>
 
+#include <cv_bridge/cv_bridge.h>
+#include <geometry_msgs/Vector3Stamped.h>
 #include <sensor_msgs/image_encodings.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include "rviz/image/ros_image_texture.h"
 
@@ -49,9 +55,12 @@ ROSImageTexture::ROSImageTexture()
 , width_(0)
 , height_(0)
 , median_frames_(5)
+, target_frame_("")
 {
   empty_image_.load("no_image.png", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
 
+  tf_sub_.reset(new tf2_ros::TransformListener(tf_buffer_));
+
   static uint32_t count = 0;
   std::stringstream ss;
   ss << "ROSImageTexture" << count++;
@@ -88,6 +97,11 @@ void ROSImageTexture::setMedianFrames( unsigned median_frames )
   median_frames_ = median_frames;
 }
 
+void ROSImageTexture::setRotateImageFrame(std::string target_frame)
+{
+  target_frame_ = target_frame;
+}
+
 double ROSImageTexture::updateMedian( std::deque<double>& buffer, double value )
 {
   //update buffer
@@ -165,29 +179,108 @@ void ROSImageTexture::normalize( T* image_data, size_t image_data_size, std::vec
   }
 }
 
+sensor_msgs::Image::Ptr ROSImageTexture::rotateImage(sensor_msgs::Image::ConstPtr msg)
+{
+  // Convert the image into something opencv can handle.
+  cv::Mat in_image = cv_bridge::toCvShare(msg, msg->encoding)->image;
+
+  // Compute the output image size.
+  int max_dim = in_image.cols > in_image.rows ? in_image.cols : in_image.rows;
+  int min_dim = in_image.cols < in_image.rows ? in_image.cols : in_image.rows;
+  int noblack_dim = min_dim / sqrt(2);
+  int diag_dim = sqrt(in_image.cols*in_image.cols + in_image.rows*in_image.rows);
+  int candidates[] = {noblack_dim, min_dim, max_dim, diag_dim, diag_dim}; // diag_dim repeated to simplify limit case.
+  int output_image_size = 2;
+  int step = output_image_size;
+  int out_size = candidates[step] + (candidates[step + 1] - candidates[step]) * (output_image_size - step);
+
+  geometry_msgs::Vector3Stamped target_vector;
+  target_vector.header.stamp = msg->header.stamp;
+  target_vector.header.frame_id = target_frame_;
+  target_vector.vector.z = 1;
+  if (!target_frame_.empty())
+  {
+    // Transform the target vector into the image frame.
+    try
+    {
+      geometry_msgs::TransformStamped transform = tf_buffer_.lookupTransform(
+        target_frame_, msg->header.frame_id, msg->header.stamp);
+      tf2::doTransform(target_vector, target_vector, transform);
+    }
+    catch (tf2::LookupException& e)
+    {
+      ROS_ERROR_THROTTLE(30, "Error rotating image: %s", e.what());
+    }
+  }
+
+  // Transform the source vector into the image frame.
+  geometry_msgs::Vector3Stamped source_vector;
+  source_vector.header.stamp = msg->header.stamp;
+  source_vector.header.frame_id = msg->header.frame_id;
+  source_vector.vector.y = -1;
+  if (!msg->header.frame_id.empty())
+  {
+    try
+    {
+      geometry_msgs::TransformStamped transform = tf_buffer_.lookupTransform(
+        msg->header.frame_id, msg->header.frame_id, msg->header.stamp);
+      tf2::doTransform(source_vector, source_vector, transform);
+    }
+    catch (tf2::LookupException& e)
+    {
+      ROS_ERROR_THROTTLE(30, "Error rotating image: %s", e.what());
+    }
+  }
+
+  // Calculate the angle of the rotation.
+  double angle = 0;
+  if ((target_vector.vector.x != 0 || target_vector.vector.y != 0) &&
+      (source_vector.vector.x != 0 || source_vector.vector.y != 0))
+  {
+    angle = atan2(target_vector.vector.y, target_vector.vector.x);
+    angle -= atan2(source_vector.vector.y, source_vector.vector.x);
+  }
+
+  // Compute the rotation matrix.
+  cv::Mat rot_matrix = cv::getRotationMatrix2D(cv::Point2f(in_image.cols / 2.0, in_image.rows / 2.0), 180 * angle / M_PI, 1);
+  cv::Mat translation = rot_matrix.col(2);
+  rot_matrix.at<double>(0, 2) += (out_size - in_image.cols) / 2.0;
+  rot_matrix.at<double>(1, 2) += (out_size - in_image.rows) / 2.0;
+
+  // Do the rotation
+  cv::Mat out_image;
+  cv::warpAffine(in_image, out_image, rot_matrix, cv::Size(out_size, out_size));
+
+  sensor_msgs::Image::Ptr dst_msg = cv_bridge::CvImage(msg->header, msg->encoding, out_image).toImageMsg();
+  dst_msg->header.frame_id = target_frame_;
+  return dst_msg;
+}
+
 bool ROSImageTexture::update()
 {
-  sensor_msgs::Image::ConstPtr image;
+  sensor_msgs::Image::ConstPtr raw_image;
   bool new_image = false;
   {
     boost::mutex::scoped_lock lock(mutex_);
 
-    image = current_image_;
+    raw_image = current_image_;
     new_image = new_image_;
   }
 
-  if (!image || !new_image)
+  if (!raw_image || !new_image)
   {
     return false;
   }
 
   new_image_ = false;
 
-  if (image->data.empty())
+  if (raw_image->data.empty())
   {
     return false;
   }
 
+  sensor_msgs::Image::Ptr image = rotateImage(raw_image);
+
   Ogre::PixelFormat format = Ogre::PF_R8G8B8;
   Ogre::Image ogre_image;
   std::vector<uint8_t> buffer;

src/rviz/image/ros_image_texture.h

@@ -40,6 +40,8 @@
 #include <boost/thread/mutex.hpp>
 
 #include <ros/ros.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
 
 #include <stdexcept>
 
@@ -73,14 +75,19 @@ class ROSImageTexture
   // automatic range normalization
   void setNormalizeFloatImage( bool normalize, double min=0.0, double max=1.0 );
   void setMedianFrames( unsigned median_frames );
+  void setRotateImageFrame(std::string source_frame);
 
 private:
 
   double updateMedian( std::deque<double>& buffer, double new_value );
 
+  sensor_msgs::Image::Ptr rotateImage(sensor_msgs::Image::ConstPtr msg);
+
   template<typename T>
   void normalize( T* image_data, size_t image_data_size, std::vector<uint8_t> &buffer  );
 
+  tf2_ros::Buffer tf_buffer_;
+  boost::shared_ptr<tf2_ros::TransformListener> tf_sub_;
   sensor_msgs::Image::ConstPtr current_image_;
   boost::mutex mutex_;
   bool new_image_;
@@ -98,6 +105,7 @@ class ROSImageTexture
   unsigned median_frames_;
   std::deque<double> min_buffer_;
   std::deque<double> max_buffer_;
+  std::string target_frame_;
 };
 
 }