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