find_object_on_plane_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros/find_object_on_plane.h"
00037 #include <opencv2/opencv.hpp>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <sensor_msgs/image_encodings.h>
00040 
00041 namespace jsk_pcl_ros
00042 {
00043   void FindObjectOnPlane::onInit()
00044   {
00045     DiagnosticNodelet::onInit();
00046     pub_min_area_rect_image_ = advertise<sensor_msgs::Image>(
00047       *pnh_, "debug/min_area_rect_image", 1);
00048     onInitPostProcess();
00049   }
00050 
00051   void FindObjectOnPlane::subscribe()
00052   {
00053     sub_image_.subscribe(*pnh_, "input", 1);
00054     sub_info_.subscribe(*pnh_, "input/camera_info", 1);
00055     sub_coefficients_.subscribe(*pnh_, "input/coefficients", 1);
00056     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00057     sync_->connectInput(sub_image_, sub_info_, sub_coefficients_);
00058     sync_->registerCallback(boost::bind(&FindObjectOnPlane::find,
00059                                         this, _1, _2, _3));
00060   }
00061 
00062   void FindObjectOnPlane::unsubscribe()
00063   {
00064     sub_image_.unsubscribe();
00065     sub_coefficients_.unsubscribe();
00066   }
00067 
00068   void FindObjectOnPlane::find(
00069     const sensor_msgs::Image::ConstPtr& image_msg,
00070     const sensor_msgs::CameraInfo::ConstPtr& info_msg,
00071     const pcl_msgs::ModelCoefficients::ConstPtr& polygon_3d_coefficient_msg)
00072   {
00073     cv::Mat object_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
00074     
00075     image_geometry::PinholeCameraModel model;
00076     pcl::ModelCoefficients::Ptr polygon_coefficients
00077       (new pcl::ModelCoefficients);
00078     pcl_conversions::toPCL(*polygon_3d_coefficient_msg, *polygon_coefficients);
00079     jsk_recognition_utils::Plane::Ptr plane
00080       (new jsk_recognition_utils::Plane(polygon_coefficients->values));
00081     model.fromCameraInfo(info_msg);
00082     std::vector<cv::Point> all_points;
00083     for (int j = 0; j < object_image.rows; j++) {
00084       for (int i = 0; i < object_image.cols; i++) {
00085         if (object_image.at<uchar>(j, i) == 255) {
00086           all_points.push_back(cv::Point(i, j));
00087         }
00088       }
00089     }
00090     cv::RotatedRect obb = cv::minAreaRect(all_points);
00091     
00092     cv::Mat min_area_rect_image;
00093     cv::cvtColor(object_image, min_area_rect_image, CV_GRAY2BGR);
00094     cv::Point2f vertices2f[4];
00095     obb.points(vertices2f);
00096     cv::line(min_area_rect_image, vertices2f[0], vertices2f[1],
00097              cv::Scalar(0, 0, 255), 4);
00098     cv::line(min_area_rect_image, vertices2f[1], vertices2f[2],
00099              cv::Scalar(0, 0, 255), 4);
00100     cv::line(min_area_rect_image, vertices2f[2], vertices2f[3],
00101              cv::Scalar(0, 0, 255), 4);
00102     cv::line(min_area_rect_image, vertices2f[3], vertices2f[0],
00103              cv::Scalar(0, 0, 255), 4);
00104     cv::Rect bb = obb.boundingRect();
00105     std::vector<cv::Point3f> search_points_3d;
00106     std::vector<cv::Point2f> search_points_2d;
00107     generateStartPoints(cv::Point2f(bb.x, bb.y),
00108                         model, polygon_coefficients,
00109                         search_points_3d, search_points_2d);
00110     for (size_t i = 0; i < search_points_2d.size(); i++) {
00111       cv::circle(min_area_rect_image, search_points_2d[i], 5, cv::Scalar(0, 255, 0), 1);
00112     }
00113     
00114     //for (size_t i = 0; i < search_points_3d.size(); i++) {
00115     double min_area = DBL_MAX;
00116     double min_angle;
00117     cv::Point2f min_search_point;
00118     double min_x;
00119     double min_y;
00120     for (size_t i = 0; i < search_points_2d.size(); i++) {
00121       std::vector<double> angles;
00122       std::vector<double> max_x;
00123       std::vector<double> max_y;
00124       generateAngles(object_image, search_points_2d[i], angles,
00125                      max_x, max_y,
00126                      model, plane);
00127       // draw angles
00128       for (size_t j = 0; j < angles.size(); j++) {
00129         double area = drawAngle(min_area_rect_image, search_points_2d[i], angles[j],
00130                                 max_x[j], max_y[j], model, plane, cv::Scalar(0, 255, 0));
00131         if (area < min_area) {
00132           min_area = area;
00133           min_x = max_x[j];
00134           min_y = max_y[j];
00135           min_angle = angles[j];
00136           min_search_point = search_points_2d[i];
00137         }
00138       }
00139     }
00140     drawAngle(min_area_rect_image, min_search_point, min_angle,
00141               min_x, min_y, model, plane, cv::Scalar(0, 255, 255));
00142     // convert the points into 3-D
00143     pub_min_area_rect_image_.publish(
00144       cv_bridge::CvImage(image_msg->header,
00145                          sensor_msgs::image_encodings::BGR8,
00146                          min_area_rect_image).toImageMsg());
00147     NODELET_INFO("published");
00148   }
00149 
00150 
00151   void FindObjectOnPlane::generateAngles(
00152     const cv::Mat& blob_image, const cv::Point2f& test_point,
00153     std::vector<double>& angles,
00154     std::vector<double>& max_x,
00155     std::vector<double>& max_y,
00156     const image_geometry::PinholeCameraModel& model,
00157     const jsk_recognition_utils::Plane::Ptr& plane)
00158   {
00159     angles.clear();
00160     const double angle_step = 3;
00161     std::vector<cv::Point> indices;
00162     for (int j = 0; j < blob_image.rows; j++) {
00163         for (int i = 0; i < blob_image.cols; i++) {
00164           if (blob_image.at<uchar>(j, i) != 0) { // need to check
00165             indices.push_back(cv::Point(i, j));
00166           }
00167         }
00168     }
00169     for (double angle = -180; angle < 180; angle += angle_step) {
00170       Eigen::Vector2f ux(cos(angle/180*M_PI), sin(angle/180*M_PI));
00171       //Eigen::Vector2f uy(sin(angle/180*M_PI), -cos(angle/180*M_PI));
00172       cv::Point2d uy_end = getUyEnd(test_point, cv::Point2d(test_point.x + ux[0], test_point.y + ux[1]),
00173                                     model,
00174                                     plane);
00175       Eigen::Vector2f uy(uy_end.x - test_point.x, uy_end.y - test_point.y);
00176 
00177       Eigen::Matrix2f A;
00178       A << ux[0], uy[0],
00179         ux[1], uy[1];
00180       bool excluded = false;
00181       double max_alpha = -DBL_MAX;
00182       double max_beta = -DBL_MAX;
00183       for (size_t i = 0; i < indices.size(); i++) {
00184         Eigen::Vector2f p_q = Eigen::Vector2f(indices[i].x, indices[i].y) - Eigen::Vector2f(test_point.x, test_point.y);
00185         Eigen::Vector2f a_b = A.inverse() * p_q;
00186         double alpha = a_b[0];
00187         double beta = a_b[1];
00188         if (alpha < 0 || beta < 0) {
00189           excluded = true;
00190           break;
00191         }
00192         if (alpha > max_alpha) {
00193           max_alpha = alpha;
00194         }
00195         if (beta > max_beta) {
00196           max_beta = beta;
00197         }
00198         
00199       }
00200       if (!excluded) {
00201         angles.push_back(angle);
00202         max_x.push_back(max_alpha);
00203         max_y.push_back(max_beta);
00204       }
00205     }
00206   }
00207 
00208   Eigen::Vector3f FindObjectOnPlane::rayPlaneInteersect(
00209     const cv::Point3d& ray,
00210     const jsk_recognition_utils::Plane::Ptr& plane)
00211   {
00212     Eigen::Vector3f n = plane->getNormal();
00213     Eigen::Vector3f d(ray.x, ray.y, ray.z);
00214     double alpha = - plane->getD() / n.dot(d);
00215     return alpha * d;
00216   }
00217   
00218   cv::Point2d FindObjectOnPlane::getUyEnd(
00219     const cv::Point2d& ux_start,
00220     const cv::Point2d& ux_end,
00221     const image_geometry::PinholeCameraModel& model,
00222     const jsk_recognition_utils::Plane::Ptr& plane)
00223   {
00224     cv::Point3d start_ray = model.projectPixelTo3dRay(ux_start);
00225     cv::Point3d end_ray = model.projectPixelTo3dRay(ux_end);
00226     Eigen::Vector3f ux_start_3d = rayPlaneInteersect(start_ray, plane);
00227     Eigen::Vector3f ux_end_3d = rayPlaneInteersect(end_ray, plane);
00228     Eigen::Vector3f ux_3d = ux_end_3d - ux_start_3d;
00229     Eigen::Vector3f normal = plane->getNormal();
00230     Eigen::Vector3f uy_3d = normal.cross(ux_3d).normalized();
00231     Eigen::Vector3f uy_end_3d = ux_start_3d + uy_3d;
00232     cv::Point2d uy_end = model.project3dToPixel(cv::Point3d(uy_end_3d[0],
00233                                                             uy_end_3d[1],
00234                                                             uy_end_3d[2]));
00235     return uy_end;
00236   }
00237   
00238   double FindObjectOnPlane::drawAngle(
00239     cv::Mat& out_image, const cv::Point2f& test_point, const double angle,
00240     const double max_x, const double max_y,
00241     const image_geometry::PinholeCameraModel& model,
00242     const jsk_recognition_utils::Plane::Ptr& plane,
00243     cv::Scalar color)
00244   {
00245     Eigen::Vector2f ux(cos(angle/180*M_PI), sin(angle/180*M_PI));
00246     cv::Point2d uy_end = getUyEnd(
00247       test_point, cv::Point2d(test_point.x + ux[0], test_point.y + ux[1]),
00248       model, plane);
00249     Eigen::Vector2f uy(uy_end.x - test_point.x, uy_end.y - test_point.y);
00250     //Eigen::Vector2f uy(sin(angle/180*M_PI), -cos(angle/180*M_PI));
00251     Eigen::Vector2f to_point = ux * max_x + Eigen::Vector2f(test_point.x, test_point.y);
00252     Eigen::Vector2f to_point2 = uy * max_y + Eigen::Vector2f(test_point.x, test_point.y);
00253     cv::Point2f to_point_cv(to_point[0], to_point[1]);
00254     cv::Point2f to_point2_cv(to_point2[0], to_point2[1]);
00255     cv::Point2f to_point3_cv = to_point_cv + to_point2_cv - test_point;
00256     cv::line(out_image, test_point, to_point_cv, color, 4);
00257     cv::line(out_image, test_point, to_point2_cv, color, 4);
00258     cv::line(out_image, to_point_cv, to_point3_cv, color, 4);
00259     cv::line(out_image, to_point2_cv, to_point3_cv, color, 4);
00260     return max_x * max_y;       // TODO: it's not correct!
00261   }
00262 
00263   void FindObjectOnPlane::generateStartPoints(
00264     const cv::Point2f& point_2d,
00265     const image_geometry::PinholeCameraModel& model,
00266     const pcl::ModelCoefficients::Ptr& coefficients,
00267     std::vector<cv::Point3f>& search_points_3d,
00268     std::vector<cv::Point2f>& search_points_2d)
00269   {
00270     NODELET_INFO("generateStartPoints");
00271     jsk_recognition_utils::Plane::Ptr plane
00272       (new jsk_recognition_utils::Plane(coefficients->values));
00273     cv::Point3d ray = model.projectPixelTo3dRay(point_2d);
00274     Eigen::Vector3f projected_point = rayPlaneInteersect(ray, plane);
00275     const double resolution_step = 0.01;
00276     const int resolution = 5;
00277     search_points_3d.clear();
00278     search_points_2d.clear();
00279     for (int i = - resolution; i < resolution; ++i) {
00280       for (int j = - resolution; j < resolution; ++j) {
00281         double x_diff = resolution_step * i;
00282         double y_diff = resolution_step * j;
00283         Eigen::Vector3f moved_point = projected_point + Eigen::Vector3f(x_diff, y_diff, 0);
00284         Eigen::Vector3f projected_moved_point;
00285         plane->project(moved_point, projected_moved_point);
00286         cv::Point3f projected_moved_point_cv(projected_moved_point[0],
00287                                              projected_moved_point[1],
00288                                              projected_moved_point[2]);
00289         search_points_3d.push_back(cv::Point3f(projected_moved_point_cv));
00290         cv::Point2d p2d = model.project3dToPixel(projected_moved_point_cv);
00291         search_points_2d.push_back(p2d);
00292       }
00293     }
00294   }
00295 }
00296 
00297 
00298 #include <pluginlib/class_list_macros.h>
00299 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::FindObjectOnPlane, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:43