convex_connected_voxels.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 
37 #ifndef _CONVEX_CONNECTED_VOXELS_H_
38 #define _CONVEX_CONNECTED_VOXELS_H_
39 
41 #include <jsk_recognition_msgs/ClusterPointIndices.h>
44 
45 // ROS header directives
46 #include <ros/ros.h>
47 #include <ros/console.h>
48 
49 // ROS sensor message header directives
51 #include <cv_bridge/cv_bridge.h>
53 #include <sensor_msgs/PointCloud2.h>
55 
56 // OpenCV header directives
57 #include <opencv2/imgproc/imgproc.hpp>
58 #include <opencv2/highgui/highgui.hpp>
59 
60 // PCL header directives
61 #include <pcl/point_cloud.h>
62 #include <pcl/point_types.h>
63 #include <pcl/kdtree/kdtree_flann.h>
64 #include <pcl/kdtree/kdtree.h>
65 #include <pcl/filters/passthrough.h>
66 #include <pcl/features/normal_3d_omp.h>
67 #include <pcl/sample_consensus/method_types.h>
68 #include <pcl/sample_consensus/model_types.h>
69 #include <pcl/segmentation/sac_segmentation.h>
70 #include <pcl/segmentation/extract_clusters.h>
71 #include <pcl/filters/extract_indices.h>
72 #include <pcl/common/centroid.h>
73 #include <pcl/common/impl/common.hpp>
74 #include <pcl/registration/distances.h>
75 
76 #include <map>
77 #include <string>
78 
79 
80 namespace jsk_pcl_ros
81 {
83  {
84  public:
85  ConvexConnectedVoxels() : DiagnosticNodelet("ConvexConnectedVoxels") {}
86  typedef pcl::PointXYZRGB PointT;
87 
88  protected:
89  void cloud_cb(
90  const sensor_msgs::PointCloud2::ConstPtr &);
91  void indices_cb(
92  const jsk_recognition_msgs::ClusterPointIndices &);
93  void segmentCloud(
94  const pcl::PointCloud<PointT>::Ptr,
95  const std::vector<pcl::PointIndices> &,
96  std::vector<pcl::PointCloud<PointT>::Ptr> &,
97  std::vector<pcl::PointCloud<pcl::Normal>::Ptr> &,
98  pcl::PointCloud<pcl::PointXYZ>::Ptr);
100  const pcl::PointCloud<PointT>::Ptr,
101  pcl::PointCloud<pcl::Normal>::Ptr,
102  const int = 8, const double = 0.02,
103  bool = true);
105  pcl::PointCloud<pcl::PointXYZ>::Ptr,
106  std::vector<std::vector<int> > &,
107  const int = 8,
108  const double = 0.02,
109  bool = true);
111  const std::vector<pcl::PointCloud<PointT>::Ptr> &,
112  pcl::PointCloud<PointT>::Ptr,
113  const std::vector<int> &,
114  std::map<int, pcl::PointIndices> &);
115 
121 
122  virtual void onInit();
123  virtual void subscribe();
124  virtual void unsubscribe();
125 
126  private:
127  std::vector<pcl::PointIndices> indices_;
128  };
129 } // namespace jsk_pcl_ros
130 
131 #endif // _CONVEX_CONNECTED_VOXELS_H_
void segmentCloud(const pcl::PointCloud< PointT >::Ptr, const std::vector< pcl::PointIndices > &, std::vector< pcl::PointCloud< PointT >::Ptr > &, std::vector< pcl::PointCloud< pcl::Normal >::Ptr > &, pcl::PointCloud< pcl::PointXYZ >::Ptr)
void cloud_cb(const sensor_msgs::PointCloud2::ConstPtr &)
void getConvexLabelCloudIndices(const std::vector< pcl::PointCloud< PointT >::Ptr > &, pcl::PointCloud< PointT >::Ptr, const std::vector< int > &, std::map< int, pcl::PointIndices > &)
std::vector< pcl::PointIndices > indices_
DiagnosticNodelet(const std::string &name)
void nearestNeigborSearch(pcl::PointCloud< pcl::PointXYZ >::Ptr, std::vector< std::vector< int > > &, const int=8, const double=0.02, bool=true)
void indices_cb(const jsk_recognition_msgs::ClusterPointIndices &)
boost::mutex mutex
global mutex.
void estimatePointCloudNormals(const pcl::PointCloud< PointT >::Ptr, pcl::PointCloud< pcl::Normal >::Ptr, const int=8, const double=0.02, bool=true)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:46