6 DiagnosticNodelet::onInit();
14 reconfigure_server_ = boost::make_shared <dynamic_reconfigure::Server<LogPolarConfig> > (*pnh_);
15 ReconfigureServer::CallbackType
f 48 void LogPolar::process(
const sensor_msgs::ImageConstPtr &src_img,
const sensor_msgs::CameraInfoConstPtr &src_info,
49 sensor_msgs::ImagePtr &dst_img, sensor_msgs::CameraInfo &dst_info){
50 int image_width, image_height;
52 image_width = src_info->width;
53 image_height = src_info->height;
55 image_width = src_img->width;
56 image_height = src_img->height;
66 #if ( CV_MAJOR_VERSION >= 4) 67 IplImage src = cvIplImage(cv_img->image);
69 IplImage src = cv_img->image;
71 IplImage* dst = cvCloneImage(&src);
73 int log_polar_flags = CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS;
75 log_polar_flags += CV_WARP_INVERSE_MAP;
77 cvLogPolar( &src, dst, cvPoint2D32f(image_width/2, image_height/2),
log_polar_scale_, log_polar_flags);
79 cv_img->image = cv::cvarrToMat(dst);
81 dst_img = cv_img->toImageMsg();
84 dst_info.height = height;
85 dst_info.width = width;
86 dst_info.K[0] = dst_info.K[0] * scale_x;
87 dst_info.K[2] = dst_info.K[2] * scale_x;
88 dst_info.K[4] = dst_info.K[4] * scale_y;
89 dst_info.K[5] = dst_info.K[5] * scale_y;
91 dst_info.P[0] = dst_info.P[0] * scale_x;
92 dst_info.P[2] = dst_info.P[2] * scale_x;
93 dst_info.P[3] = dst_info.P[3] * scale_x;
94 dst_info.P[5] = dst_info.P[5] * scale_y;
95 dst_info.P[6] = dst_info.P[6] * scale_y;
void process(const sensor_msgs::ImageConstPtr &src_img, const sensor_msgs::CameraInfoConstPtr &src_info, sensor_msgs::ImagePtr &dst_img, sensor_msgs::CameraInfo &dst_info)
boost::shared_ptr< ReconfigureServer > reconfigure_server_
void initPublishersAndSubscribers()
resized_image_transport::LogPolar LogPolar
PLUGINLIB_EXPORT_CLASS(LogPolar, nodelet::Nodelet)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void config_cb(LogPolarConfig &config, uint32_t level)
#define NODELET_INFO(...)
#define NODELET_DEBUG(...)