Go to the documentation of this file.00001 #include "resized_image_transport/image_resizer_nodelet.h"
00002
00003 namespace resized_image_transport
00004 {
00005 void ImageResizer::onInit() {
00006 raw_width_ = 0;
00007 raw_height_ = 0;
00008 initNodeHandle();
00009 initParams();
00010 initReconfigure();
00011 initPublishersAndSubscribers();
00012 sub_ = pnh.subscribe("input/mask", 1, &ImageResizer::mask_region_callback, this);
00013 }
00014
00015 void ImageResizer::initReconfigure() {
00016 reconfigure_server_ = boost::make_shared <dynamic_reconfigure::Server<ImageResizerConfig> > (pnh);
00017 ReconfigureServer::CallbackType f
00018 = boost::bind(&ImageResizer::config_cb, this, _1, _2);
00019 reconfigure_server_->setCallback(f);
00020 }
00021
00022 void ImageResizer::initParams() {
00023 ImageProcessing::initParams();
00024 period_ = ros::Duration(1.0);
00025 std::string interpolation_method;
00026 pnh.param<std::string>("interpolation", interpolation_method, "LINEAR");
00027 if(interpolation_method == "NEAREST"){
00028 interpolation_ = cv::INTER_NEAREST;
00029 }else if(interpolation_method == "LINEAR") {
00030 interpolation_ = cv::INTER_LINEAR;
00031 }else if(interpolation_method == "AREA") {
00032 interpolation_ = cv::INTER_AREA;
00033 }else if(interpolation_method == "CUBIC") {
00034 interpolation_ = cv::INTER_CUBIC;
00035 }else if(interpolation_method == "LANCZOS4") {
00036 interpolation_ = cv::INTER_LANCZOS4;
00037 }else{
00038 ROS_ERROR("unknown interpolation method");
00039 }
00040 }
00041
00042 void ImageResizer::mask_region_callback(const sensor_msgs::Image::ConstPtr& msg) {
00043 boost::mutex::scoped_lock lock(mutex_);
00044 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy
00045 (msg, sensor_msgs::image_encodings::MONO8);
00046 cv::Mat mask = cv_ptr->image;
00047 int step_x, step_y, ox, oy;
00048 int pixel_x = 0;
00049 int pixel_y = 0;
00050 int maskwidth = mask.cols;
00051 int maskheight = mask.rows;
00052 int cnt = 0;
00053 for (size_t j = 0; j < maskheight; j++) {
00054 for (size_t i = 0; i < maskwidth; i++) {
00055 if (mask.at<uchar>(j, i) != 0) {
00056 cnt++;
00057 }
00058 }
00059 }
00060 int surface_per = ((double) cnt) / (maskwidth * maskheight) * 100;
00061
00062 step_x = sqrt (surface_per);
00063 if (step_x < 1) {
00064 step_x = 1;
00065 }
00066 step_y = step_x;
00067
00068
00069 ox = step_x / 2;
00070 oy = step_y / 2;
00071 for (int i = ox; i < raw_width_; i += step_x) {
00072 pixel_x++;
00073 }
00074 for (int i = oy; i < raw_height_; i += step_y) {
00075 pixel_y++;
00076 }
00077 resize_x_ = ((double) pixel_x) / raw_width_;
00078 resize_y_ = ((double) pixel_y) / raw_height_;
00079 }
00080
00081 void ImageResizer::config_cb ( ImageResizerConfig &config, uint32_t level) {
00082 NODELET_INFO("config_cb");
00083 resize_x_ = config.resize_scale_x;
00084 resize_y_ = config.resize_scale_y;
00085 period_ = ros::Duration(1.0/config.msg_par_second);
00086 verbose_ = config.verbose;
00087 NODELET_DEBUG("resize_scale_x : %f", resize_x_);
00088 NODELET_DEBUG("resize_scale_y : %f", resize_y_);
00089 NODELET_DEBUG("message period : %f", period_.toSec());
00090 }
00091
00092
00093 void ImageResizer::process(const sensor_msgs::ImageConstPtr &src_img, const sensor_msgs::CameraInfoConstPtr &src_info,
00094 sensor_msgs::ImagePtr &dst_img, sensor_msgs::CameraInfo &dst_info){
00095 int image_width, image_height;
00096 if(use_camera_info_) {
00097 image_width = src_info->width;
00098 image_height = src_info->height;
00099 }
00100 else {
00101 image_width = src_img->width;
00102 image_height = src_img->height;
00103 }
00104
00105 int width = dst_width_ ? dst_width_ : (resize_x_ * image_width);
00106 int height = dst_height_ ? dst_height_ : (resize_y_ * image_height);
00107
00108 double scale_x = dst_width_ ? ((double)dst_width_)/image_width : resize_x_;
00109 double scale_y = dst_height_ ? ((double)dst_height_)/image_height : resize_y_;
00110
00111 cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(src_img);
00112
00113 cv::Mat tmpmat(height, width, cv_img->image.type());
00114 if (raw_width_ == 0) {
00115 raw_width_ = tmpmat.cols;
00116 raw_height_ = tmpmat.rows;
00117 }
00118 cv::resize(cv_img->image, tmpmat, cv::Size(width, height), 0, 0, interpolation_);
00119 NODELET_DEBUG("mat rows:%d cols:%d", tmpmat.rows, tmpmat.cols);
00120 cv_img->image = tmpmat;
00121
00122 dst_img = cv_img->toImageMsg();
00123 if (use_camera_info_) {
00124 dst_info = *src_info;
00125 dst_info.height = height;
00126 dst_info.width = width;
00127 dst_info.K[0] = dst_info.K[0] * scale_x;
00128 dst_info.K[2] = dst_info.K[2] * scale_x;
00129 dst_info.K[4] = dst_info.K[4] * scale_y;
00130 dst_info.K[5] = dst_info.K[5] * scale_y;
00131
00132 dst_info.P[0] = dst_info.P[0] * scale_x;
00133 dst_info.P[2] = dst_info.P[2] * scale_x;
00134 dst_info.P[3] = dst_info.P[3] * scale_x;
00135 dst_info.P[5] = dst_info.P[5] * scale_y;
00136 dst_info.P[6] = dst_info.P[6] * scale_y;
00137 }
00138 }
00139 }
00140
00141 #include <pluginlib/class_list_macros.h>
00142 typedef resized_image_transport::ImageResizer ImageResizer;
00143 PLUGINLIB_EXPORT_CLASS(ImageResizer, nodelet::Nodelet);