00001 #include <ros/ros.h>
00002 #include <opencv2/opencv.hpp>
00003 #include <jsk_topic_tools/log_utils.h>
00004 #include <rospack/rospack.h>
00005 #include <stdio.h>
00006 #include <stdlib.h>
00007 #include <cv_bridge/cv_bridge.h>
00008 #include <image_transport/image_transport.h>
00009 #include <image_transport/subscriber_filter.h>
00010 #include <image_geometry/pinhole_camera_model.h>
00011 #include <boost/thread/mutex.hpp>
00012 #include <dynamic_reconfigure/server.h>
00013 #include <sensor_msgs/image_encodings.h>
00014 #include <sensor_msgs/Image.h>
00015 #include <sensor_msgs/CameraInfo.h>
00016 #include <geometry_msgs/PolygonStamped.h>
00017 #include <geometry_msgs/PointStamped.h>
00018 #include <jsk_recognition_msgs/Rect.h>
00019 #include <jsk_perception/ColorHistogramSlidingMatcherConfig.h>
00020 #include <jsk_recognition_msgs/BoundingBoxArray.h>
00021 #include <message_filters/subscriber.h>
00022 #include <message_filters/synchronizer.h>
00023 #include <message_filters/sync_policies/exact_time.h>
00024 #include <message_filters/sync_policies/approximate_time.h>
00025
00026 using namespace std;
00027 using namespace ros;
00028
00029 class MatcherNode
00030 {
00031 typedef message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::CameraInfo > SyncPolicy;
00032 boost::mutex _mutex;
00033 ros::NodeHandle _node;
00034 image_transport::ImageTransport _it;
00035 image_transport::SubscriberFilter _subImage;
00036 message_filters::Subscriber<sensor_msgs::CameraInfo> _subInfo;
00037 message_filters::Synchronizer< SyncPolicy > sync;
00038
00039 ros::Publisher _pubBestRect;
00040 ros::Publisher _pubBestPolygon;
00041 ros::Publisher _pubBestPoint;
00042 ros::Publisher _pubBestBoundingBox;
00043 image_transport::Publisher _debug_pub;
00044 std::vector< cv::Mat > template_images;
00045 std::vector< std::vector<unsigned int> > template_vecs;
00046 std::vector< std::vector< std::vector<unsigned int> > >hsv_integral;
00047 int standard_height, standard_width;
00048 int best_window_estimation_method;
00049 double coefficient_thre;
00050 double coefficient_thre_for_best_window;
00051 double sliding_factor;
00052 bool show_result_;
00053 bool pub_debug_image_;
00054 double template_width;
00055 double template_height;
00056 inline double calc_coe(std::vector< unsigned int > &a, std::vector<unsigned int> &b){
00057 unsigned long sum_a=0, sum_b=0;
00058 double coe=0;
00059 for(int k=0, size=a.size(); k<size; ++k){
00060 sum_a += a[k]; sum_b+= b[k];
00061 }
00062 for(int k=0; k<256; ++k){
00063 coe +=std::min((double)a[k]/sum_a, (double)b[k]/sum_b);
00064 }
00065 return coe;
00066 }
00067 typedef struct BOX{
00068 int x, y, dx, dy;
00069 double coefficient;
00070 BOX(int _x, int _y, int _dx, int _dy){
00071 x=_x, y=_y, dx=_dx, dy=_dy;
00072 }
00073 BOX(int _x, int _y, int _dx, int _dy, double _coefficient){
00074 x=_x, y=_y, dx=_dx, dy=_dy, coefficient = _coefficient;
00075 }
00076 } box;
00077 inline bool point_in_box(int x, int y, box a_box){
00078 return a_box.x <= x && a_box.x+a_box.dx >= x && a_box.y <= y && a_box.y+a_box.dy >= y;
00079 }
00080 inline bool in_box(int x, int y, int dx, int dy, box a_box){
00081 return point_in_box(x, y, a_box) || point_in_box(x+dx, y, a_box) || point_in_box(x, y+dy, a_box) || point_in_box(x+dx, y+dy, a_box);
00082 }
00083 inline bool in_boxes(int x, int y, int dx, int dy, std::vector<box> boxes){
00084 for (int i=0; i<boxes.size(); ++i){
00085 if(in_box(x, y, dx, dy, boxes[i])){
00086 return true;
00087 }
00088 }
00089 return false;
00090 }
00091 public:
00092 MatcherNode() : _it(_node)
00093 , _subImage( _it, "image", 1)
00094 , _subInfo(_node, "camera_info", 1)
00095 , sync( SyncPolicy(10), _subImage, _subInfo)
00096 {
00097 _pubBestRect = _node.advertise< jsk_recognition_msgs::Rect >("best_rect", 1);
00098 _pubBestPolygon = _node.advertise< geometry_msgs::PolygonStamped >("best_polygon", 1);
00099 _pubBestPoint = _node.advertise< geometry_msgs::PointStamped >("rough_point", 1);
00100 _pubBestBoundingBox = _node.advertise< jsk_recognition_msgs::BoundingBoxArray >("best_box", 1);
00101 _debug_pub = _it.advertise("debug_image", 1);
00102
00103
00104 sync.registerCallback( boost::bind (&MatcherNode::image_cb , this, _1, _2) );
00105
00106
00107 ros::NodeHandle local_nh("~");
00108 std::string template_filename;
00109 std::string default_template_file_name;
00110 try {
00111 #ifdef ROSPACK_EXPORT
00112 rospack::ROSPack rp;
00113 rospack::Package *p = rp.get_pkg("jsk_perception");
00114 if (p!=NULL) default_template_file_name = p->path + std::string("/sample/opencv-logo2.png");
00115 #else
00116 rospack::Rospack rp;
00117 std::vector<std::string> search_path;
00118 rp.getSearchPathFromEnv(search_path);
00119 rp.crawl(search_path, 1);
00120 std::string path;
00121 if (rp.find("jsk_perception",path)==true) default_template_file_name = path + std::string("/sample/opencv-logo2.png");
00122 #endif
00123 } catch (std::runtime_error &e) {}
00124 local_nh.param("template_filename", template_filename, default_template_file_name);
00125 local_nh.param("standard_width", standard_width, 12);
00126 local_nh.param("standard_height", standard_height, 12);
00127 local_nh.param("best_window_estimation_method", best_window_estimation_method, 1);
00128 local_nh.param("coefficient_threshold", coefficient_thre, 0.5);
00129 local_nh.param("coefficient_threshold_for_best_window", coefficient_thre_for_best_window, 0.67);
00130 local_nh.param("sliding_factor", sliding_factor, 1.0);
00131 local_nh.param("show_result", show_result_, true);
00132 local_nh.param("pub_debug_image", pub_debug_image_, true);
00133 local_nh.param("object_width", template_width, 0.06);
00134 local_nh.param("object_height", template_height, 0.0739);
00135
00136 cv::Mat template_image=cv::imread(template_filename);
00137 if(template_add(template_image))template_images.push_back(template_image);
00138 }
00139 bool template_add(cv::Mat template_image){
00140 if(template_image.cols==0){
00141 ROS_INFO("template_error size==0");
00142 return false;
00143 }
00144 cv::Mat template_hsv_image;
00145 cv::cvtColor(template_image, template_hsv_image, CV_BGR2HSV);
00146 std::vector<unsigned int> template_vec(256);
00147 for (int k=0; k<256; ++k){
00148 template_vec[k]=0;
00149 }
00150 for (int i = 0; i<template_image.rows; ++i){
00151 for (int j = 0; j<template_image.cols; ++j) {
00152 uchar h=template_hsv_image.data[i*template_hsv_image.step+j*3+0];
00153 uchar s=template_hsv_image.data[i*template_hsv_image.step+j*3+1];
00154 int index_h=0;
00155 int index_s=0;
00156 while(h>15)
00157 {
00158 h-=16;++index_h;
00159 }
00160 while(s>15)
00161 {
00162 s-=16;++index_s;
00163 }
00164 ++template_vec[index_h*16 + index_s];
00165 }
00166 }
00167 template_vecs.push_back(template_vec);
00168 ROS_INFO("template vec was set successfully");
00169 return true;
00170 }
00171
00172 void image_cb(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_ptr)
00173 {
00174 boost::mutex::scoped_lock lock(_mutex);
00175 if(template_vecs.size()==0){
00176 ROS_INFO("template vec is empty");
00177 return;
00178 }
00179 cv_bridge::CvImagePtr cv_ptr;
00180 try{
00181 cv_ptr = cv_bridge::toCvCopy(msg_ptr, "bgr8");
00182 }
00183 catch (cv_bridge::Exception& e){
00184 ROS_ERROR("cv_bridge exception: %s", e.what());
00185 return;
00186 }
00187 cv::Mat image=cv_ptr->image.clone();
00188 ROS_INFO("mat made");
00189 if(hsv_integral.size()==0||(image.cols!=hsv_integral.size() && image.rows!=hsv_integral[0].size())){
00190 ROS_INFO("before memory size was changed");
00191 hsv_integral.resize(image.cols);
00192 for(int i=0; i<image.cols; ++i){
00193 hsv_integral[i].resize(image.rows);
00194 for(int j=0; j<image.rows; ++j){
00195 hsv_integral[i][j].resize(255);
00196 }
00197 }
00198 ROS_INFO("memory size was changed");
00199 }
00200 cv::Mat hsv_image;
00201 cv::cvtColor(image, hsv_image, CV_BGR2HSV);
00202 unsigned char hsv_mat[image.cols][image.rows];
00203 for (size_t i = 0, image_rows=image.rows; i<image_rows; ++i){
00204 for (size_t j = 0, image_cols=image.cols; j<image_cols; ++j) {
00205 uchar h=hsv_image.data[i*hsv_image.step+j*3+0];
00206 uchar s=hsv_image.data[i*hsv_image.step+j*3+1];
00207 int index_h=0;
00208 int index_s=0;
00209 while(h>15)
00210 {
00211 h-=16;++index_h;
00212 }
00213 while(s>15)
00214 {
00215 s-=16;++index_s;
00216 }
00217 hsv_mat[j][i] = index_h*16+index_s;
00218 }
00219 }
00220
00221 #ifdef _OPENMP
00222 #pragma omp parallel for
00223 #endif
00224 for (size_t k =0; k<16*16; ++k){
00225 if (k == hsv_mat[0][0]){
00226 hsv_integral[0][0][k]=1;
00227 }
00228 else{
00229 hsv_integral[0][0][k]=0;
00230 }
00231 for (size_t j=1, image_cols=image.cols; j<image_cols; ++j){
00232 if(k == hsv_mat[j][0]){
00233 hsv_integral[j][0][k] = hsv_integral[j-1][0][k]+1;
00234 }
00235 else{
00236 hsv_integral[j][0][k] = hsv_integral[j-1][0][k];
00237 }
00238 }
00239 for (int i=1,image_rows=image.rows; i<image_rows; ++i){
00240 unsigned int s=0;
00241 for (int j=0, image_cols=image.cols; j<image_cols; ++j){
00242 if(k == hsv_mat[j][i]){
00243 ++s;
00244 }
00245 hsv_integral[j][i][k] = hsv_integral[j][i-1][k]+ s;
00246 }
00247 }
00248 }
00249 ROS_INFO("integral histogram made");
00250 for(size_t template_index=0; template_index<template_vecs.size(); ++template_index){
00251 std::vector<unsigned int> template_vec = template_vecs[template_index];
00252 double max_coe = 0;
00253 int index_i=-1, index_j=-1 ,index_width_d, index_height_d;
00254 float max_scale=-1;
00255
00256 std::vector<box> boxes;
00257 for(float scale=image.cols/32; scale > 1; scale/=1.2){
00258 int dot = (int)(scale*2*sliding_factor);
00259 if(dot<1) dot = 1;
00260 int width_d = (int)(scale*standard_width);
00261 int height_d = (int)(scale*standard_height);
00262 for(size_t i=0, image_rows=image.rows; i+height_d < image_rows; i+=dot){
00263 for(size_t j=0, image_cols=image.cols; j+width_d < image_cols; j+=dot){
00264
00265 std::vector <unsigned int> vec(256);
00266 for(size_t k=0; k<256; ++k){
00267 vec[k] = hsv_integral[j+width_d][i+height_d][k]
00268 - hsv_integral[j][i+height_d][k]
00269 - hsv_integral[j+width_d][i][k]
00270 + hsv_integral[j][i][k];
00271 }
00272 double temp_coe = calc_coe(template_vec, vec);
00273
00274 if(temp_coe > coefficient_thre){
00275 boxes.push_back(box(j, i, width_d, height_d, temp_coe));
00276
00277 if (temp_coe > max_coe){
00278 max_coe = temp_coe;
00279 index_i=i; index_j=j; max_scale = scale;
00280 index_width_d=width_d; index_height_d=height_d;
00281 }
00282 }
00283 }
00284 }
00285 }
00286 ROS_INFO("max_coefficient:%f", max_coe);
00287 if(boxes.size() == 0 || max_coe < coefficient_thre_for_best_window){
00288 ROS_INFO("no objects found");
00289 if(show_result_){
00290 cv::imshow("result", image);
00291 cv::imshow("template", template_images[template_index]);
00292 cv::waitKey(20);
00293 }
00294 if(pub_debug_image_){
00295 cv_bridge::CvImage out_msg;
00296 out_msg.header = msg_ptr->header;
00297 out_msg.encoding = "bgr8";
00298 out_msg.image = image;
00299 _debug_pub.publish(out_msg.toImageMsg());
00300 }
00301 return;
00302 }
00303 if(best_window_estimation_method==0){
00304 }
00305
00306 if(best_window_estimation_method==1){
00307 box best_large_box(index_j, index_i, index_width_d, index_height_d, 0);
00308 max_coe = 0;
00309 int temp_width = 0;
00310 bool large_found=false;
00311 for(size_t i=0; i<boxes.size(); ++i){
00312 if(temp_width!=boxes[i].dx){
00313 if(large_found) break;
00314 temp_width = boxes[i].dx;
00315 }
00316 if(in_box(index_j, index_i, index_width_d, index_height_d, boxes[i])){
00317 large_found=true;
00318 if(max_coe < boxes[i].coefficient){
00319 best_large_box = boxes[i];
00320 max_coe = boxes[i].coefficient;
00321 }
00322 }
00323 }
00324 index_j = best_large_box.x; index_i = best_large_box.y;
00325 max_scale = best_large_box.dx/standard_width;
00326 }
00327 if(best_window_estimation_method==2){
00328 max_coe = 0;
00329 std::vector<box> boxes_temp;
00330 for(size_t i=0; i<boxes.size(); ++i){
00331 box box_temp = boxes[i];
00332 if(in_boxes(box_temp.x, box_temp.y, box_temp.dx, box_temp.dy, boxes_temp)){
00333 continue;
00334 }
00335 boxes_temp.push_back(box_temp);
00336 if(box_temp.coefficient > max_coe){
00337 max_coe = box_temp.coefficient;
00338 index_j = box_temp.x, index_j=box_temp.y;
00339 max_scale = box_temp.dx/standard_width;
00340 }
00341 }
00342 }
00343
00344
00345 cv::rectangle(image, cv::Point(index_j, index_i), cv::Point(index_j+(max_scale*standard_width), index_i+(max_scale*standard_height)), cv::Scalar(0, 0, 200), 3, 4);
00346 jsk_recognition_msgs::Rect rect;
00347 rect.x=index_j, rect.y=index_i, rect.width=(int)(max_scale*standard_width); rect.height=(int)(max_scale*standard_height);
00348 _pubBestRect.publish(rect);
00349 geometry_msgs::PolygonStamped polygon_msg;
00350 polygon_msg.header = msg_ptr->header;
00351 polygon_msg.polygon.points.resize(2);
00352 polygon_msg.polygon.points[0].x=index_j;
00353 polygon_msg.polygon.points[0].y=index_i;
00354 polygon_msg.polygon.points[1].x=(int)(max_scale*standard_width)+index_j;
00355 polygon_msg.polygon.points[1].y=(int)(max_scale*standard_height)+index_i;
00356 _pubBestPolygon.publish(polygon_msg);
00357
00358 image_geometry::PinholeCameraModel pcam;
00359 pcam.fromCameraInfo(*info_ptr);
00360 cv::Point2f corners2d[4] = {cv::Point2f(index_j, index_i),
00361 cv::Point2f((int)(max_scale*standard_width)+index_j, index_i),
00362 cv::Point2f((int)(max_scale*standard_width)+index_j, (int)(max_scale*standard_height)+index_i),
00363 cv::Point2f(index_j, (int)(max_scale*standard_height)+index_i)
00364 };
00365 cv::Mat corners2d_mat(cv::Size(4, 1), CV_32FC2, corners2d);
00366 cv::Point3f corners3d[4] = {cv::Point3f(-template_height/2,-template_width/2,0),
00367 cv::Point3f(-template_height/2,template_width/2,0),
00368 cv::Point3f(template_height/2,template_width/2,0),
00369 cv::Point3f(template_height/2,-template_width/2,0)
00370 };
00371 cv::Mat corners3d_mat (cv::Size(4, 1), CV_32FC3, corners3d);
00372 double fR3[3], fT3[3];
00373 cv::Mat rvec(3, 1, CV_64FC1, fR3);
00374 cv::Mat tvec(3, 1, CV_64FC1, fT3);
00375 cv::Mat zero_distortion_mat = cv::Mat::zeros(4, 1, CV_64FC1);
00376
00377 cv::solvePnP (corners3d_mat, corners2d_mat,
00378 pcam.intrinsicMatrix(),
00379 zero_distortion_mat,
00380 rvec, tvec);
00381 geometry_msgs::PointStamped best_point;
00382 best_point.point.x = fT3[0];
00383 best_point.point.y = fT3[1];
00384 best_point.point.z = fT3[2];
00385 best_point.header = msg_ptr->header;
00386 _pubBestPoint.publish(best_point);
00387 jsk_recognition_msgs::BoundingBoxArray box_array_msg;
00388 jsk_recognition_msgs::BoundingBox box_msg;
00389 box_array_msg.header = box_msg.header = msg_ptr->header;
00390 box_msg.pose.position = best_point.point;
00391 box_msg.pose.orientation.x = box_msg.pose.orientation.y = box_msg.pose.orientation.z = 0;
00392 box_msg.pose.orientation.w = 1;
00393 box_msg.dimensions.x = box_msg.dimensions.z = template_width;
00394 box_msg.dimensions.y = template_height;
00395 box_array_msg.boxes.push_back(box_msg);
00396 _pubBestBoundingBox.publish(box_array_msg);
00397
00398 if(show_result_){
00399 cv::imshow("result", image);
00400 cv::imshow("template", template_images[template_index]);
00401 cv::waitKey(20);
00402 }
00403 if(pub_debug_image_){
00404 cv_bridge::CvImage out_msg;
00405 out_msg.header = msg_ptr->header;
00406 out_msg.encoding = "bgr8";
00407 out_msg.image = image;
00408 _debug_pub.publish(out_msg.toImageMsg());
00409 }
00410 }
00411 }
00412 void dyn_conf_callback(jsk_perception::ColorHistogramSlidingMatcherConfig &config, uint32_t level){
00413 boost::mutex::scoped_lock lock(_mutex);
00414 standard_height = config.standard_height;
00415 standard_width = config.standard_width;
00416 coefficient_thre = config.coefficient_threshold;
00417 sliding_factor = config.sliding_factor;
00418 coefficient_thre_for_best_window = config.coefficient_threshold_for_best_window;
00419 best_window_estimation_method = config.best_window_estimation_method;
00420 if(level==1){
00421 }
00422 }
00423 };
00424
00425 inline double calc_coe(unsigned int * a, unsigned int* b){
00426 unsigned long sum_a=0, sum_b=0;
00427 double coe=0;
00428 for(int k=0; k<256; ++k){
00429 sum_a += a[k]; sum_b+= b[k];
00430 }
00431 for(int k=0; k<256; ++k){
00432 coe +=std::min((double)a[k]/sum_a, (double)b[k]/sum_b);
00433 }
00434 return coe;
00435 }
00436
00437
00438 int main(int argc, char **argv){
00439 ros::init (argc, argv, "ColorHistogramSlidingMatcher");
00440
00441 MatcherNode matcher;
00442
00443 dynamic_reconfigure::Server<jsk_perception::ColorHistogramSlidingMatcherConfig> server;
00444 dynamic_reconfigure::Server<jsk_perception::ColorHistogramSlidingMatcherConfig>::CallbackType f;
00445 f = boost::bind(&MatcherNode::dyn_conf_callback, &matcher, _1, _2);
00446 server.setCallback(f);
00447 ros::spin();
00448 return 0;
00449 }