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
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 #include <Eigen/Geometry>
00038 #include <jsk_recognition_utils/pcl_conversion_util.h>
00039 #include "jsk_perception/tabletop_color_difference_likelihood.h"
00040 #include <cv_bridge/cv_bridge.h>
00041 #include <jsk_recognition_utils/sensor_model/camera_depth_sensor.h>
00042 #include <sensor_msgs/image_encodings.h>
00043 #include <jsk_recognition_utils/sensor_model_utils.h>
00044 #include <jsk_recognition_utils/color_utils.h>
00045 #include <jsk_topic_tools/color_utils.h>
00046 #include <jsk_recognition_utils/cv_utils.h>
00047 #include <sstream>
00048
00049 namespace jsk_perception
00050 {
00051 void TabletopColorDifferenceLikelihood::onInit()
00052 {
00053 DiagnosticNodelet::onInit();
00054 pnh_->param("tf_queue_size", tf_queue_size_, 10);
00055 pnh_->param("cyclic_value", cyclic_value_, true);
00056 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00057 dynamic_reconfigure::Server<Config>::CallbackType f =
00058 boost::bind (
00059 &TabletopColorDifferenceLikelihood::configCallback, this, _1, _2);
00060 srv_->setCallback (f);
00061 tf_listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance();
00062 pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00063 pub_debug_histogram_image_ = advertise<sensor_msgs::Image>(*pnh_, "debug/histogram_image", 1);
00064 pub_debug_polygon_ = advertise<sensor_msgs::Image>(*pnh_, "debug/polygon_image", 1);
00065 }
00066
00067 void TabletopColorDifferenceLikelihood::subscribe()
00068 {
00069 sub_info_ = pnh_->subscribe("input/camera_info", 1,
00070 &TabletopColorDifferenceLikelihood::infoCallback, this);
00071 sub_polygons_ = pnh_->subscribe("input/polygons", 1,
00072 &TabletopColorDifferenceLikelihood::polygonCallback, this);
00073 sub_image_.subscribe(*pnh_, "input", 1);
00074 }
00075
00076 void TabletopColorDifferenceLikelihood::unsubscribe()
00077 {
00078 sub_info_.shutdown();
00079 sub_polygons_.shutdown();
00080 sub_image_.unsubscribe();
00081 }
00082
00083 void TabletopColorDifferenceLikelihood::infoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
00084 {
00085 boost::mutex::scoped_lock lock(mutex_);
00086 latest_info_msg_ = msg;
00087 }
00088
00089 void TabletopColorDifferenceLikelihood::polygonCallback(const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
00090 {
00091 boost::mutex::scoped_lock lock(mutex_);
00092 latest_polygon_msg_ = msg;
00093 if (!tf_filter_) {
00094
00095
00096
00097 tf_filter_.reset(new tf::MessageFilter<sensor_msgs::Image>(sub_image_,
00098 *tf_listener_,
00099 msg->header.frame_id,
00100 tf_queue_size_));
00101 tf_filter_->registerCallback(boost::bind(&TabletopColorDifferenceLikelihood::imageCallback, this, _1));
00102 }
00103 }
00104
00105 void TabletopColorDifferenceLikelihood::debugPolygonImage(const jsk_recognition_utils::CameraDepthSensor& model,
00106 cv::Mat& image,
00107 jsk_recognition_utils::Polygon::Ptr polygon,
00108 size_t pi) const
00109 {
00110 polygon->drawLineToImage(model, image,
00111 jsk_recognition_utils::colorROSToCVRGB(jsk_topic_tools::colorCategory20(pi)));
00112 if (polygon->centroid()[2] > 0) {
00113 std::stringstream ss;
00114 ss << pi;
00115 cv::putText(image, ss.str(),
00116 jsk_recognition_utils::project3DPointToPixel(model.getPinholeCameraModel(), polygon->centroid()),
00117 cv::FONT_HERSHEY_SIMPLEX,
00118 0.5,
00119 jsk_recognition_utils::colorROSToCVRGB(jsk_topic_tools::colorCategory20(pi)));
00120 }
00121 }
00122
00123 void TabletopColorDifferenceLikelihood::imageCallback(const sensor_msgs::Image::ConstPtr& msg)
00124 {
00125 boost::mutex::scoped_lock lock(mutex_);
00126
00127 if (!latest_info_msg_) {
00128 JSK_ROS_WARN("no camera info is available yet");
00129 return;
00130 }
00131 if (!latest_polygon_msg_) {
00132 JSK_ROS_WARN("no polygon is available yet");
00133 return;
00134 }
00135
00136
00137 try
00138 {
00139 cv_bridge::CvImagePtr input_cv_image
00140 = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);
00141 cv::Mat input_image = input_cv_image->image;
00142
00143 tf::StampedTransform transform;
00144 tf_listener_->lookupTransform(msg->header.frame_id,
00145 latest_polygon_msg_->header.frame_id,
00146 msg->header.stamp,
00147 transform);
00148 Eigen::Affine3f polygon_transform;
00149 tf::transformTFToEigen(transform, polygon_transform);
00150
00151
00152 std::vector<jsk_recognition_utils::Polygon::Ptr> polygons
00153 = jsk_recognition_utils::Polygon::fromROSMsg(*latest_polygon_msg_, polygon_transform);
00154 jsk_recognition_utils::CameraDepthSensor model;
00155 model.setCameraInfo(*latest_info_msg_);
00156 cv::Mat image = model.image(CV_8UC1);
00157
00158 cv::Mat debug_polygon_image = model.image(CV_8UC3);
00159
00160 cv::Mat histogram_image;
00161 for (size_t pi = 0; pi < polygons.size(); pi++) {
00162 jsk_recognition_utils::Polygon::Ptr polygon = polygons[pi];
00163 cv::Mat mask_image;
00164 polygon->maskImage(model, mask_image);
00165 debugPolygonImage(model, debug_polygon_image, polygon, pi);
00166
00167 cv::MatND hist = jsk_recognition_utils::computeHistogram(input_image, bin_size_,
00168 pixel_min_value_, pixel_max_value_,
00169 mask_image);
00170 std::vector<jsk_recognition_msgs::HistogramWithRangeBin> histogram_bins
00171 = jsk_recognition_utils::cvMatNDToHistogramWithRangeBinArray(hist, pixel_min_value_, pixel_max_value_);
00172 jsk_recognition_utils::sortHistogramWithRangeBinArray(histogram_bins);
00173 std::vector<jsk_recognition_msgs::HistogramWithRangeBin> top_n_histogram_bins
00174 = jsk_recognition_utils::topNHistogramWithRangeBins(histogram_bins, histogram_top_n_ratio_);
00175 cv::Mat one_histogram_image = cv::Mat(30, 180, CV_8UC3);
00176 one_histogram_image = cv::Scalar::all(255);
00177 for (size_t j = 0; j < histogram_bins.size(); j++) {
00178 jsk_recognition_utils::drawHistogramWithRangeBin(one_histogram_image,
00179 histogram_bins[j],
00180 pixel_min_value_,
00181 pixel_max_value_,
00182 top_n_histogram_bins[0].count,
00183 cv::Scalar(0, 0, 0));
00184 }
00185 for (size_t j = 0; j < top_n_histogram_bins.size(); j++) {
00186 jsk_recognition_utils::drawHistogramWithRangeBin(one_histogram_image,
00187 top_n_histogram_bins[j],
00188 pixel_min_value_,
00189 pixel_max_value_,
00190 top_n_histogram_bins[0].count,
00191 cv::Scalar(255, 0, 0));
00192 }
00193 if (histogram_image.empty()) {
00194 histogram_image = one_histogram_image;
00195 }
00196 else {
00197 cv::vconcat(histogram_image, one_histogram_image, histogram_image);
00198 }
00199 const cv::Scalar center_color = cv::mean(input_image, mask_image);
00200 for (size_t j = 0; j < model.height(); j++) {
00201 for (size_t i = 0; i < model.width(); i++) {
00202 if (mask_image.at<unsigned char>(j, i) != 0) {
00203 const unsigned char current_value = image.at<unsigned char>(j, i);
00204
00205
00206 const unsigned char new_value = computePixelHistogramDistance(input_image.at<unsigned char>(j, i),
00207 top_n_histogram_bins);
00208 if (current_value > new_value || current_value == 0) {
00209 image.at<unsigned char>(j, i) = new_value;
00210 }
00211 }
00212 }
00213 }
00214 }
00215 pub_.publish(cv_bridge::CvImage(msg->header,
00216 sensor_msgs::image_encodings::MONO8,
00217 image).toImageMsg());
00218 pub_debug_polygon_.publish(cv_bridge::CvImage(msg->header,
00219 sensor_msgs::image_encodings::RGB8,
00220 debug_polygon_image).toImageMsg());
00221 pub_debug_histogram_image_.publish(cv_bridge::CvImage(msg->header,
00222 sensor_msgs::image_encodings::RGB8,
00223 histogram_image).toImageMsg());
00224 }
00225 catch (...)
00226 {
00227 JSK_NODELET_ERROR("Failed to resolve tf");
00228 }
00229 }
00230
00231 void TabletopColorDifferenceLikelihood::configCallback(Config& config, uint32_t level)
00232 {
00233 boost::mutex::scoped_lock lock(mutex_);
00234 bin_size_ = config.bin_size;
00235 pixel_min_value_ = config.pixel_min_value;
00236 pixel_max_value_ = config.pixel_max_value;
00237 histogram_top_n_ratio_ = config.histogram_top_n_ratio;
00238 }
00239 }
00240
00241 #include <pluginlib/class_list_macros.h>
00242 PLUGINLIB_EXPORT_CLASS (jsk_perception::TabletopColorDifferenceLikelihood, nodelet::Nodelet);