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