37 #include <boost/assign.hpp> 39 #include <opencv2/opencv.hpp> 47 DiagnosticNodelet::onInit();
48 pub_ = advertise<sensor_msgs::Image>(
59 ros::V_string names = boost::assign::list_of(
"~input")(
"~input/camera_info");
70 const sensor_msgs::CameraInfo::ConstPtr& info_msg)
77 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg)
84 cv::Mat mask_image = cv::Mat::zeros(
camera_info_->height,
88 int label_counter = 1;
90 for (
size_t i_polygon = 0; i_polygon < polygon_msg->polygons.size(); ++i_polygon) {
91 geometry_msgs::Polygon polygon = polygon_msg->polygons[i_polygon].polygon;
92 std::vector<cv::Point> points;
93 if (polygon.points.size() >= 3) {
94 bool all_outside =
true;
95 for (
size_t i = 0; i < polygon.points.size(); i++) {
96 geometry_msgs::Point32
p = polygon.points[i];
102 points.push_back(uv);
105 cv::fillConvexPoly(mask_image, &(points[0]), points.size(), label_counter++);
sensor_msgs::CameraInfo::ConstPtr camera_info_
PLUGINLIB_EXPORT_CLASS(jsk_perception::PolygonArrayToLabelImage, nodelet::Nodelet)
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
virtual void unsubscribe()
std::vector< std::string > V_string
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
virtual void convert(const jsk_recognition_msgs::PolygonArray::ConstPtr &polygon_msg)
ros::Subscriber sub_info_
cv::Point2d project3dToPixel(const cv::Point3d &xyz) const
const std::string TYPE_32SC1
sensor_msgs::ImagePtr toImageMsg() const