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_pcl_ros/mask_image_to_depth_considered_mask_image.h"
00037 #include <sensor_msgs/image_encodings.h>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <pcl/kdtree/kdtree_flann.h>
00040 #include <pcl/filters/impl/filter.hpp>
00041
00042 namespace jsk_pcl_ros
00043 {
00044 void MaskImageToDepthConsideredMaskImage::onInit()
00045 {
00046 DiagnosticNodelet::onInit();
00047 pnh_->param("approximate_sync", approximate_sync_, false);
00048 pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00049 applypub_ = advertise<sensor_msgs::Image>(*pnh_, "applyoutput", 1);
00050 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00051 dynamic_reconfigure::Server<Config>::CallbackType f =
00052 boost::bind (
00053 &MaskImageToDepthConsideredMaskImage::configCallback, this, _1, _2);
00054 srv_->setCallback (f);
00055 sub_ = pnh_->subscribe("input/maskregion", 1, &MaskImageToDepthConsideredMaskImage::mask_region_callback, this);
00056 region_width_ = 0;
00057 region_height_ = 0;
00058 region_x_off_ = 0;
00059 region_y_off_ = 0;
00060 }
00061
00062 void MaskImageToDepthConsideredMaskImage::configCallback(Config &config, uint32_t level){
00063 boost::mutex::scoped_lock lock(mutex_);
00064 extract_num_ = config.extract_num;
00065 use_mask_region_ = config.use_mask_region;
00066 in_the_order_of_depth_ = config.in_the_order_of_depth;
00067 }
00068
00069
00070 void MaskImageToDepthConsideredMaskImage::mask_region_callback(const sensor_msgs::Image::ConstPtr& msg){
00071 boost::mutex::scoped_lock lock(mutex_);
00072 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy
00073 (msg, sensor_msgs::image_encodings::MONO8);
00074 cv::Mat mask = cv_ptr->image;
00075 int tmp_width = 0;
00076 int tmp_height = 0;
00077 int tmp_x_off = 0;
00078 int tmp_y_off = 0;
00079 bool flag = true;
00080 int maskwidth = mask.cols;
00081 int maskheight = mask.rows;
00082 int cnt = 0;
00083 for (size_t j = 0; j < maskheight; j++) {
00084 for (size_t i = 0; i < maskwidth; i++) {
00085 if (mask.at<uchar>(j, i) != 0) {
00086 cnt++;
00087 if (flag == true) {
00088 tmp_x_off = i;
00089 tmp_y_off = j;
00090 flag = false;
00091 }
00092 else {
00093 tmp_width = i-tmp_x_off + 1;
00094 tmp_height = j-tmp_y_off + 1;
00095 }}}}
00096 JSK_NODELET_INFO("mask_image_to_depth_considered_mask_image_nodelet : tmp width:%d height:%d x_off:%d y_off:%d", tmp_width, tmp_height, tmp_x_off, tmp_y_off);
00097 region_width_ratio_ = ((double) tmp_width) / maskwidth;
00098 region_height_ratio_ = ((double) tmp_height) / maskheight;
00099 region_x_off_ratio_ = ((double) tmp_x_off) / maskwidth;
00100 region_y_off_ratio_ = ((double) tmp_y_off) / maskheight;
00101 use_region_ratio_ = true;
00102 JSK_NODELET_INFO("mask_image_to_depth_considered_mask_image_nodelet : next region width_ratio:%f height_ratio:%f x_off_ratio:%f y_off_ratio:%f", region_width_ratio_, region_height_ratio_, region_x_off_ratio_, region_y_off_ratio_);
00103 }
00104
00105
00106 void MaskImageToDepthConsideredMaskImage::subscribe()
00107 {
00108 sub_input_.subscribe(*pnh_, "input", 1);
00109 sub_image_.subscribe(*pnh_, "input/image", 1);
00110 if (approximate_sync_) {
00111 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(100);
00112 async_->connectInput(sub_input_, sub_image_);
00113 async_->registerCallback(boost::bind(&MaskImageToDepthConsideredMaskImage::extractmask, this, _1, _2));
00114 }
00115 else {
00116 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00117 sync_->connectInput(sub_input_, sub_image_);
00118 sync_->registerCallback(boost::bind(&MaskImageToDepthConsideredMaskImage::extractmask,
00119 this, _1, _2));
00120 }
00121 }
00122
00123 void MaskImageToDepthConsideredMaskImage::unsubscribe()
00124 {
00125 sub_input_.unsubscribe();
00126 sub_image_.unsubscribe();
00127 }
00128
00129 void MaskImageToDepthConsideredMaskImage::extractmask
00130 (
00131 const sensor_msgs::PointCloud2::ConstPtr& point_cloud2_msg,
00132 const sensor_msgs::Image::ConstPtr& image_msg)
00133 {
00134 boost::mutex::scoped_lock lock(mutex_);
00135 if (point_cloud2_msg->width == image_msg->width && point_cloud2_msg->height == image_msg->height) {
00136 if (in_the_order_of_depth_ == true) {
00137 vital_checker_->poke();
00138 pcl::PointCloud<pcl::PointXYZ>::Ptr
00139 cloud (new pcl::PointCloud<pcl::PointXYZ>);
00140 pcl::fromROSMsg(*point_cloud2_msg, *cloud);
00141 pcl::PointCloud<pcl::PointXYZ>::Ptr
00142 edge_cloud (new pcl::PointCloud<pcl::PointXYZ>);
00143 pcl::PointCloud<pcl::PointXYZ>::Ptr
00144 nan_removed_edge_cloud (new pcl::PointCloud<pcl::PointXYZ>);
00145
00146 int width = image_msg->width;
00147 int height = image_msg->height;
00148 pcl::PointXYZ nan_point;
00149 nan_point.x = NAN;
00150 nan_point.y = NAN;
00151 nan_point.z = NAN;
00152 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy
00153 (image_msg, sensor_msgs::image_encodings::MONO8);
00154 cv::Mat mask = cv_ptr->image;
00155 edge_cloud->is_dense = false;
00156 edge_cloud->points.resize(width * height);
00157 edge_cloud->width = width;
00158 edge_cloud->height = height;
00159 if (use_region_ratio_) {
00160 region_width_ = width * region_width_ratio_;
00161 region_height_ = height * region_height_ratio_;
00162 region_x_off_ = width * region_x_off_ratio_;
00163 region_y_off_ = height * region_y_off_ratio_;
00164 }
00165 if (use_mask_region_ == false || region_width_ == 0 || region_height_ == 0) {
00166 for (size_t j = 0; j < mask.rows; j++) {
00167 for (size_t i = 0; i < mask.cols; i++) {
00168 if (mask.at<uchar>(j, i) != 0) {
00169 edge_cloud->points[j * width + i] = cloud->points[j * width + i];
00170 }
00171 else {
00172 edge_cloud->points[j * width + i] = nan_point;
00173 }
00174 }
00175 }
00176 }
00177 else {
00178 JSK_ROS_INFO("directed region width:%d height:%d", region_width_, region_height_);
00179
00180 for (size_t j = 0; j < mask.rows; j++) {
00181 for (size_t i = 0; i < mask.cols; i++) {
00182 edge_cloud->points[j * width + i] = nan_point;
00183 }
00184 }
00185 int x_end = region_x_off_+ region_width_;
00186 int y_end = region_y_off_+ region_height_;
00187 for (size_t j = region_y_off_; j < y_end; j++) {
00188 for (size_t i = region_x_off_; i < x_end; i++) {
00189 if (i < image_msg->width && j <image_msg->height) {
00190 if (mask.at<uchar>(j, i) != 0) {
00191 edge_cloud->points[j * width + i] = cloud->points[j * width + i];
00192 }
00193 }
00194 }
00195 }
00196 }
00197 std::vector<int> indices;
00198 pcl::removeNaNFromPointCloud (*edge_cloud, *nan_removed_edge_cloud, indices);
00199 if (nan_removed_edge_cloud->points.size() != 0) {
00200 cv::Mat mask_image = cv::Mat::zeros(height, width, CV_8UC1);
00201 pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
00202 kdtree.setInputCloud(edge_cloud);
00203 pcl::PointXYZ zero;
00204 std::vector<int> near_indices;
00205 std::vector<float> near_distances;
00206 kdtree.nearestKSearch(zero, extract_num_, near_indices, near_distances);
00207 JSK_ROS_INFO("directed num of extract points:%d num of nearestKSearch points:%d", extract_num_, ((int) near_indices.size()));
00208 int ext_num=std::min(extract_num_, ((int) near_indices.size()));
00209 for (int idx = 0; idx < ext_num; idx++) {
00210 int x = near_indices.at(idx) % width;
00211 int y = near_indices.at(idx) / width;
00212 mask_image.at<uchar>(y, x) = 255;
00213 }
00214 cv_bridge::CvImage mask_bridge(point_cloud2_msg->header,
00215 sensor_msgs::image_encodings::MONO8,
00216 mask_image);
00217 pub_.publish(mask_bridge.toImageMsg());
00218 if (use_mask_region_ == false || region_width_ == 0 | region_height_ == 0) {
00219 applypub_.publish(mask_bridge.toImageMsg());
00220 }
00221 else {
00222 int min_x = region_x_off_;
00223 int min_y = region_y_off_;
00224 int max_x = region_width_ + region_x_off_ -1;
00225 int max_y = region_height_ + region_y_off_ -1;
00226 JSK_NODELET_INFO("minx:%d miny:%d maxx:%d maxy:%d", min_x, min_y, max_x, max_y);
00227 cv::Rect region = cv::Rect(min_x, min_y, std::max(max_x - min_x, 0), std::max(max_y - min_y, 0));
00228 cv::Mat clipped_mask_image = mask_image(region);
00229 cv_bridge::CvImage clipped_mask_bridge(point_cloud2_msg->header,
00230 sensor_msgs::image_encodings::MONO8,
00231 clipped_mask_image);
00232 applypub_.publish(clipped_mask_bridge.toImageMsg());
00233 }
00234 }
00235 }
00236 else {
00237 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy
00238 (image_msg, sensor_msgs::image_encodings::MONO8);
00239 cv::Mat mask = cv_ptr->image;
00240 cv::Mat tmp_mask = cv::Mat::zeros(image_msg->height, image_msg->width, CV_8UC1);
00241 int data_len=image_msg->width * image_msg->height;
00242 int cnt = 0;
00243 if (use_mask_region_ ==false || region_width_ == 0 || region_height_ == 0) {
00244 for (size_t j = 0; j < mask.rows; j++) {
00245 for (size_t i = 0; i < mask.cols; i++) {
00246 if (mask.at<uchar>(j, i) != 0) {
00247 cnt++;
00248 if (std::min(extract_num_ , data_len) > cnt) {
00249 tmp_mask.at<uchar>(j, i) = mask.at<uchar>(j, i);
00250 }
00251 }
00252 }
00253 }
00254 }
00255 else {
00256 JSK_ROS_INFO("directed region width:%d height:%d", region_width_, region_height_);
00257 int x_end = region_x_off_ + region_width_;
00258 int y_end = region_y_off_ + region_height_;
00259 for (size_t j = region_y_off_; j < y_end; j++) {
00260 for (size_t i = region_x_off_; i < x_end; i++) {
00261 if (i < image_msg->width && j <image_msg->height){
00262 if (mask.at<uchar>(j, i) != 0) {
00263 cnt++;
00264 if (std::min(extract_num_ , data_len) > cnt) {
00265 tmp_mask.at<uchar>(j, i) = mask.at<uchar>(j, i);
00266 }
00267 }
00268 }
00269 }
00270 }
00271 }
00272 cv_bridge::CvImage mask_bridge(point_cloud2_msg->header,
00273 sensor_msgs::image_encodings::MONO8,
00274 tmp_mask);
00275 pub_.publish(mask_bridge.toImageMsg());
00276 if (use_mask_region_ == false || region_width_ == 0 | region_height_ == 0) {
00277 applypub_.publish(mask_bridge.toImageMsg());
00278 }
00279 else {
00280 int min_x = region_x_off_;
00281 int min_y = region_y_off_;
00282 int max_x = region_width_ + region_x_off_ -1;
00283 int max_y = region_height_ + region_y_off_ -1;
00284 cv::Rect region = cv::Rect(min_x, min_y, std::max(max_x - min_x, 0), std::max(max_y - min_y, 0));
00285 cv::Mat clipped_mask_image = tmp_mask (region);
00286 JSK_NODELET_INFO("minx:%d miny:%d maxx:%d maxy:%d", min_x, min_y, max_x, max_y);
00287 cv_bridge::CvImage clipped_mask_bridge(point_cloud2_msg->header,
00288 sensor_msgs::image_encodings::MONO8,
00289 clipped_mask_image);
00290 applypub_.publish(clipped_mask_bridge.toImageMsg());
00291 }
00292 }
00293 }
00294 else {
00295 JSK_ROS_ERROR ("ERROR: Different width and height. Points[width:%d height:%d] Image[width:%d height:%d]", point_cloud2_msg->width, point_cloud2_msg->height, image_msg->width, image_msg->height);
00296 }
00297 }
00298
00299 void MaskImageToDepthConsideredMaskImage::updateDiagnostic(
00300 diagnostic_updater::DiagnosticStatusWrapper &stat)
00301 {
00302 if (vital_checker_->isAlive()) {
00303 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00304 "MaskImageToDepthConsideredMaskImage running");
00305 }
00306 else {
00307 jsk_topic_tools::addDiagnosticErrorSummary(
00308 "MaskImageToDepthConsideredMaskImage", vital_checker_, stat);
00309 }
00310 }
00311 }
00312
00313 #include <pluginlib/class_list_macros.h>
00314 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::MaskImageToDepthConsideredMaskImage, nodelet::Nodelet);