find_object_on_plane.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 
37 #ifndef JSK_PCL_ROS_FIND_OBJECT_ON_PLANE_H_
38 #define JSK_PCL_ROS_FIND_OBJECT_ON_PLANE_H_
39 
45 #include <sensor_msgs/Image.h>
46 #include <sensor_msgs/CameraInfo.h>
47 #include <pcl_msgs/ModelCoefficients.h>
51 
52 namespace jsk_pcl_ros
53 {
55  {
56  public:
58  sensor_msgs::Image,
59  sensor_msgs::CameraInfo,
60  pcl_msgs::ModelCoefficients > SyncPolicy;
61 
62  FindObjectOnPlane(): DiagnosticNodelet("FindObjectOnPlane") {}
63  protected:
64  virtual void onInit();
65  virtual void subscribe();
66  virtual void unsubscribe();
67  virtual void find(
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);
71  virtual void generateStartPoints(
72  const cv::Point2f& point_2d,
74  const pcl::ModelCoefficients::Ptr& coefficients,
75  std::vector<cv::Point3f>& search_points_3d,
76  std::vector<cv::Point2f>& search_points_2d);
77  virtual void generateAngles(
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,
84  virtual double drawAngle(
85  cv::Mat& out_image, const cv::Point2f& test_point, const double angle,
86  const double max_x, const double max_y,
89  cv::Scalar color);
90  Eigen::Vector3f rayPlaneInteersect(
91  const cv::Point3d& ray,
93 
94  virtual cv::Point2d getUyEnd(
95  const cv::Point2d& ux_start,
96  const cv::Point2d& ux_end,
99 
105 
106  private:
107 
108  };
109 }
110 
111 #endif
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)
coefficients
message_filters::Subscriber< sensor_msgs::Image > sub_image_
info_msg
Eigen::Vector3f rayPlaneInteersect(const cv::Point3d &ray, const jsk_recognition_utils::Plane::Ptr &plane)
DiagnosticNodelet(const std::string &name)
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)
GLfloat angle
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_


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:46