00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_perception/polygon_array_color_histogram.h"
00037 #include <cv_bridge/cv_bridge.h>
00038 #include <jsk_recognition_utils/sensor_model/camera_depth_sensor.h>
00039 #include <jsk_recognition_msgs/HistogramWithRangeArray.h>
00040 #include <jsk_recognition_utils/color_utils.h>
00041 #include <jsk_topic_tools/color_utils.h>
00042 #include <jsk_recognition_utils/sensor_model_utils.h>
00043 #include <jsk_recognition_utils/cv_utils.h>
00044 #include <sensor_msgs/image_encodings.h>
00045 #include <jsk_recognition_utils/pcl_ros_util.h>
00046
00047 namespace jsk_perception
00048 {
00049 void PolygonArrayColorHistogram::onInit()
00050 {
00051 DiagnosticNodelet::onInit();
00052 tf_listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance();
00053 pnh_->param("max_queue_size", max_queue_size_, 10);
00054 pnh_->param("synchronizer_queue_size", sync_queue_size_, 100);
00055 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00056 dynamic_reconfigure::Server<Config>::CallbackType f =
00057 boost::bind (
00058 &PolygonArrayColorHistogram::configCallback, this, _1, _2);
00059 srv_->setCallback (f);
00060 pub_ = advertise<jsk_recognition_msgs::HistogramWithRangeArray>(*pnh_, "output", 1);
00061 pub_debug_polygon_ = advertise<sensor_msgs::Image>(*pnh_, "debug/polygon_image", 1);
00062 onInitPostProcess();
00063 }
00064
00065 void PolygonArrayColorHistogram::subscribe()
00066 {
00067 sub_info_ = pnh_->subscribe("input/info", 1, &PolygonArrayColorHistogram::infoCallback, this);
00068 sub_polygon_.subscribe(*pnh_, "input", max_queue_size_);
00069 sub_image_.subscribe(*pnh_, "input/image", max_queue_size_);
00070 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(sync_queue_size_);
00071 async_->connectInput(sub_image_, sub_polygon_);
00072 async_->registerCallback(
00073 boost::bind(&PolygonArrayColorHistogram::compute, this, _1, _2));
00074 }
00075
00076 void PolygonArrayColorHistogram::unsubscribe()
00077 {
00078 sub_info_.shutdown();
00079 sub_polygon_.unsubscribe();
00080 sub_image_.unsubscribe();
00081 }
00082
00083 void PolygonArrayColorHistogram::infoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
00084 {
00085 boost::mutex::scoped_lock lock(mutex_);
00086 info_ = msg;
00087 }
00088
00089 void PolygonArrayColorHistogram::debugPolygonImage(
00090 const jsk_recognition_utils::CameraDepthSensor& model,
00091 cv::Mat& image,
00092 jsk_recognition_utils::Polygon::Ptr polygon,
00093 size_t pi) const
00094 {
00095 polygon->drawLineToImage(model, image,
00096 jsk_recognition_utils::colorROSToCVRGB(jsk_topic_tools::colorCategory20(pi)),
00097 debug_line_width_);
00098 if (polygon->centroid()[2] > 0) {
00099 std::stringstream ss;
00100 ss << pi;
00101 cv::putText(image, ss.str(),
00102 jsk_recognition_utils::project3DPointToPixel(model.getPinholeCameraModel(), polygon->centroid()),
00103 cv::FONT_HERSHEY_SIMPLEX,
00104 0.5,
00105 jsk_recognition_utils::colorROSToCVRGB(jsk_topic_tools::colorCategory20(pi)));
00106 }
00107 }
00108
00109
00110 void PolygonArrayColorHistogram::compute(
00111 const sensor_msgs::Image::ConstPtr& image_msg,
00112 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg)
00113 {
00114 boost::mutex::scoped_lock lock(mutex_);
00115 if (!info_) {
00116 NODELET_WARN("No camera_info is available");
00117 return;
00118 }
00119
00120 if (!jsk_recognition_utils::isSameFrameId(image_msg->header.frame_id,
00121 polygon_msg->header.frame_id)) {
00122 NODELET_ERROR("frame_id does not match. image: %s, polygon: %s",
00123 image_msg->header.frame_id.c_str(),
00124 polygon_msg->header.frame_id.c_str());
00125 return;
00126 }
00127 try
00128 {
00129 tf::StampedTransform transform
00130 = jsk_recognition_utils::lookupTransformWithDuration(
00131 tf_listener_, polygon_msg->header.frame_id, image_msg->header.frame_id,
00132 image_msg->header.stamp, ros::Duration(1.0));
00133 Eigen::Affine3f polygon_transform;
00134 tf::transformTFToEigen(transform, polygon_transform);
00135 cv_bridge::CvImagePtr input_cv_image
00136 = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8);
00137 cv::Mat input_image = input_cv_image->image;
00138 jsk_recognition_utils::CameraDepthSensor model;
00139 model.setCameraInfo(*info_);
00140 cv::Mat debug_polygon_image = model.image(CV_8UC3);
00141 std::vector<jsk_recognition_utils::Polygon::Ptr> polygons
00142 = jsk_recognition_utils::Polygon::fromROSMsg(*polygon_msg, polygon_transform);
00143 cv::Mat image = model.image(CV_8UC1);
00144 jsk_recognition_msgs::HistogramWithRangeArray histogram_array;
00145 histogram_array.header = polygon_msg->header;
00146 for (size_t pi = 0; pi < polygons.size(); pi++) {
00147 jsk_recognition_utils::Polygon::Ptr polygon = polygons[pi];
00148 debugPolygonImage(model, debug_polygon_image, polygon, pi);
00149 cv::Mat mask_image;
00150 polygon->maskImage(model, mask_image);
00151 cv::MatND hist = jsk_recognition_utils::computeHistogram(input_image, bin_size_,
00152 pixel_min_value_, pixel_max_value_,
00153 mask_image);
00154 jsk_recognition_msgs::HistogramWithRange histogram;
00155 histogram.header = polygon_msg->header;
00156 histogram.bins
00157 = jsk_recognition_utils::cvMatNDToHistogramWithRangeBinArray(
00158 hist, pixel_min_value_, pixel_max_value_);
00159 histogram_array.histograms.push_back(histogram);
00160 }
00161 pub_debug_polygon_.publish(cv_bridge::CvImage(image_msg->header,
00162 sensor_msgs::image_encodings::RGB8,
00163 debug_polygon_image).toImageMsg());
00164 pub_.publish(histogram_array);
00165 }
00166 catch (...)
00167 {
00168 NODELET_ERROR("Failed to resolve tf");
00169 }
00170
00171 }
00172
00173 void PolygonArrayColorHistogram::configCallback(Config& config, uint32_t level)
00174 {
00175 boost::mutex::scoped_lock lock(mutex_);
00176 bin_size_ = config.bin_size;
00177 pixel_min_value_ = config.pixel_min_value;
00178 pixel_max_value_ = config.pixel_max_value;
00179 debug_line_width_ = config.debug_line_width;
00180 }
00181
00182 }
00183
00184 #include <pluginlib/class_list_macros.h>
00185 PLUGINLIB_EXPORT_CLASS (jsk_perception::PolygonArrayColorHistogram, nodelet::Nodelet);