35 #define BOOST_PARAMETER_MAX_ARITY 7 39 #include <jsk_recognition_msgs/HistogramWithRangeArray.h> 51 DiagnosticNodelet::onInit();
55 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
56 dynamic_reconfigure::Server<Config>::CallbackType
f =
59 srv_->setCallback (f);
60 pub_ = advertise<jsk_recognition_msgs::HistogramWithRangeArray>(*
pnh_,
"output", 1);
95 polygon->drawLineToImage(model, image,
98 if (polygon->centroid()[2] > 0) {
101 cv::putText(image, ss.str(),
103 cv::FONT_HERSHEY_SIMPLEX,
111 const sensor_msgs::Image::ConstPtr& image_msg,
112 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg)
121 polygon_msg->header.frame_id)) {
122 NODELET_ERROR(
"frame_id does not match. image: %s, polygon: %s",
123 image_msg->header.frame_id.c_str(),
124 polygon_msg->header.frame_id.c_str());
131 tf_listener_, polygon_msg->header.frame_id, image_msg->header.frame_id,
133 Eigen::Affine3f polygon_transform;
137 cv::Mat input_image = input_cv_image->image;
140 cv::Mat debug_polygon_image = model.
image(CV_8UC3);
141 std::vector<jsk_recognition_utils::Polygon::Ptr> polygons
143 cv::Mat image = model.
image(CV_8UC1);
144 jsk_recognition_msgs::HistogramWithRangeArray histogram_array;
145 histogram_array.header = polygon_msg->header;
146 for (
size_t pi = 0; pi < polygons.size(); pi++) {
150 polygon->maskImage(model, mask_image);
154 jsk_recognition_msgs::HistogramWithRange histogram;
155 histogram.header = polygon_msg->header;
159 histogram_array.histograms.push_back(histogram);
virtual void configCallback(Config &config, uint32_t level)
#define NODELET_ERROR(...)
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
sensor_msgs::CameraInfo::ConstPtr info_
tf::StampedTransform lookupTransformWithDuration(tf::TransformListener *listener, const std::string &to_frame, const std::string &from_frame, const ros::Time &stamp, ros::Duration duration)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
cv::MatND computeHistogram(const cv::Mat &input_image, int bin_size, float min_value, float max_value, const cv::Mat &mask_image)
virtual cv::Mat image(const int type) const
static Polygon fromROSMsg(const geometry_msgs::Polygon &polygon)
ros::Publisher pub_debug_polygon_
virtual image_geometry::PinholeCameraModel getPinholeCameraModel() const
cv::Point project3DPointToPixel(const image_geometry::PinholeCameraModel &model, const Eigen::Vector3f &p)
std::vector< jsk_recognition_msgs::HistogramWithRangeBin > cvMatNDToHistogramWithRangeBinArray(const cv::MatND &cv_hist, float min_value, float max_value)
jsk_perception::PolygonArrayColorHistogramConfig Config
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &msg)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void unsubscribe()
cv::Scalar colorROSToCVRGB(const std_msgs::ColorRGBA &ros_color)
ros::Subscriber sub_info_
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_polygon_
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
virtual void debugPolygonImage(const jsk_recognition_utils::CameraDepthSensor &model, cv::Mat &image, jsk_recognition_utils::Polygon::Ptr polygon, size_t pi) const
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
virtual void compute(const sensor_msgs::Image::ConstPtr &image_msg, const jsk_recognition_msgs::PolygonArray::ConstPtr &polygon_msg)
tf::TransformListener * tf_listener_
bool isSameFrameId(const std::string &a, const std::string &b)
static tf::TransformListener * getInstance()
message_filters::Subscriber< sensor_msgs::Image > sub_image_
sensor_msgs::ImagePtr toImageMsg() const
virtual void setCameraInfo(const sensor_msgs::CameraInfo &info)
PLUGINLIB_EXPORT_CLASS(jsk_perception::PolygonArrayColorHistogram, nodelet::Nodelet)