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 geometry_msgs::PolygonStamped::ConstPtr& polygon_msg)
82 if (polygon_msg->header.frame_id !=
camera_info_->header.frame_id) {
83 NODELET_ERROR(
"frame_id of polygon (%s) and camera (%s) are not same.",
84 polygon_msg->header.frame_id.c_str(),
camera_info_->header.frame_id.c_str());
89 cv::Mat mask_image = cv::Mat::zeros(
camera_info_->height,
92 std::vector<cv::Point> points;
94 if (polygon_msg->polygon.points.size() >= 3) {
95 for (
size_t i = 0; i < polygon_msg->polygon.points.size(); i++) {
96 geometry_msgs::Point32
p = polygon_msg->polygon.points[i];
97 cv::Point uv = model.project3dToPixel(cv::Point3d(p.x, p.y, p.z));
100 cv::fillConvexPoly(mask_image, &(points[0]), points.size(), cv::Scalar(255));
ros::Subscriber sub_info_
#define NODELET_ERROR(...)
PLUGINLIB_EXPORT_CLASS(jsk_perception::PolygonToMaskImage, nodelet::Nodelet)
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
virtual void unsubscribe()
std::vector< std::string > V_string
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
virtual void convert(const geometry_msgs::PolygonStamped::ConstPtr &polygon_msg)
sensor_msgs::CameraInfo::ConstPtr camera_info_
sensor_msgs::ImagePtr toImageMsg() const