35 #define BOOST_PARAMETER_MAX_ARITY 7 39 #include <pcl/kdtree/kdtree_flann.h> 40 #include <pcl/filters/impl/filter.hpp> 46 DiagnosticNodelet::onInit();
47 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
48 applypub_ = advertise<sensor_msgs::Image>(*
pnh_,
"applyoutput", 1);
49 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
50 dynamic_reconfigure::Server<Config>::CallbackType
f =
53 srv_->setCallback (f);
85 cv::Mat mask = cv_ptr->image;
91 int maskwidth = mask.cols;
92 int maskheight = mask.rows;
94 for (
size_t j = 0; j < maskheight; j++) {
95 for (
size_t i = 0; i < maskwidth; i++) {
96 if (mask.at<uchar>(j, i) != 0) {
104 tmp_width = i-tmp_x_off + 1;
105 tmp_height = j-tmp_y_off + 1;
107 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);
122 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(
queue_size_);
127 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(
queue_size_);
142 const sensor_msgs::PointCloud2::ConstPtr& point_cloud2_msg,
143 const sensor_msgs::Image::ConstPtr& image_msg)
146 if (point_cloud2_msg->width == image_msg->width && point_cloud2_msg->height == image_msg->height) {
149 pcl::PointCloud<pcl::PointXYZ>::Ptr
150 cloud (
new pcl::PointCloud<pcl::PointXYZ>);
152 pcl::PointCloud<pcl::PointXYZ>::Ptr
153 edge_cloud (
new pcl::PointCloud<pcl::PointXYZ>);
154 pcl::PointCloud<pcl::PointXYZ>::Ptr
155 nan_removed_edge_cloud (
new pcl::PointCloud<pcl::PointXYZ>);
157 int width = image_msg->width;
158 int height = image_msg->height;
159 pcl::PointXYZ nan_point;
165 cv::Mat mask = cv_ptr->image;
166 edge_cloud->is_dense =
false;
167 edge_cloud->points.resize(width * height);
168 edge_cloud->width =
width;
169 edge_cloud->height =
height;
177 for (
size_t j = 0; j < mask.rows; j++) {
178 for (
size_t i = 0; i < mask.cols; i++) {
179 if (mask.at<uchar>(j, i) != 0) {
180 edge_cloud->points[j * width + i] = cloud->points[j * width + i];
183 edge_cloud->points[j * width + i] = nan_point;
191 for (
size_t j = 0; j < mask.rows; j++) {
192 for (
size_t i = 0; i < mask.cols; i++) {
193 edge_cloud->points[j * width + i] = nan_point;
200 if (i < image_msg->width && j <image_msg->height) {
201 if (mask.at<uchar>(j, i) != 0) {
202 edge_cloud->points[j * width + i] = cloud->points[j * width + i];
208 std::vector<int> indices;
209 pcl::removeNaNFromPointCloud (*edge_cloud, *nan_removed_edge_cloud, indices);
210 if (nan_removed_edge_cloud->points.size() != 0) {
211 cv::Mat mask_image = cv::Mat::zeros(height, width, CV_8UC1);
212 pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
213 kdtree.setInputCloud(edge_cloud);
217 kdtree.nearestKSearch(zero,
extract_num_, near_indices, near_distances);
218 NODELET_DEBUG(
"directed num of extract points:%d num of nearestKSearch points:%d",
extract_num_, ((
int) near_indices.size()));
219 int ext_num=std::min(
extract_num_, ((
int) near_indices.size()));
220 for (
int idx = 0; idx < ext_num; idx++) {
221 int x = near_indices.at(idx) %
width;
222 int y = near_indices.at(idx) /
width;
223 mask_image.at<uchar>(y, x) = 255;
237 NODELET_DEBUG(
"minx:%d miny:%d maxx:%d maxy:%d", min_x, min_y, max_x, max_y);
238 cv::Rect region = cv::Rect(min_x, min_y, std::max(max_x - min_x, 0), std::max(max_y - min_y, 0));
239 cv::Mat clipped_mask_image = mask_image(region);
250 cv::Mat mask = cv_ptr->image;
251 cv::Mat tmp_mask = cv::Mat::zeros(image_msg->height, image_msg->width, CV_8UC1);
252 int data_len=image_msg->width * image_msg->height;
255 for (
size_t j = 0; j < mask.rows; j++) {
256 for (
size_t i = 0; i < mask.cols; i++) {
257 if (mask.at<uchar>(j, i) != 0) {
260 tmp_mask.at<uchar>(j, i) = mask.at<uchar>(j, i);
273 if (mask.at<uchar>(j, i) != 0) {
276 tmp_mask.at<uchar>(j, i) = mask.at<uchar>(j, i);
295 cv::Rect region = cv::Rect(min_x, min_y, std::max(max_x - min_x, 0), std::max(max_y - min_y, 0));
296 cv::Mat clipped_mask_image = tmp_mask (region);
297 NODELET_INFO(
"minx:%d miny:%d maxx:%d maxy:%d", min_x, min_y, max_x, max_y);
306 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);
void publish(const boost::shared_ptr< M > &message) const
double region_x_off_ratio_
jsk_pcl_ros_utils::MaskImageToDepthConsideredMaskImageConfig Config
virtual void mask_region_callback(const sensor_msgs::Image::ConstPtr &msg)
virtual void configCallback(Config &config, uint32_t level)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::MaskImageToDepthConsideredMaskImage, nodelet::Nodelet)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
double region_width_ratio_
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void unsubscribe()
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
#define NODELET_INFO(...)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
bool in_the_order_of_depth_
#define NODELET_DEBUG(...)
double region_y_off_ratio_
message_filters::Subscriber< sensor_msgs::Image > sub_image_
sensor_msgs::ImagePtr toImageMsg() const
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
double region_height_ratio_
virtual void extractmask(const sensor_msgs::PointCloud2::ConstPtr &point_cloud2_msg, const sensor_msgs::Image::ConstPtr &image_msg)