Go to the documentation of this file.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 "jsk_pcl_ros/mask_image_filter.h"
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <image_geometry/pinhole_camera_model.h>
00040 #include "jsk_recognition_utils/pcl_conversion_util.h"
00041 #include <sensor_msgs/image_encodings.h>
00042
00043 namespace jsk_pcl_ros
00044 {
00045 void MaskImageFilter::onInit()
00046 {
00047 DiagnosticNodelet::onInit();
00048 pnh_->param("negative", negative_, false);
00049 pub_ = advertise<PCLIndicesMsg>(
00050 *pnh_, "output", 1);
00051 DiagnosticNodelet::onInitPostProcess();
00052 }
00053
00054 void MaskImageFilter::subscribe()
00055 {
00056 sub_cloud_ = pnh_->subscribe("input", 1, &MaskImageFilter::filter, this);
00057 sub_image_ = pnh_->subscribe("input/mask", 1,
00058 &MaskImageFilter::imageCalback, this);
00059 sub_info_ = pnh_->subscribe("input/camera_info", 1,
00060 &MaskImageFilter::infoCalback, this);
00061 }
00062
00063 void MaskImageFilter::unsubscribe()
00064 {
00065 sub_cloud_.shutdown();
00066 sub_info_.shutdown();
00067 sub_info_.shutdown();
00068 }
00069
00070 void MaskImageFilter::infoCalback(
00071 const sensor_msgs::CameraInfo::ConstPtr& info_ms)
00072 {
00073 boost::mutex::scoped_lock lock(mutex_);
00074 camera_info_ = info_ms;
00075 }
00076
00077 void MaskImageFilter::imageCalback(
00078 const sensor_msgs::Image::ConstPtr& mask_msg)
00079 {
00080 boost::mutex::scoped_lock lock(mutex_);
00081 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
00082 mask_msg, sensor_msgs::image_encodings::MONO8);
00083 mask_image_ = cv_ptr->image;
00084
00085 if (negative_) {
00086 cv::bitwise_not(mask_image_, mask_image_);
00087 }
00088 }
00089
00090 void MaskImageFilter::filter(
00091 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00092 {
00093 boost::mutex::scoped_lock lock(mutex_);
00094 if (camera_info_ && !mask_image_.empty()) {
00095 image_geometry::PinholeCameraModel model;
00096 model.fromCameraInfo(camera_info_);
00097 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
00098 (new pcl::PointCloud<pcl::PointXYZ>);
00099 pcl::fromROSMsg(*cloud_msg, *cloud);
00100 PCLIndicesMsg indices;
00101 indices.header = cloud_msg->header;
00102 for (size_t i = 0; i < cloud->points.size(); i++) {
00103 pcl::PointXYZ p = cloud->points[i];
00104 cv::Point2d uv = model.project3dToPixel(cv::Point3d(p.x, p.y, p.z));
00105
00106 if (uv.x > 0 && uv.x < mask_image_.cols &&
00107 uv.y > 0 && uv.y < mask_image_.rows) {
00108 if (mask_image_.at<uchar>(uv.y, uv.x) == 255) {
00109 indices.indices.push_back(i);
00110 }
00111 }
00112 }
00113 pub_.publish(indices);
00114 }
00115 }
00116 }
00117
00118 #include <pluginlib/class_list_macros.h>
00119 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::MaskImageFilter, nodelet::Nodelet);
00120
00121