36 #define BOOST_PARAMETER_MAX_ARITY 7 37 #include <Eigen/Geometry> 38 #include <boost/assign.hpp> 55 DiagnosticNodelet::onInit();
58 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
59 dynamic_reconfigure::Server<Config>::CallbackType
f =
62 srv_->setCallback (f);
64 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
77 ros::V_string names = boost::assign::list_of(
"~input")(
"~input/camera_info")(
"~input/polygons");
104 msg->header.frame_id,
115 polygon->drawLineToImage(model, image,
117 if (polygon->centroid()[2] > 0) {
118 std::stringstream ss;
120 cv::putText(image, ss.str(),
122 cv::FONT_HERSHEY_SIMPLEX,
133 ROS_WARN(
"no camera info is available yet");
137 ROS_WARN(
"no polygon is available yet");
146 cv::Mat input_image = input_cv_image->image;
153 Eigen::Affine3f polygon_transform;
157 std::vector<jsk_recognition_utils::Polygon::Ptr> polygons
161 cv::Mat image = model.
image(CV_8UC1);
163 cv::Mat debug_polygon_image = model.
image(CV_8UC3);
165 cv::Mat histogram_image;
166 for (
size_t pi = 0; pi < polygons.size(); pi++) {
169 polygon->maskImage(model, mask_image);
175 std::vector<jsk_recognition_msgs::HistogramWithRangeBin> histogram_bins
178 std::vector<jsk_recognition_msgs::HistogramWithRangeBin> top_n_histogram_bins
180 cv::Mat one_histogram_image = cv::Mat(30, 180, CV_8UC3);
181 one_histogram_image = cv::Scalar::all(255);
182 for (
size_t j = 0; j < histogram_bins.size(); j++) {
187 top_n_histogram_bins[0].
count,
188 cv::Scalar(0, 0, 0));
190 for (
size_t j = 0; j < top_n_histogram_bins.size(); j++) {
192 top_n_histogram_bins[j],
195 top_n_histogram_bins[0].
count,
196 cv::Scalar(255, 0, 0));
198 if (histogram_image.empty()) {
199 histogram_image = one_histogram_image;
202 cv::vconcat(histogram_image, one_histogram_image, histogram_image);
204 const cv::Scalar center_color =
cv::mean(input_image, mask_image);
205 for (
size_t j = 0; j < model.
height(); j++) {
206 for (
size_t i = 0; i < model.
width(); i++) {
207 if (mask_image.at<
unsigned char>(j, i) != 0) {
208 const unsigned char current_value = image.at<
unsigned char>(j, i);
212 top_n_histogram_bins);
213 if (current_value > new_value || current_value == 0) {
214 image.at<
unsigned char>(j, i) = new_value;
sensor_msgs::CameraInfo::ConstPtr latest_info_msg_
#define NODELET_ERROR(...)
virtual void polygonCallback(const jsk_recognition_msgs::PolygonArray::ConstPtr &msg)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
TabletopColorDifferenceLikelihoodConfig Config
virtual int width() const
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &msg)
virtual void imageCallback(const sensor_msgs::Image::ConstPtr &msg)
cv::MatND computeHistogram(const cv::Mat &input_image, int bin_size, float min_value, float max_value, const cv::Mat &mask_image)
boost::shared_ptr< tf::MessageFilter< sensor_msgs::Image > > tf_filter_
virtual cv::Mat image(const int type) const
static Polygon fromROSMsg(const geometry_msgs::Polygon &polygon)
PLUGINLIB_EXPORT_CLASS(jsk_perception::TabletopColorDifferenceLikelihood, nodelet::Nodelet)
virtual void configCallback(Config &config, uint32_t level)
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)
message_filters::Subscriber< sensor_msgs::Image > sub_image_
std::vector< jsk_recognition_msgs::HistogramWithRangeBin > topNHistogramWithRangeBins(const std::vector< jsk_recognition_msgs::HistogramWithRangeBin > &bins, double top_n_rate)
virtual int height() const
std::vector< std::string > V_string
void sortHistogramWithRangeBinArray(std::vector< jsk_recognition_msgs::HistogramWithRangeBin > &bins)
double histogram_top_n_ratio_
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void unsubscribe()
ros::Publisher pub_debug_histogram_image_
cv::Scalar colorROSToCVRGB(const std_msgs::ColorRGBA &ros_color)
virtual unsigned char computePixelHistogramDistance(const unsigned char from, const std::vector< jsk_recognition_msgs::HistogramWithRangeBin > &bins)
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)
void drawHistogramWithRangeBin(cv::Mat &image, const jsk_recognition_msgs::HistogramWithRangeBin &bin, float min_width_value, float max_width_value, float max_height_value, cv::Scalar color)
ros::Publisher pub_debug_polygon_
virtual void debugPolygonImage(const jsk_recognition_utils::CameraDepthSensor &model, cv::Mat &image, jsk_recognition_utils::Polygon::Ptr polygon, size_t pi) const
ros::Subscriber sub_info_
tf::TransformListener * tf_listener_
ros::Subscriber sub_polygons_
static tf::TransformListener * getInstance()
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
jsk_recognition_msgs::PolygonArray::ConstPtr latest_polygon_msg_
sensor_msgs::ImagePtr toImageMsg() const
virtual void setCameraInfo(const sensor_msgs::CameraInfo &info)