From 205475e3740d64ab7256565432c71d192648191d Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sun, 17 Jul 2016 03:30:49 +0900 Subject: [PATCH] Rotate image with transformation from image frame to target one --- CMakeLists.txt | 3 + package.xml | 2 + src/rviz/CMakeLists.txt | 12 ++- src/rviz/default_plugin/image_display.cpp | 17 ++++ src/rviz/default_plugin/image_display.h | 2 + src/rviz/image/ros_image_texture.cpp | 101 +++++++++++++++++++++- src/rviz/image/ros_image_texture.h | 8 ++ 7 files changed, 139 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ea4bdf36b8..261502d31f 100644 --- a/CMakeLists.txt +++ b/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 ) diff --git a/package.xml b/package.xml index 635bb25d81..66c612f24c 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ assimp-dev cmake_modules + cv_bridge eigen geometry_msgs image_geometry @@ -51,6 +52,7 @@ opengl assimp + cv_bridge eigen geometry_msgs image_geometry diff --git a/src/rviz/CMakeLists.txt b/src/rviz/CMakeLists.txt index b4caf45029..ed3ad4eecd 100644 --- a/src/rviz/CMakeLists.txt +++ b/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 ) diff --git a/src/rviz/default_plugin/image_display.cpp b/src/rviz/default_plugin/image_display.cpp index 48d9c3b502..faaa4c664f 100644 --- a/src/rviz/default_plugin/image_display.cpp +++ b/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); } diff --git a/src/rviz/default_plugin/image_display.h b/src/rviz/default_plugin/image_display.h index 2dc94f0252..c59af4e79a 100644 --- a/src/rviz/default_plugin/image_display.h +++ b/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_; }; diff --git a/src/rviz/image/ros_image_texture.cpp b/src/rviz/image/ros_image_texture.cpp index f694622298..a4b018bc1d 100644 --- a/src/rviz/image/ros_image_texture.cpp +++ b/src/rviz/image/ros_image_texture.cpp @@ -36,8 +36,14 @@ #include #include +#include +#include +#include #include +#include +#include +#include #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& 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(0, 2) += (out_size - in_image.cols) / 2.0; + rot_matrix.at(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 buffer; diff --git a/src/rviz/image/ros_image_texture.h b/src/rviz/image/ros_image_texture.h index d79cf11cd1..ce3982a1f3 100644 --- a/src/rviz/image/ros_image_texture.h +++ b/src/rviz/image/ros_image_texture.h @@ -40,6 +40,8 @@ #include #include +#include +#include #include @@ -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& buffer, double new_value ); + sensor_msgs::Image::Ptr rotateImage(sensor_msgs::Image::ConstPtr msg); + template void normalize( T* image_data, size_t image_data_size, std::vector &buffer ); + tf2_ros::Buffer tf_buffer_; + boost::shared_ptr 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 min_buffer_; std::deque max_buffer_; + std::string target_frame_; }; }