37 #ifndef JSK_PCL_ROS_FIND_OBJECT_ON_PLANE_H_ 38 #define JSK_PCL_ROS_FIND_OBJECT_ON_PLANE_H_ 45 #include <sensor_msgs/Image.h> 46 #include <sensor_msgs/CameraInfo.h> 47 #include <pcl_msgs/ModelCoefficients.h> 59 sensor_msgs::CameraInfo,
68 const sensor_msgs::Image::ConstPtr& image_msg,
69 const sensor_msgs::CameraInfo::ConstPtr&
info_msg,
70 const pcl_msgs::ModelCoefficients::ConstPtr& polygon_3d_coefficient_msg);
72 const cv::Point2f& point_2d,
75 std::vector<cv::Point3f>& search_points_3d,
76 std::vector<cv::Point2f>& search_points_2d);
78 const cv::Mat& blob_image,
const cv::Point2f& test_point,
79 std::vector<double>&
angles,
80 std::vector<double>& max_x,
81 std::vector<double>& max_y,
85 cv::Mat& out_image,
const cv::Point2f& test_point,
const double angle,
86 const double max_x,
const double max_y,
91 const cv::Point3d& ray,
95 const cv::Point2d& ux_start,
96 const cv::Point2d& ux_end,
virtual cv::Point2d getUyEnd(const cv::Point2d &ux_start, const cv::Point2d &ux_end, const image_geometry::PinholeCameraModel &model, const jsk_recognition_utils::Plane::Ptr &plane)
virtual void generateStartPoints(const cv::Point2f &point_2d, const image_geometry::PinholeCameraModel &model, const pcl::ModelCoefficients::Ptr &coefficients, std::vector< cv::Point3f > &search_points_3d, std::vector< cv::Point2f > &search_points_2d)
virtual void find(const sensor_msgs::Image::ConstPtr &image_msg, const sensor_msgs::CameraInfo::ConstPtr &info_msg, const pcl_msgs::ModelCoefficients::ConstPtr &polygon_3d_coefficient_msg)
message_filters::Subscriber< sensor_msgs::Image > sub_image_
Eigen::Vector3f rayPlaneInteersect(const cv::Point3d &ray, const jsk_recognition_utils::Plane::Ptr &plane)
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
virtual double drawAngle(cv::Mat &out_image, const cv::Point2f &test_point, const double angle, const double max_x, const double max_y, const image_geometry::PinholeCameraModel &model, const jsk_recognition_utils::Plane::Ptr &plane, cv::Scalar color)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::CameraInfo, pcl_msgs::ModelCoefficients > SyncPolicy
virtual void generateAngles(const cv::Mat &blob_image, const cv::Point2f &test_point, std::vector< double > &angles, std::vector< double > &max_x, std::vector< double > &max_y, const image_geometry::PinholeCameraModel &model, const jsk_recognition_utils::Plane::Ptr &plane)
message_filters::Subscriber< pcl_msgs::ModelCoefficients > sub_coefficients_
ros::Publisher pub_min_area_rect_image_
virtual void unsubscribe()