37 #include <pcl/common/common.h> 45 ConnectionBasedNodelet::onInit();
46 pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*
pnh_,
"output", 1);
47 dynamic_reconfigure::Server<Config>::CallbackType
f =
49 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
50 srv_->setCallback (f);
66 boost::mutex::scoped_lock(
mutex_);
67 if (config.grid_size == 0.0) {
79 boost::mutex::scoped_lock(
mutex_);
80 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
82 Eigen::Vector4f minpt, maxpt;
83 pcl::getMinMax3D<pcl::PointXYZRGB>(*pcl_cloud, minpt, maxpt);
84 int x_bin_num = ceil((maxpt[0] - minpt[0]) /
grid_size_);
85 int y_bin_num = ceil((maxpt[1] - minpt[1]) /
grid_size_);
86 int z_bin_num = ceil((maxpt[2] - minpt[2]) /
grid_size_);
89 std::map<int, std::map<int, std::map<int, std::vector<size_t > > > >
grid;
90 for (
size_t i = 0; i < pcl_cloud->points.size(); i++) {
91 pcl::PointXYZRGB
point = pcl_cloud->points[i];
92 if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) {
96 int xbin = int((point.x - minpt[0]) /
grid_size_);
97 int ybin = int((point.y - minpt[1]) /
grid_size_);
98 int zbin = int((point.z - minpt[2]) /
grid_size_);
99 std::map<int, std::map<int, std::map<int, std::vector<size_t> > > >::iterator xit
101 if (xit == grid.end()) {
103 std::map<int, std::vector<size_t> > new_z;
104 std::vector<size_t> new_indices;
105 new_indices.push_back(i);
106 new_z[zbin] = new_indices;
107 std::map<int, std::map<int, std::vector<size_t> > > new_y;
113 std::map<int, std::map<int, std::vector<size_t> > > ybins = xit->second;
114 std::map<int, std::map<int, std::vector<size_t> > >::iterator yit
116 if (yit == ybins.end()) {
118 std::map<int, std::vector<size_t> > new_z;
119 std::vector<size_t> new_indices;
120 new_indices.push_back(i);
121 new_z[zbin] = new_indices;
122 xit->second[ybin] = new_z;
126 std::map<int, std::vector<size_t> > zbins = yit->second;
127 std::map<int, std::vector<size_t> >::iterator zit
129 if (zit == zbins.end()) {
131 std::vector<size_t> new_indices;
132 new_indices.push_back(i);
133 xit->second[ybin][zbin] = new_indices;
137 xit->second[ybin][zbin].push_back(i);
143 jsk_recognition_msgs::ClusterPointIndices
output;
144 output.header = msg->header;
145 for (std::map<
int, std::map<
int, std::map<
int, std::vector<size_t> > > >::iterator xit = grid.begin();
148 std::map<int, std::map<int, std::vector<size_t> > > ybins = xit->second;
149 for (std::map<
int, std::map<
int, std::vector<size_t> > >::iterator yit = ybins.begin();
152 std::map<int, std::vector<size_t> > zbins = yit->second;
153 for (std::map<
int, std::vector<size_t> >::iterator zit = zbins.begin();
156 std::vector<size_t> indices = zit->second;
160 ros_indices.header = msg->header;
161 for (
size_t j = 0; j < indices.size(); j++) {
162 ros_indices.indices.push_back(indices[j]);
164 output.cluster_indices.push_back(ros_indices);
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void unsubscribe()
void output(int index, double value)
#define NODELET_DEBUG_STREAM(...)
virtual void configCallback(Config &config, uint32_t level)
virtual void sample(const sensor_msgs::PointCloud2::ConstPtr &msg)
jsk_pcl_ros::GridSamplerConfig Config
pcl::PointIndices PCLIndicesMsg
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::GridSampler, nodelet::Nodelet)