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