point_pose_extractor.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <rospack/rospack.h>
00003 #include <opencv/highgui.h>
00004 #include <cv_bridge/cv_bridge.h>
00005 #pragma message "Compiling " __FILE__ "..."
00006 #include <posedetection_msgs/Feature0DDetect.h>
00007 #include <posedetection_msgs/ImageFeature0D.h>
00008 #include <posedetection_msgs/ObjectDetection.h>
00009 #include <posedetection_msgs/Object6DPose.h>
00010 #include <geometry_msgs/Pose.h>
00011 #include <geometry_msgs/Quaternion.h>
00012 #include <sensor_msgs/image_encodings.h>
00013 #include <image_transport/image_transport.h>
00014 #include <image_geometry/pinhole_camera_model.h>
00015 #include <tf/tf.h>
00016 #include <boost/shared_ptr.hpp>
00017 #include <boost/foreach.hpp>
00018 #include <boost/filesystem.hpp>
00019 #include <vector>
00020 #include <sstream>
00021 #include <iostream>
00022 #include <dynamic_reconfigure/server.h>
00023 #include <jsk_perception/point_pose_extractorConfig.h>
00024 #include <jsk_perception/SetTemplate.h>
00025 
00026 namespace enc = sensor_msgs::image_encodings;
00027 
00028 bool _first_sample_change;
00029 
00030 void features2keypoint (posedetection_msgs::Feature0D features,
00031                         std::vector<cv::KeyPoint>& keypoints,
00032                         cv::Mat& descriptors){
00033   keypoints.resize(features.scales.size());
00034   descriptors.create(features.scales.size(),features.descriptor_dim,CV_32FC1);
00035   std::vector<cv::KeyPoint>::iterator keypoint_it = keypoints.begin();
00036   for ( int i=0; keypoint_it != keypoints.end(); ++keypoint_it, ++i ) {
00037     *keypoint_it = cv::KeyPoint(cv::Point2f(features.positions[i*2+0],
00038                                             features.positions[i*2+1]),
00039                                 features.descriptor_dim, // size
00040                                 features.orientations[i], //angle
00041                                 0, // resonse
00042                                 features.scales[i] // octave
00043                                 );
00044     for (int j = 0; j < features.descriptor_dim; j++){
00045       descriptors.at<float>(i,j) =
00046         features.descriptors[i*features.descriptor_dim+j];
00047     }
00048   }
00049 }
00050 
00051 
00052 class Matching_Template
00053 {
00054 public:
00055   std::string _matching_frame;
00056   cv::Mat _template_img;
00057   std::vector<cv::KeyPoint> _template_keypoints;
00058   cv::Mat _template_descriptors;
00059   int _original_width_size;
00060   int _original_height_size;
00061   double _template_width;       // width of template [m]
00062   double _template_height;      // height of template [m]
00063   tf::Transform _relativepose;
00064   cv::Mat _affine_matrix;
00065   std::string _window_name;
00066   // The maximum allowed reprojection error to treat a point pair as an inlier
00067   double _reprojection_threshold;
00068   // threshold on squared ratio of distances between NN and 2nd NN
00069   double _distanceratio_threshold;
00070   std::vector<cv::Point2d> _correspondances;
00071   cv::Mat _previous_stack_img;
00072 
00073   Matching_Template(){
00074   }
00075   Matching_Template(cv::Mat img,
00076                     std::string matching_frame,
00077                     int original_width_size,
00078                     int original_height_size,
00079                     double template_width,
00080                     double template_height,
00081                     tf::Transform relativepose,
00082                     cv::Mat affine_matrix,
00083                     double reprojection_threshold,
00084                     double distanceratio_threshold,
00085                     std::string window_name,
00086                     bool autosize){
00087 
00088     _template_img = img.clone();
00089     _matching_frame = matching_frame;
00090     _original_width_size = original_width_size;
00091     _original_height_size = original_height_size;
00092     _template_width = template_width;
00093     _template_height = template_height;
00094     _relativepose = relativepose;
00095     _affine_matrix = affine_matrix;
00096     _window_name = window_name;
00097     _reprojection_threshold = reprojection_threshold;
00098     _distanceratio_threshold = distanceratio_threshold;
00099     _template_keypoints.clear();
00100     _correspondances.clear();
00101     //    cv::namedWindow(_window_name, autosize ? CV_WINDOW_AUTOSIZE : 0);
00102   }
00103 
00104 
00105   virtual ~Matching_Template(){
00106     std::cerr << "delete " << _window_name << std::endl;
00107   }
00108 
00109 
00110   std::string get_window_name(){
00111     return _window_name;
00112   }
00113 
00114   std::vector<cv::Point2d>* correspondances(){
00115     return &_correspondances;
00116   }
00117 
00118   bool check_template (){
00119     if (_template_img.empty() && _template_keypoints.size() == 0 &&
00120         _template_descriptors.empty()){
00121       return false;
00122     }
00123     else {
00124       return true;
00125     }
00126   }
00127 
00128 
00129   int set_template(ros::ServiceClient client){
00130     posedetection_msgs::Feature0DDetect srv;
00131 
00132     if ( _template_img.empty()) {
00133       ROS_ERROR ("template picture is empty.");
00134       return -1;
00135     }
00136     ROS_INFO_STREAM("read template image and call " << client.getService() << " service");
00137     cv_bridge::CvImage cv_img;
00138     cv_img.image = cv::Mat(_template_img);
00139     cv_img.encoding = std::string("bgr8");
00140     srv.request.image = *(cv_img.toImageMsg());
00141     if (client.call(srv)) {
00142       ROS_INFO_STREAM("get features with " << srv.response.features.scales.size() << " descriptoins");
00143       features2keypoint (srv.response.features, _template_keypoints, _template_descriptors);
00144 
00145       // inverse affine translation
00146       cv::Mat M_inv;
00147       cv::Mat dst;
00148       cv::invert(_affine_matrix, M_inv);
00149       //      cv::drawKeypoints (tmp, _template_keypoints, dst, cv::Scalar::all(1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
00150       //      cv::drawKeypoints (tmp, _template_keypoints, dst);
00151       // cv::warpPerspective(_template_img, dst, M_inv,
00152       //                          cv::Size(_original_width_size, _original_height_size),
00153       //                          CV_INTER_LINEAR, IPL_BORDER_CONSTANT, 0);
00154       for (int i = 0; i < (int)_template_keypoints.size(); i++){
00155         cv::Point2f pt;
00156         cv::Mat pt_mat(cv::Size(1,1), CV_32FC2, &pt);
00157         cv::Mat pt_src_mat(cv::Size(1,1), CV_32FC2, &_template_keypoints.at(i).pt);
00158         cv::perspectiveTransform (pt_src_mat, pt_mat, M_inv);
00159         _template_keypoints.at(i).pt = pt_mat.at<cv::Point2f>(0,0);
00160       }
00161       //      cvSetMouseCallback (_window_name.c_str(), &PointPoseExtractor::cvmousecb, this);
00162       //      _template_img = dst;
00163       return 0;
00164     } else {
00165       ROS_ERROR("Failed to call service Feature0DDetect");
00166       return 1;
00167     }
00168   }
00169 
00170 
00171   double log_fac( int n )
00172   {
00173     static std::vector<double> slog_table;
00174     int osize = slog_table.size();
00175     if(osize <= n){
00176       slog_table.resize(n+1);
00177       if(osize == 0){
00178         slog_table[0] = -1;
00179         slog_table[1] = log(1.0);
00180         osize = 2;
00181       }
00182       for(int i = osize; i <= n; i++ ){
00183         slog_table[i] = slog_table[i-1] + log(i);
00184       }
00185     }
00186     return slog_table[n];
00187   }
00188 
00189   int min_inlier( int n, int m, double p_badsupp, double p_badxform )
00190   {
00191     double pi, sum;
00192     int i, j;
00193     double lp_badsupp = log( p_badsupp );
00194     double lp_badsupp1 = log( 1.0 - p_badsupp );
00195     for( j = m+1; j <= n; j++ ){
00196       sum = 0;
00197       for( i = j; i <= n; i++ ){
00198         pi = (i-m) * lp_badsupp + (n-i+m) * lp_badsupp1 +
00199           log_fac( n - m ) - log_fac( i - m ) - log_fac( n - i );
00200         sum += exp( pi );
00201       }
00202       if( sum < p_badxform )
00203         break;
00204     }
00205     return j;
00206   }
00207 
00208 
00209   bool estimate_od (ros::ServiceClient client, cv::Mat src_img,
00210                     std::vector<cv::KeyPoint> sourceimg_keypoints,
00211                     image_geometry::PinholeCameraModel pcam,
00212                     double err_thr, cv::Mat &stack_img,
00213                     cv::flann::Index* ft, posedetection_msgs::Object6DPose* o6p){
00214 
00215     if ( _template_keypoints.size()== 0 &&
00216          _template_descriptors.empty() ){
00217       set_template(client);
00218     }
00219     if ( _template_keypoints.size()== 0 &&
00220          _template_descriptors.empty() ) {
00221       ROS_ERROR ("Template image was not set.");
00222       return false;
00223     }
00224 
00225     // stacked image
00226     cv::Size stack_size  = cv::Size(MAX(src_img.cols,_template_img.cols),
00227                                     src_img.rows+_template_img.rows);
00228     stack_img = cv::Mat(stack_size,CV_8UC3);
00229     stack_img = cv::Scalar(0);
00230     cv::Mat stack_img_tmp(stack_img,cv::Rect(0,0,_template_img.cols,_template_img.rows));
00231     cv::add(stack_img_tmp, _template_img, stack_img_tmp);
00232     cv::Mat stack_img_src(stack_img,cv::Rect(0,_template_img.rows,src_img.cols,src_img.rows));
00233     cv::add(stack_img_src, src_img, stack_img_src);
00234     _previous_stack_img = stack_img.clone();
00235 
00236     // matching
00237     cv::Mat m_indices(_template_descriptors.rows, 2, CV_32S);
00238     cv::Mat m_dists(_template_descriptors.rows, 2, CV_32F);
00239     ft->knnSearch(_template_descriptors, m_indices, m_dists, 2, cv::flann::SearchParams(-1) );
00240 
00241     // matched points
00242     std::vector<cv::Point2f> pt1, pt2;
00243     std::vector<int> queryIdxs,trainIdxs;
00244     for ( unsigned int j = 0; j < _template_keypoints.size(); j++ ) {
00245       if ( m_dists.at<float>(j,0) < m_dists.at<float>(j,1) * _distanceratio_threshold) {
00246         queryIdxs.push_back(j);
00247         trainIdxs.push_back(m_indices.at<int>(j,0));
00248       }
00249     }
00250     if ( queryIdxs.size() == 0 ) {
00251       ROS_WARN_STREAM("could not found matched points with distanceratio(" <<_distanceratio_threshold << ")");
00252     } else {
00253       cv::KeyPoint::convert(_template_keypoints,pt1,queryIdxs);
00254       cv::KeyPoint::convert(sourceimg_keypoints,pt2,trainIdxs);
00255     }
00256 
00257     ROS_INFO ("Found %d total matches among %d template keypoints", (int)pt2.size(), (int)_template_keypoints.size());
00258 
00259     cv::Mat H;
00260     std::vector<uchar> mask((int)pt2.size());
00261 
00262     if ( pt1.size() > 4 ) {
00263           // ToDO for curve face
00264       H = cv::findHomography(cv::Mat(pt1), cv::Mat(pt2), mask, CV_RANSAC, _reprojection_threshold);
00265     }
00266 
00267     // draw line
00268     for (int j = 0; j < (int)pt1.size(); j++){
00269       cv::Point2f pt, pt_orig;
00270       cv::Mat pt_mat(cv::Size(1,1), CV_32FC2, &pt);
00271       cv::Mat pt_src_mat(cv::Size(1,1), CV_32FC2, &pt1.at(j));
00272       cv::perspectiveTransform (pt_src_mat, pt_mat, _affine_matrix);
00273       pt_orig = pt_mat.at<cv::Point2f>(0,0);
00274       if ( mask.at(j)){
00275         cv::line(stack_img, pt_orig, pt2.at(j)+cv::Point2f(0,_template_img.rows),
00276                  CV_RGB(0,255,0), 1,8,0);
00277       }
00278       else {
00279         cv::line(stack_img, pt_orig, pt2.at(j)+cv::Point2f(0,_template_img.rows),
00280                  CV_RGB(255,0,255), 1,8,0);
00281       }
00282     }
00283     int inlier_sum = 0;
00284     for (int k=0; k < (int)mask.size(); k++){
00285       inlier_sum += (int)mask.at(k);
00286     }
00287 
00288     double text_scale = 1.5;
00289     {
00290       int fontFace = 0, thickness = 0, baseLine;
00291       int x, y;
00292       cv::Size text_size;
00293       std::string text;
00294 
00295       text = "inlier: " + boost::lexical_cast<std::string>((int)inlier_sum) + " / " + boost::lexical_cast<std::string>((int)pt2.size());
00296       text_size = cv::getTextSize(text, fontFace, text_scale, thickness, &baseLine);
00297       x = stack_img.size().width - text_size.width;
00298       y = text_size.height + thickness + 10; // 10pt pading
00299       cv::putText (stack_img, text, cv::Point(x, y),
00300                    fontFace, text_scale, CV_RGB(0, 255, 0),
00301                    thickness, 8, false);
00302 
00303       text = "template: " + boost::lexical_cast<std::string>((int)_template_keypoints.size());
00304       text_size = cv::getTextSize(text, fontFace, text_scale, thickness, &baseLine);
00305       x = stack_img.size().width - text_size.width;
00306       y += text_size.height + thickness + 10; // 10pt pading
00307       cv::putText (stack_img, text, cv::Point(x, y),
00308                    fontFace, text_scale, CV_RGB(0, 255, 0),
00309                    thickness, 8, false);
00310     }
00311 
00312     // draw correspondances
00313     ROS_INFO("_correspondances.size: %d", (int)_correspondances.size());
00314     for (int j = 0; j < (int)_correspondances.size(); j++){
00315       cv::circle(stack_img, cv::Point2f(_correspondances.at(j).x, _correspondances.at(j).y + _template_img.size().height),
00316                  8, CV_RGB(255,0,0), -1);
00317     }
00318 
00319     if ((cv::countNonZero( H ) == 0) || (inlier_sum < min_inlier((int)pt2.size(), 4, 0.10, 0.01))){
00320       if( _window_name != "" )
00321         cv::imshow(_window_name, stack_img);
00322       return false;
00323     }
00324 
00325     std::string _type;
00326     char chr[20];
00327 
00328     // number of match points
00329     sprintf(chr, "%d", (int)pt2.size());
00330     _type = _matching_frame + "_" + std::string(chr);
00331 
00332     sprintf(chr, "%d", inlier_sum);
00333     _type = _type + "_" + std::string(chr);
00334 
00335     cv::Point2f corners2d[4] = {cv::Point2f(0,0),
00336                                 cv::Point2f(_original_width_size,0),
00337                                 cv::Point2f(_original_width_size,_original_height_size),
00338                                 cv::Point2f(0,_original_height_size)};
00339     cv::Mat corners2d_mat (cv::Size(4, 1), CV_32FC2, corners2d);
00340     cv::Point3f corners3d[4] = {cv::Point3f(0,0,0),
00341                                 cv::Point3f(0,_template_width,0),
00342                                 cv::Point3f(_template_height,_template_width,0),
00343                                 cv::Point3f(_template_height,0,0)};
00344     cv::Mat corners3d_mat (cv::Size(4, 1), CV_32FC3, corners3d);
00345 
00346     cv::Mat corners2d_mat_trans;
00347 
00348     cv::perspectiveTransform (corners2d_mat, corners2d_mat_trans, H);
00349 
00350     double fR3[3], fT3[3];
00351     cv::Mat rvec(3, 1, CV_64FC1, fR3);
00352     cv::Mat tvec(3, 1, CV_64FC1, fT3);
00353     cv::Mat zero_distortion_mat = cv::Mat::zeros(4, 1, CV_64FC1);
00354 
00355     cv::Mat camera_matrix = pcam.projectionMatrix()(cv::Range::all(), cv::Range(0, 3));
00356     cv::solvePnP (corners3d_mat, corners2d_mat_trans, camera_matrix,
00357                   zero_distortion_mat,//if unrectified: pcam.distortionCoeffs()
00358                   rvec, tvec);
00359 
00360     tf::Transform checktf, resulttf;
00361 
00362     checktf.setOrigin( tf::Vector3(fT3[0], fT3[1], fT3[2] ) );
00363 
00364     double rx = fR3[0], ry = fR3[1], rz = fR3[2];
00365     tf::Quaternion quat;
00366     double angle = cv::norm(rvec);
00367     quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle);
00368     checktf.setRotation( quat );
00369 
00370     resulttf = checktf * _relativepose;
00371 
00372     ROS_INFO( "tx: (%0.2lf,%0.2lf,%0.2lf) rx: (%0.2lf,%0.2lf,%0.2lf)",
00373               resulttf.getOrigin().getX(),
00374               resulttf.getOrigin().getY(),
00375               resulttf.getOrigin().getZ(),
00376               resulttf.getRotation().getAxis().x() * resulttf.getRotation().getAngle(),
00377               resulttf.getRotation().getAxis().y() * resulttf.getRotation().getAngle(),
00378               resulttf.getRotation().getAxis().z() * resulttf.getRotation().getAngle());
00379 
00380     o6p->pose.position.x = resulttf.getOrigin().getX();
00381     o6p->pose.position.y = resulttf.getOrigin().getY();
00382     o6p->pose.position.z = resulttf.getOrigin().getZ();
00383     o6p->pose.orientation.w = resulttf.getRotation().w();
00384     o6p->pose.orientation.x = resulttf.getRotation().x();
00385     o6p->pose.orientation.y = resulttf.getRotation().y();
00386     o6p->pose.orientation.z = resulttf.getRotation().z();
00387     o6p->type = _matching_frame; // _type
00388 
00389     // draw 3d cube model
00390     std::vector<cv::Point2f> projected_top;
00391     {
00392       tf::Vector3 coords[8] = {tf::Vector3(0,0,0),
00393                                tf::Vector3(0, _template_width, 0),
00394                                tf::Vector3(_template_height, _template_width,0),
00395                                tf::Vector3(_template_height, 0, 0),
00396                                tf::Vector3(0, 0, -0.03),
00397                                tf::Vector3(0, _template_width, -0.03),
00398                                tf::Vector3(_template_height, _template_width, -0.03),
00399                                tf::Vector3(_template_height, 0, -0.03)};
00400 
00401       projected_top = std::vector<cv::Point2f>(8);
00402 
00403       for(int i=0; i<8; i++) {
00404         coords[i] = checktf * coords[i];
00405         cv::Point3f pt(coords[i].getX(), coords[i].getY(), coords[i].getZ());
00406         projected_top[i] = pcam.project3dToPixel(pt);
00407       }
00408     }
00409 
00410     { // check if the matched region does not too big or too small
00411       float max_x, max_y, min_x, min_y;
00412       max_x = max_y = -1e9;
00413       min_x = min_y = 1e9;
00414       for (int j = 0; j < 4; j++){
00415         cv::Point2f pt = corners2d_mat_trans.at<cv::Point2f>(0,j);
00416         max_x = std::max(max_x, pt.x), max_y = std::max(max_y, pt.y);
00417         min_x = std::min(min_x, pt.x), min_y = std::min(min_y, pt.y);
00418       }
00419       if((max_x - min_x) < 30 || (max_y - min_y) < 30 ||
00420          src_img.rows < (max_x - min_x)/2 || src_img.cols < (max_y - min_y)/2){
00421         return false;
00422       }
00423     }
00424 
00425     double err_sum = 0;
00426     bool err_success = true;
00427     for (int j = 0; j < 4; j++){
00428       double err = sqrt(pow((corners2d_mat_trans.at<cv::Point2f>(0,j).x - projected_top.at(j).x), 2) +
00429                       pow((corners2d_mat_trans.at<cv::Point2f>(0,j).y - projected_top.at(j).y), 2));
00430       err_sum += err;
00431     }
00432     if (err_sum > err_thr){
00433       err_success = false;
00434     }
00435 
00436     // draw lines around the detected object
00437     for (int j = 0; j < corners2d_mat_trans.cols; j++){
00438       cv::Point2f p1(corners2d_mat_trans.at<cv::Point2f>(0,j).x,
00439                      corners2d_mat_trans.at<cv::Point2f>(0,j).y+_template_img.rows);
00440       cv::Point2f p2(corners2d_mat_trans.at<cv::Point2f>(0,(j+1)%corners2d_mat_trans.cols).x,
00441                      corners2d_mat_trans.at<cv::Point2f>(0,(j+1)%corners2d_mat_trans.cols).y+_template_img.rows);
00442       cv::line (stack_img, p1, p2, CV_RGB(255, 0, 0),
00443                 (err_success?4:1), // width
00444                 CV_AA, 0);
00445     }
00446 
00447     // draw 3d cube model
00448     if(projected_top.size() == 8) { // verify, the size is 8
00449       int cnt = 8;
00450       std::vector<cv::Point2f> ps(cnt);
00451       for(int i=0; i<cnt; i++)
00452         ps[i] = cv::Point2f(projected_top[i].x,
00453                             projected_top[i].y+_template_img.rows);
00454 
00455       int draw_width = ( err_success ? 3 : 1);
00456       for(int i=0; i<4; i++) {
00457         cv::line (stack_img, ps[i], ps[(i+1)%4], CV_RGB(0, 0, 255), draw_width, CV_AA, 0);
00458         cv::line (stack_img, ps[i+4], ps[(i+1)%4+4], CV_RGB(0, 0, 255), draw_width, CV_AA, 0);
00459         cv::line (stack_img, ps[i], ps[i+4], CV_RGB(0, 0, 255), draw_width, CV_AA, 0);
00460       }
00461     }
00462 
00463     // draw coords
00464     if ( err_success )
00465     {
00466       tf::Vector3 coords[4] = { tf::Vector3(0,0,0),
00467                                 tf::Vector3(0.05,0,0),
00468                                 tf::Vector3(0,0.05,0),
00469                                 tf::Vector3(0,0,0.05)};
00470       std::vector<cv::Point2f> ps(4);
00471 
00472       for(int i=0; i<4; i++) {
00473         coords[i] = resulttf * coords[i];
00474         cv::Point3f pt(coords[i].getX(), coords[i].getY(), coords[i].getZ());   
00475         ps[i] = pcam.project3dToPixel(pt);
00476         ps[i].y += _template_img.rows; // draw on camera image
00477       }
00478 
00479       cv::line (stack_img, ps[0], ps[1], CV_RGB(255, 0, 0), 3, CV_AA, 0);
00480       cv::line (stack_img, ps[0], ps[2], CV_RGB(0, 255, 0), 3, CV_AA, 0);
00481       cv::line (stack_img, ps[0], ps[3], CV_RGB(0, 0, 255), 3, CV_AA, 0);
00482     }
00483 
00484     // write text on image
00485     {
00486       std::string text;
00487       int x, y;
00488       text = "error: " + boost::lexical_cast<std::string>(err_sum);
00489       x = stack_img.size().width - 16*17*text_scale; // 16pt * 17
00490       y = _template_img.size().height - (16 + 2)*text_scale*6;
00491       cv::putText (stack_img, text, cv::Point(x, y),
00492                    0, text_scale, CV_RGB(0, 255, 0),
00493                    2, 8, false);
00494 
00495     }
00496     // for debug window
00497     if( _window_name != "" )
00498       cv::imshow(_window_name, stack_img);
00499 
00500     return err_success;
00501   }
00502 };  // the end of difinition of class Matching_Template
00503 
00504 
00505 class PointPoseExtractor
00506 {
00507   ros::NodeHandle _n;
00508   image_transport::ImageTransport it;
00509   ros::Subscriber _sub;
00510   ros::ServiceServer _server;
00511   ros::ServiceClient _client;
00512   ros::Publisher _pub, _pub_agg;
00513   image_transport::Publisher _debug_pub;
00514   double _reprojection_threshold;
00515   double _distanceratio_threshold;
00516   double _th_step;
00517   double _phi_step;
00518   bool _autosize;
00519   double _err_thr;
00520   static std::vector<Matching_Template *> _templates;
00521   image_geometry::PinholeCameraModel pcam;
00522   bool pnod;
00523   bool _initialized;
00524   bool _viewer;
00525 
00526 public:
00527   PointPoseExtractor() : it(ros::NodeHandle("~")) {
00528     // _sub = _n.subscribe("ImageFeature0D", 1,
00529     //                     &PointPoseExtractor::imagefeature_cb, this);
00530     _client = _n.serviceClient<posedetection_msgs::Feature0DDetect>("Feature0DDetect");
00531     _pub = _n.advertise<posedetection_msgs::ObjectDetection>("ObjectDetection", 10);
00532     _pub_agg = _n.advertise<posedetection_msgs::ObjectDetection>("ObjectDetection_agg", 10);
00533     _debug_pub = it.advertise("debug_image", 1);
00534     _server = _n.advertiseService("SetTemplate", &PointPoseExtractor::settemplate_cb, this);
00535     _initialized = false;
00536   }
00537 
00538   virtual ~PointPoseExtractor(){
00539     _sub.shutdown();
00540     _client.shutdown();
00541     _pub.shutdown();
00542     _pub_agg.shutdown();
00543   }
00544 
00545   static void make_template_from_mousecb(Matching_Template *mt){
00546     cv::Mat H;
00547     cv::Mat tmp_template, tmp_warp_template;
00548     std::vector<cv::Point2f>pt1, pt2;
00549     double width, height;
00550     std::string filename;
00551     std::cout << "input template's [width]" << std::endl;
00552     std::cin >> width;
00553     std::cout << "input template's [height]" << std::endl;
00554     std::cin >> height;
00555     std::cout << "input template's [filename]" << std::endl;
00556     std::cin >> filename;
00557 
00558     for (int i = 0; i < 4; i++){
00559       pt1.push_back(cv::Point2d((int)mt->_correspondances.at(i).x,
00560                                 (int)mt->_correspondances.at(i).y + mt->_template_img.size().height));
00561     }
00562     cv::Rect rect = cv::boundingRect(cv::Mat(pt1));
00563     double scale = std::max(width, height) / 500.0;
00564     int iwidth = width / scale, iheight = height / scale;
00565     pt2.push_back(cv::Point2d(0,0));
00566     pt2.push_back(cv::Point2d(iwidth,0));
00567     pt2.push_back(cv::Point2d(iwidth,iheight));
00568     pt2.push_back(cv::Point2d(0,     iheight));
00569     H = cv::findHomography(cv::Mat(pt1), cv::Mat(pt2));
00570 
00571     cv::getRectSubPix(mt->_previous_stack_img, rect.size(),
00572                       cv::Point2f((rect.tl().x + rect.br().x)/2.0,(rect.tl().y + rect.br().y)/2.0),
00573                       tmp_template);
00574     cv::warpPerspective(mt->_previous_stack_img, tmp_warp_template, H, cv::Size(iwidth, iheight));
00575 
00576     try {
00577       cv::imwrite(filename,tmp_template);
00578       boost::filesystem::path fname(filename);
00579       std::stringstream ss;
00580       ss << fname.stem() << "_wrap" << fname.extension();
00581       cv::imwrite(ss.str(),tmp_warp_template);
00582     }catch (cv::Exception e) {
00583       std::cerr << e.what()  << std::endl;
00584     }
00585 
00586     for (int i = 0; i < (int)pt1.size(); i++){
00587       pt2.push_back(cv::Point2d((int)pt1.at(i).x - rect.x,
00588                                 (int)pt1.at(i).y - rect.y - mt->_template_img.size().height));
00589     }
00590     // cv::Mat mask_img = cv::Mat::zeros(tmp_template.size(), CV_8UC3);
00591     // cv::fillConvexPoly(mask_img, pt2.begin(), (int)pt2.size(), CV_RGB(255, 255, 255));
00592 
00593     // cv::namedWindow("hoge", 1);
00594     // cv::imshow("hoge", mask_img);
00595 
00596     cv::Mat M = (cv::Mat_<double>(3,3) << 1,0,0, 0,1,0, 0,0,1);
00597     std::string window_name = "sample" + boost::lexical_cast<std::string>((int)_templates.size());
00598 
00599     Matching_Template* tmplt = 
00600       new Matching_Template (tmp_warp_template, "sample",
00601                              tmp_warp_template.size().width, tmp_warp_template.size().height,
00602                              width, height,
00603                              tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
00604                              M,
00605                              mt->_reprojection_threshold,
00606                              mt->_distanceratio_threshold,
00607                              _first_sample_change ? window_name : mt->_window_name,
00608                              cv::getWindowProperty(mt->_window_name, CV_WND_PROP_AUTOSIZE));
00609 
00610     mt->_correspondances.clear();
00611     _templates.push_back(tmplt);
00612     cv::namedWindow(_first_sample_change ? window_name : mt->_window_name,
00613                     cv::getWindowProperty(mt->_window_name, CV_WND_PROP_AUTOSIZE));
00614     cvSetMouseCallback (_first_sample_change ? window_name.c_str() : mt->_window_name.c_str(),
00615                         &cvmousecb, static_cast<void *>(_templates.back()));
00616     _first_sample_change = true;
00617   }
00618 
00619   static void cvmousecb (int event, int x, int y, int flags, void* param){
00620     Matching_Template *mt = (Matching_Template*)param;
00621     // std::cerr << "mousecb_ -> " << mt << std::endl;
00622     switch (event){
00623     case CV_EVENT_LBUTTONUP: {
00624       cv::Point2d pt(x,y - (int)mt->_template_img.size().height);
00625       ROS_INFO("add correspondence (%d, %d)", (int)pt.x, (int)pt.y);
00626       mt->_correspondances.push_back(pt);
00627       if ((int)mt->_correspondances.size() >= 4){
00628         make_template_from_mousecb(mt);
00629         mt->_correspondances.clear();
00630         ROS_INFO("reset");
00631       }
00632       break;
00633     }
00634     case CV_EVENT_RBUTTONUP: {
00635       mt->_correspondances.clear();
00636       ROS_INFO("reset");
00637       break;
00638     }
00639     }
00640   }
00641 
00642 
00643   void initialize () {
00644     std::string matching_frame;
00645     std::string _pose_str;
00646     double template_width;
00647     double template_height;
00648     std::string template_filename;
00649     std::string window_name;
00650     ros::NodeHandle local_nh("~");
00651 
00652     local_nh.param("child_frame_id", matching_frame, std::string("matching"));
00653     local_nh.param("object_width",  template_width,  0.06);
00654     local_nh.param("object_height", template_height, 0.0739);
00655     local_nh.param("relative_pose", _pose_str, std::string("0 0 0 0 0 0 1"));
00656     std::string default_template_file_name;
00657     try {
00658 #ifdef ROSPACK_EXPORT
00659       rospack::ROSPack rp;
00660       rospack::Package *p = rp.get_pkg("jsk_perception");
00661       if (p!=NULL) default_template_file_name = p->path + std::string("/sample/opencv-logo2.png");
00662 #else
00663       rospack::Rospack rp;
00664       std::vector<std::string> search_path;
00665       rp.getSearchPathFromEnv(search_path);
00666       rp.crawl(search_path, 1);
00667       std::string path;
00668       if (rp.find("jsk_perception",path)==true) default_template_file_name = path + std::string("/sample/opencv-logo2.png");
00669 #endif
00670     } catch (std::runtime_error &e) {
00671     }
00672     local_nh.param("template_filename", template_filename, default_template_file_name);
00673     local_nh.param("reprojection_threshold", _reprojection_threshold, 3.0);
00674     local_nh.param("distanceratio_threshold", _distanceratio_threshold, 0.49);
00675     local_nh.param("error_threshold", _err_thr, 50.0);
00676     local_nh.param("theta_step", _th_step, 5.0);
00677     local_nh.param("phi_step", _phi_step, 5.0);
00678     local_nh.param("viewer_window", _viewer, true);
00679     local_nh.param("window_name", window_name, std::string("sample1"));
00680     local_nh.param("autosize", _autosize, false);
00681     local_nh.param("publish_null_object_detection", pnod, false);
00682 
00683     _first_sample_change = false;
00684 
00685     // make one template
00686     cv::Mat template_img;
00687     template_img = cv::imread (template_filename, 1);
00688     if ( template_img.empty()) {
00689       ROS_ERROR ("template picture <%s> cannot read. template picture is not found or uses unsuported format.", template_filename.c_str());
00690       return;
00691     }
00692 
00693     // relative pose
00694     std::vector<double> rv(7);
00695     std::istringstream iss(_pose_str);
00696     for(int i=0; i<7; i++)
00697       iss >> rv[i];
00698     tf::Transform transform(tf::Quaternion(rv[3],rv[4],rv[5],rv[6]),
00699                             tf::Vector3(rv[0], rv[1], rv[2]));
00700 
00701     // add the image to template list 
00702     add_new_template(template_img, window_name, transform,
00703                      template_width, template_height, _th_step, _phi_step);
00704 
00705   } // initialize
00706 
00707 
00708   cv::Mat make_homography(cv::Mat src, cv::Mat rvec, cv::Mat tvec,
00709                           double template_width, double template_height, cv::Size &size){
00710 
00711     cv::Point3f coner[4] = {cv::Point3f(-(template_width/2.0), -(template_height/2.0),0),
00712                             cv::Point3f(template_width/2.0,-(template_height/2.0),0),
00713                             cv::Point3f(template_width/2.0,template_height/2.0,0),
00714                             cv::Point3f(-(template_width/2.0),template_height/2.0,0)};
00715     cv::Mat coner_mat (cv::Size(4, 1), CV_32FC3, coner);
00716     std::vector<cv::Point2f> coner_img_points;
00717 
00718     cv::Mat zero_distortion_mat = cv::Mat::zeros(4, 1, CV_64FC1);
00719     cv::projectPoints(coner_mat, rvec, tvec,
00720                       pcam.projectionMatrix()(cv::Range::all(), cv::Range(0,3)),
00721                       zero_distortion_mat, // pcam.distortionCoeffs(),
00722                       coner_img_points);
00723 
00724     float x_min = 10000, x_max = 0;
00725     float y_min = 10000, y_max = 0;
00726     for (int i = 0; i < (int)coner_img_points.size(); i++){
00727       x_min = std::min(x_min, coner_img_points.at(i).x);
00728       x_max = std::max(x_max, coner_img_points.at(i).x);
00729       y_min = std::min(y_min, coner_img_points.at(i).y);
00730       y_max = std::max(y_max, coner_img_points.at(i).y);
00731     }
00732 
00733     std::vector<cv::Point2f> coner_img_points_trans;
00734     for (int i = 0; i < (int)coner_img_points.size(); i++){
00735       cv::Point2f pt_tmp(coner_img_points.at(i).x - x_min,
00736                          coner_img_points.at(i).y - y_min);
00737       coner_img_points_trans.push_back(pt_tmp);
00738     }
00739 
00740     cv::Point2f template_points[4] = {cv::Point2f(0,0),
00741                                       cv::Point2f(src.size().width,0),
00742                                       cv::Point2f(src.size().width,src.size().height),
00743                                       cv::Point2f(0,src.size().height)};
00744     cv::Mat template_points_mat (cv::Size(4, 1), CV_32FC2, template_points);
00745 
00746     size = cv::Size(x_max - x_min, y_max - y_min);
00747     return cv::findHomography(template_points_mat, cv::Mat(coner_img_points_trans), 0, 3);
00748   }
00749 
00750 
00751   int make_warped_images (cv::Mat src, std::vector<cv::Mat> &imgs,
00752                           std::vector<cv:: Mat> &Mvec,
00753                           double template_width, double template_height,
00754                           double th_step, double phi_step){
00755 
00756     std::vector<cv::Size> sizevec;
00757 
00758     for (int i = (int)((-3.14/4.0)/th_step); i*th_step < 3.14/4.0; i++){
00759       for (int j = (int)((-3.14/4.0)/phi_step); j*phi_step < 3.14/4.0; j++){
00760         double fR3[3], fT3[3];
00761         cv::Mat rvec(3, 1, CV_64FC1, fR3);
00762         cv::Mat tvec(3, 1, CV_64FC1, fT3);
00763 
00764         tf::Quaternion quat;
00765         quat.setEuler(0, th_step*i, phi_step*j);
00766         fR3[0] = quat.getAxis().x() * quat.getAngle();
00767         fR3[1] = quat.getAxis().y() * quat.getAngle();
00768         fR3[2] = quat.getAxis().z() * quat.getAngle();
00769         fT3[0] = 0;
00770         fT3[1] = 0;
00771         fT3[2] = 0.5;
00772 
00773         cv::Mat M;
00774         cv::Size size;
00775         M = make_homography(src, rvec, tvec, template_width, template_height, size);
00776         Mvec.push_back(M);
00777         sizevec.push_back(size);
00778       }
00779     }
00780 
00781     for (int i = 0; i < (int)Mvec.size(); i++){
00782       cv::Mat dst;
00783       cv::warpPerspective(src, dst, Mvec.at(i), sizevec.at(i),
00784                           CV_INTER_LINEAR, IPL_BORDER_CONSTANT, 0);
00785       imgs.push_back(dst);
00786     }
00787     return 0;
00788   }
00789 
00790   bool add_new_template(cv::Mat img, std::string typestr, tf::Transform relative_pose,
00791                         double template_width, double template_height,
00792                         double theta_step=5.0, double phi_step=5.0)
00793   {
00794     std::vector<cv::Mat> imgs;
00795     std::vector<cv::Mat> Mvec;
00796     make_warped_images(img, imgs, Mvec,
00797                        template_width, template_height, theta_step, phi_step);
00798 
00799     for (int i = 0; i < (int)imgs.size(); i++){
00800       std::string type = typestr;
00801       if(imgs.size() > 1) {
00802         char chr[20];
00803         sprintf(chr, "%d", i);
00804         type += "_" + std::string(chr);
00805       }
00806 
00807       Matching_Template * tmplt = 
00808         new Matching_Template(imgs.at(i), type,
00809                               img.size().width, img.size().height,
00810                               template_width, template_height,
00811                               relative_pose, Mvec.at(i),
00812                               _reprojection_threshold,
00813                               _distanceratio_threshold,
00814                               (_viewer ? type : ""), _autosize);
00815       _templates.push_back(tmplt);
00816       if( _viewer )
00817         cv::namedWindow(type, _autosize ? CV_WINDOW_AUTOSIZE : 0);
00818       cvSetMouseCallback (type.c_str(), &cvmousecb, static_cast<void *>(_templates.back()));
00819     }
00820     return true;
00821   }
00822 
00823   bool settemplate_cb (jsk_perception::SetTemplate::Request &req,
00824                        jsk_perception::SetTemplate::Response &res){
00825       cv_bridge::CvImagePtr cv_ptr;
00826       cv_ptr = cv_bridge::toCvCopy(req.image, enc::BGR8);
00827       cv::Mat img(cv_ptr->image);
00828 
00829       tf::Transform transform(tf::Quaternion(req.relativepose.orientation.x,
00830                                              req.relativepose.orientation.y,
00831                                              req.relativepose.orientation.z,
00832                                              req.relativepose.orientation.w),
00833                               tf::Vector3(req.relativepose.position.x,
00834                                           req.relativepose.position.y,
00835                                           req.relativepose.position.z));
00836 
00837       // add the image to template list 
00838       add_new_template(img, req.type, transform,
00839                        req.dimx, req.dimy, 1.0, 1.0);
00840       return true;
00841   }
00842 
00843 
00844   void imagefeature_cb (const posedetection_msgs::ImageFeature0DConstPtr& msg){
00845     std::vector<cv::KeyPoint> sourceimg_keypoints;
00846     cv::Mat sourceimg_descriptors;
00847     std::vector<posedetection_msgs::Object6DPose> vo6p;
00848     posedetection_msgs::ObjectDetection od;
00849 
00850     cv_bridge::CvImagePtr cv_ptr;
00851     cv_ptr = cv_bridge::toCvCopy(msg->image, enc::BGR8);
00852     cv::Mat src_img(cv_ptr->image);
00853 
00854     // from ros messages to keypoint and pin hole camera model
00855     features2keypoint (msg->features, sourceimg_keypoints, sourceimg_descriptors);
00856     pcam.fromCameraInfo(msg->info);
00857 
00858     if ( !_initialized ) {
00859       // make template images from camera info
00860       initialize();
00861       std::cerr << "initialize templates done" << std::endl;
00862       _initialized = true;
00863     }
00864 
00865     if ( sourceimg_keypoints.size () < 2 ) {
00866       ROS_INFO ("The number of keypoints in source image is less than 2");
00867     }
00868     else {
00869       // make KDTree
00870       cv::flann::Index *ft = new cv::flann::Index(sourceimg_descriptors, cv::flann::KDTreeIndexParams(1));
00871 
00872       // matching and detect object
00873       ROS_INFO("_templates size: %d", (int)_templates.size());
00874       for (int i = 0; i < (int)_templates.size(); i++){
00875         posedetection_msgs::Object6DPose o6p;
00876 
00877         cv::Mat debug_img;
00878         if (_templates.at(i)->estimate_od(_client, src_img, sourceimg_keypoints, pcam, _err_thr, debug_img, ft, &o6p))
00879           vo6p.push_back(o6p);
00880 
00881         // for debug Image topic
00882         cv_bridge::CvImage out_msg;
00883         out_msg.header   = msg->image.header;
00884         out_msg.encoding = "bgr8";
00885         out_msg.image    = debug_img;
00886         _debug_pub.publish(out_msg.toImageMsg());
00887       }
00888       delete ft;
00889       if (((int)vo6p.size() != 0) || pnod) {
00890         od.header.stamp = msg->image.header.stamp;
00891         od.header.frame_id = msg->image.header.frame_id;
00892         od.objects = vo6p;
00893         _pub.publish(od);
00894         _pub_agg.publish(od);
00895       }
00896     }
00897     // BOOST_FOREACH(Matching_Template* mt, _templates) {
00898     //   std::cerr << "templates_ -> " << mt << std::endl;
00899     // }
00900     cv::waitKey( 10 );
00901   }
00902 
00903   /* if there are subscribers of the output topic -> do work
00904          else if -> unregister all topics this node subscribing
00905    */
00906   void check_subscribers()
00907   {
00908         if(_pub.getNumSubscribers() == 0) {
00909           if(_sub)
00910                 _sub.shutdown();
00911         } else {
00912           if(!_sub)
00913                 _sub = _n.subscribe("ImageFeature0D", 1,
00914                                     &PointPoseExtractor::imagefeature_cb, this);
00915         }
00916   }
00917 
00918   /* callback for dynamic reconfigure */
00919   void dyn_conf_callback(jsk_perception::point_pose_extractorConfig &config,
00920                          uint32_t level) {
00921     std::cout << "id = " << config.template_id << std::endl;
00922     std::cout << "lvl = " << level << std::endl;
00923     if((int)_templates.size() <= config.template_id) {
00924       ROS_WARN("template_id is invalid");
00925       config.template_id = 0;
00926       if(_templates.size() != 0)
00927         config.frame_id = _templates[0]->_matching_frame;
00928     } else {
00929       Matching_Template* tmpl = _templates[config.template_id];
00930       if(config.frame_id == tmpl->_matching_frame) {
00931         ROS_WARN("update params");
00932         tmpl->_reprojection_threshold = config.reprojection_threshold;
00933         tmpl->_distanceratio_threshold = config.distanceratio_threshold;
00934         _err_thr = config.error_threshold;
00935       } else {
00936         ROS_WARN("get params");
00937         config.frame_id = tmpl->_matching_frame;
00938         config.reprojection_threshold = tmpl->_reprojection_threshold;
00939         config.distanceratio_threshold = tmpl->_distanceratio_threshold;
00940         config.error_threshold = _err_thr;
00941       }
00942     }
00943   }
00944 
00945 };  // the end of definition of class PointPoseExtractor
00946 
00947 
00948 std::vector<Matching_Template *> PointPoseExtractor::_templates;
00949 
00950 int main (int argc, char **argv){
00951   ros::init (argc, argv, "PointPoseExtractor");
00952 
00953   PointPoseExtractor matcher;
00954 
00955   dynamic_reconfigure::Server<jsk_perception::point_pose_extractorConfig> server;
00956   dynamic_reconfigure::Server<jsk_perception::point_pose_extractorConfig>::CallbackType f;
00957   f = boost::bind(&PointPoseExtractor::dyn_conf_callback, &matcher, _1, _2);
00958   server.setCallback(f);
00959 
00960   ros::Rate r(10); // 10 hz
00961   while(ros::ok()) {
00962         ros::spinOnce();
00963         matcher.check_subscribers();
00964         r.sleep();
00965   }
00966 
00967   return 0;
00968 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


jsk_perception
Author(s): Manabu Saito
autogenerated on Sat Mar 23 2013 18:21:32