qrcode_detection.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <hector_qrcode_detection/qrcode_detection.h>
00030 #include <hector_worldmodel_msgs/ImagePercept.h>
00031 
00032 #include <cv.h>
00033 #include <cv_bridge/cv_bridge.h>
00034 #include <zbar.h>
00035 
00036 using namespace zbar;
00037 
00038 namespace hector_qrcode_detection {
00039 
00040 qrcode_detection_impl::qrcode_detection_impl(ros::NodeHandle nh, ros::NodeHandle priv_nh)
00041   : nh_(nh)
00042   , image_transport_(nh_)
00043   , listener_(0)
00044 {
00045   scanner_ = new zbar::ImageScanner;
00046   scanner_->set_config(ZBAR_QRCODE, ZBAR_CFG_ENABLE, 1);
00047 
00048   rotation_image_size_ = 2;
00049   priv_nh.getParam("rotation_source_frame", rotation_source_frame_id_);
00050   priv_nh.getParam("rotation_target_frame", rotation_target_frame_id_);
00051   priv_nh.getParam("rotation_image_size", rotation_image_size_);
00052 
00053   percept_publisher_ = nh_.advertise<hector_worldmodel_msgs::ImagePercept>("image_percept", 10);
00054   qrcode_image_publisher_ = image_transport_.advertiseCamera("image/qrcode", 10);
00055   camera_subscriber_ = image_transport_.subscribeCamera("image", 10, &qrcode_detection_impl::imageCallback, this);
00056 
00057   if (!rotation_target_frame_id_.empty()) {
00058     listener_ = new tf::TransformListener();
00059     rotated_image_publisher_ = image_transport_.advertiseCamera("image/rotated", 10);
00060   }
00061 
00062   ROS_INFO("Successfully initialized the zbar qrcode detector for image %s", camera_subscriber_.getTopic().c_str());
00063 }
00064 
00065 qrcode_detection_impl::~qrcode_detection_impl()
00066 {
00067   delete listener_;
00068 }
00069 
00070 void qrcode_detection_impl::imageCallback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::CameraInfoConstPtr& camera_info)
00071 {
00072   cv_bridge::CvImageConstPtr cv_image;
00073   cv_image = cv_bridge::toCvShare(image, "mono8");
00074   cv::Mat rotation_matrix = cv::Mat::eye(2,3,CV_32FC1);
00075   double rotation_angle = 0.0;
00076 
00077   ROS_DEBUG("Received new image with %u x %u pixels.", image->width, image->height);
00078 
00079   if (!rotation_target_frame_id_.empty() && listener_) {
00080     tf::StampedTransform transform;
00081     std::string source_frame_id_ = rotation_source_frame_id_.empty() ? image->header.frame_id : rotation_source_frame_id_;
00082     try
00083     {
00084       listener_->waitForTransform(rotation_target_frame_id_, source_frame_id_, image->header.stamp, ros::Duration(1.0));
00085       listener_->lookupTransform(rotation_target_frame_id_, source_frame_id_, image->header.stamp, transform);
00086     } catch (tf::TransformException& e) {
00087       ROS_ERROR("%s", e.what());
00088       return;
00089     }
00090 
00091     // calculate rotation angle
00092     tfScalar roll, pitch, yaw;
00093     transform.getBasis().getRPY(roll, pitch, yaw);
00094     rotation_angle = -roll;
00095 
00096     // Transform the image.
00097     try
00098     {
00099       cv::Mat in_image = cv_image->image;
00100 
00101       // Compute the output image size.
00102       int max_dim = in_image.cols > in_image.rows ? in_image.cols : in_image.rows;
00103       int min_dim = in_image.cols < in_image.rows ? in_image.cols : in_image.rows;
00104       int noblack_dim = min_dim / sqrt(2);
00105       int diag_dim = sqrt(in_image.cols*in_image.cols + in_image.rows*in_image.rows);
00106       int out_size;
00107       int candidates[] = { noblack_dim, min_dim, max_dim, diag_dim, diag_dim }; // diag_dim repeated to simplify limit case.
00108       int step = rotation_image_size_;
00109       out_size = candidates[step] + (candidates[step + 1] - candidates[step]) * (rotation_image_size_ - step);
00110       //ROS_INFO("out_size: %d", out_size);
00111 
00112       // Compute the rotation matrix.
00113       rotation_matrix = cv::getRotationMatrix2D(cv::Point2f(in_image.cols / 2.0, in_image.rows / 2.0), 180 * rotation_angle / M_PI, 1);
00114       rotation_matrix.at<double>(0, 2) += (out_size - in_image.cols) / 2.0;
00115       rotation_matrix.at<double>(1, 2) += (out_size - in_image.rows) / 2.0;
00116 
00117       // Do the rotation
00118       cv_bridge::CvImage *temp = new cv_bridge::CvImage(*cv_image);
00119       cv::warpAffine(in_image, temp->image, rotation_matrix, cv::Size(out_size, out_size));
00120       cv_image.reset(temp);
00121 
00122       if (rotated_image_publisher_.getNumSubscribers() > 0) {
00123         sensor_msgs::Image rotated_image;
00124         cv_image->toImageMsg(rotated_image);
00125         rotated_image_publisher_.publish(rotated_image, *camera_info);
00126       }
00127     }
00128     catch (cv::Exception &e)
00129     {
00130       ROS_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00131       return;
00132     }
00133   }
00134 
00135   // wrap image data
00136   Image zbar(cv_image->image.cols, cv_image->image.rows, "Y800", cv_image->image.data, cv_image->image.cols * cv_image->image.rows);
00137 
00138   // scan the image for barcodes
00139   scanner_->scan(zbar);
00140 
00141   // extract results
00142   hector_worldmodel_msgs::ImagePercept percept;
00143   percept.header = image->header;
00144   percept.camera_info = *camera_info;
00145   percept.info.class_id = "qrcode";
00146   percept.info.class_support = 1.0;
00147 
00148   for(Image::SymbolIterator symbol = zbar.symbol_begin(); symbol != zbar.symbol_end(); ++symbol) {
00149     ROS_DEBUG_STREAM("Decoded " << symbol->get_type_name() << " symbol \"" << symbol->get_data() << '"');
00150 
00151     // percept.info.object_id = ros::this_node::getName() + "/" + symbol->get_data();
00152     //percept.info.object_id = symbol->get_data();
00153     percept.info.object_support = 1.0;
00154     percept.info.name = symbol->get_data();
00155 
00156     if (symbol->get_location_size() != 4) {
00157       ROS_WARN("Could not get symbol locations(location_size != 4)");
00158       continue;
00159     }
00160 
00161     // point order is left/top, left/bottom, right/bottom, right/top
00162     int min_x = 99999999, min_y = 99999999, max_x = 0, max_y = 0;
00163     for(int i = 0; i < 4; ++i) {
00164       if (symbol->get_location_x(i) > max_x) max_x = symbol->get_location_x(i);
00165       if (symbol->get_location_x(i) < min_x) min_x = symbol->get_location_x(i);
00166       if (symbol->get_location_y(i) > max_y) max_y = symbol->get_location_y(i);
00167       if (symbol->get_location_y(i) < min_y) min_y = symbol->get_location_y(i);
00168     }
00169 
00170     // rotate the percept back
00171     cv::Vec3f left_top_corner(min_x, min_y, 1.0f);
00172     cv::Vec3f right_bottom_corner(max_x, max_y, 1.0f);
00173 
00174     // TODO: calculate the inverse transformation of rotation_matrix
00175     if (rotation_angle != 0.0) {
00176       ROS_ERROR("Non-zero rotations are currently not supported!");
00177       continue;
00178     }
00179 
00180     percept.x      = (left_top_corner(0) + right_bottom_corner(0)) / 2;
00181     percept.y      = (left_top_corner(1) + right_bottom_corner(1)) / 2;
00182     percept.width  = right_bottom_corner(0) - left_top_corner(0);
00183     percept.height = right_bottom_corner(1) - left_top_corner(1);
00184     percept_publisher_.publish(percept);
00185 
00186 //    ROS_DEBUG("location: min_x: %d  min_y: %d  max_x: %d  max_y: %d", min_x, min_y, max_x, max_y);
00187 //    ROS_DEBUG("rotated:  min_x: %f  min_y: %f  max_x: %f  max_y: %f", left_top_corner(0), left_top_corner(1), right_bottom_corner(0), right_bottom_corner(1));
00188 //    ROS_DEBUG("percept:  x: %f  y: %f  width: %f  height: %f", percept.x, percept.y, percept.width, percept.height);
00189 
00190     if (qrcode_image_publisher_.getNumSubscribers() > 0) {
00191       try {
00192         cv::Rect rect(cv::Point2i(std::max(min_x, 0), std::max(min_y, 0)), cv::Point2i(std::min(max_x, cv_image->image.cols), std::min(max_y, cv_image->image.rows)));
00193         cv_bridge::CvImagePtr qrcode_cv(new cv_bridge::CvImage(*cv_image));
00194         qrcode_cv->image = cv_image->image(rect);
00195 
00196         sensor_msgs::Image qrcode_image;
00197         qrcode_cv->toImageMsg(qrcode_image);
00198         qrcode_image_publisher_.publish(qrcode_image, *camera_info);
00199       } catch(cv::Exception& e) {
00200         ROS_ERROR("cv::Exception: %s", e.what());
00201       }
00202     }
00203   }
00204 
00205   // clean up
00206   zbar.set_data(NULL, 0);
00207 }
00208 
00209 } // namespace hector_qrcode_detection


hector_qrcode_detection
Author(s): Johannes Meyer
autogenerated on Mon Oct 6 2014 10:37:14