Go to the documentation of this file.00001 #include "resized_image_transport/log_polar_nodelet.h"
00002
00003 namespace resized_image_transport
00004 {
00005 void LogPolar::onInit(){
00006 initNodeHandle();
00007 initReconfigure();
00008 initParams();
00009 initPublishersAndSubscribers();
00010 }
00011
00012
00013 void LogPolar::initReconfigure(){
00014 reconfigure_server_ = boost::make_shared <dynamic_reconfigure::Server<LogPolarConfig> > (pnh);
00015 ReconfigureServer::CallbackType f
00016 = boost::bind(&LogPolar::config_cb, this, _1, _2);
00017 reconfigure_server_->setCallback(f);
00018 }
00019
00020 void LogPolar::initParams(){
00021 ImageProcessing::initParams();
00022 period_ = ros::Duration(1.0);
00023 pnh.param("log_polar_scale", log_polar_scale_, 100.0);
00024 NODELET_INFO("log polar scale : %f", log_polar_scale_);
00025
00026 pnh.param("inverse_log_polar", inverse_log_polar_, false);
00027 if (inverse_log_polar_){
00028 NODELET_INFO("log polar");
00029 }else{
00030 NODELET_INFO("inverse log polar");
00031 }
00032 }
00033
00034 void LogPolar::config_cb ( LogPolarConfig &config, uint32_t level) {
00035 NODELET_INFO("config_cb");
00036 resize_x_ = config.resize_scale_x;
00037 resize_y_ = config.resize_scale_y;
00038 log_polar_scale_ = config.log_polar_scale;
00039 period_ = ros::Duration(1.0/config.msg_par_second);
00040 verbose_ = config.verbose;
00041 NODELET_DEBUG("resize_scale_x : %f", resize_x_);
00042 NODELET_DEBUG("resize_scale_y : %f", resize_y_);
00043 NODELET_DEBUG("log_polar_scale : %f", log_polar_scale_);
00044 NODELET_DEBUG("message period : %f", period_.toSec());
00045 }
00046
00047
00048 void LogPolar::process(const sensor_msgs::ImageConstPtr &src_img, const sensor_msgs::CameraInfoConstPtr &src_info,
00049 sensor_msgs::ImagePtr &dst_img, sensor_msgs::CameraInfo &dst_info){
00050 int image_width, image_height;
00051 if(use_camera_info_){
00052 image_width = src_info->width;
00053 image_height = src_info->height;
00054 }else{
00055 image_width = src_img->width;
00056 image_height = src_img->height;
00057 }
00058
00059 int width = dst_width_ ? dst_width_ : (resize_x_ * image_width);
00060 int height = dst_height_ ? dst_height_ : (resize_y_ * image_height);
00061
00062 double scale_x = dst_width_ ? ((double)dst_width_)/image_width : resize_x_;
00063 double scale_y = dst_height_ ? ((double)dst_height_)/image_height : resize_y_;
00064
00065 cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(src_img);
00066 IplImage src = cv_img->image;
00067 IplImage* dst = cvCloneImage(&src);
00068
00069 int log_polar_flags = CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS;
00070 if ( inverse_log_polar_ ){
00071 log_polar_flags += CV_WARP_INVERSE_MAP;
00072 }
00073 cvLogPolar( &src, dst, cvPoint2D32f(image_width/2, image_height/2), log_polar_scale_, log_polar_flags);
00074
00075 cv_img->image = cv::cvarrToMat(dst);
00076
00077 dst_img = cv_img->toImageMsg();
00078 if(use_camera_info_){
00079 dst_info = *src_info;
00080 dst_info.height = height;
00081 dst_info.width = width;
00082 dst_info.K[0] = dst_info.K[0] * scale_x;
00083 dst_info.K[2] = dst_info.K[2] * scale_x;
00084 dst_info.K[4] = dst_info.K[4] * scale_y;
00085 dst_info.K[5] = dst_info.K[5] * scale_y;
00086
00087 dst_info.P[0] = dst_info.P[0] * scale_x;
00088 dst_info.P[2] = dst_info.P[2] * scale_x;
00089 dst_info.P[3] = dst_info.P[3] * scale_x;
00090 dst_info.P[5] = dst_info.P[5] * scale_y;
00091 dst_info.P[6] = dst_info.P[6] * scale_y;
00092 }
00093 }
00094 }
00095
00096 #include <pluginlib/class_list_macros.h>
00097 typedef resized_image_transport::LogPolar LogPolar;
00098 PLUGINLIB_EXPORT_CLASS(LogPolar, nodelet::Nodelet);