people_detection_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016, 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/or 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 #define BOOST_PARAMETER_MAX_ARITY 7
40 
41 #include <pcl/people/ground_based_people_detection_app.h>
42 
43 namespace jsk_pcl_ros {
45  DiagnosticNodelet::onInit();
46 
47  pnh_->param("people_height_threshold", people_height_threshold_, 0.5);
48  pnh_->param("min_confidence", min_confidence_, -1.5);
49  pnh_->param("queue_size", queue_size_, 1);
50  pnh_->param("voxel_size", voxel_size_, 0.03);
51  pnh_->param("box_width", box_width_, 0.5);
52  pnh_->param("box_depth", box_depth_, 0.5);
53  pnh_->param<std::string>("filename", trained_filename_, "");
54 
55  if (trained_filename_ == "") {
56  NODELET_FATAL("Please set svm file name");
57  }
58 
59  person_classifier_.loadSVMFromFile(trained_filename_); // load trained SVM
60  people_detector_.setVoxelSize(voxel_size_); // set the voxel size
61 
62  Eigen::Matrix3f rgb_intrinsics_matrix;
63  rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0,
64  1.0; // Kinect RGB camera intrinsics
65 
66  people_detector_.setIntrinsics(
67  rgb_intrinsics_matrix); // set RGB camera intrinsic parameters
68  people_detector_.setClassifier(
69  person_classifier_); // set person classifier
70 
71  ground_coeffs_.resize(4);
72  ground_coeffs_[0] = 0.0;
73  ground_coeffs_[1] = 0.0;
74  ground_coeffs_[2] = 1.0;
75  ground_coeffs_[3] = 0.0;
76 
78  // dynamic reconfigure
80  srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
81  dynamic_reconfigure::Server<Config>::CallbackType f =
82  boost::bind(&PeopleDetection::configCallback, this, _1, _2);
83  srv_->setCallback(f);
84 
86  // Publisher
88  pub_box_ =
89  advertise<jsk_recognition_msgs::BoundingBoxArray>(*pnh_, "boxes", 1);
90 
92  }
93 
94  void PeopleDetection::configCallback(Config& config, uint32_t level) {
95  boost::mutex::scoped_lock lock(mutex_);
96  voxel_size_ = config.voxel_size;
97  min_confidence_ = config.min_confidence;
98  people_height_threshold_ = config.people_height_threshold;
99  box_width_ = config.box_width;
100  box_depth_ = config.box_depth;
101 
102  people_detector_.setVoxelSize(voxel_size_); // set the voxel size
103  }
104 
106  sub_info_ =
107  pnh_->subscribe("input/info", 1, &PeopleDetection::infoCallback, this);
108  sub_cloud_ = pnh_->subscribe("input", 1, &PeopleDetection::detect, this);
109  sub_coefficients_ = pnh_->subscribe(
110  "input/coefficients", 1, &PeopleDetection::ground_coeffs_callback, this);
111  }
112 
117  }
118 
120  const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
121  boost::mutex::scoped_lock lock(mutex_);
122  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr input_cloud(
123  new pcl::PointCloud<pcl::PointXYZRGBA>);
124  pcl::fromROSMsg(*cloud_msg, *input_cloud);
125 
126  std::vector<pcl::people::PersonCluster<pcl::PointXYZRGBA> >
127  clusters; // vector containing persons clusters
128  people_detector_.setInputCloud(input_cloud);
129  people_detector_.setGround(ground_coeffs_); // set floor coefficients
130  people_detector_.compute(clusters); // perform people detection
131 
132  jsk_recognition_msgs::BoundingBoxArray bounding_box_array;
133  bounding_box_array.header = cloud_msg->header;
134  jsk_recognition_msgs::BoundingBox bounding_box;
135  bounding_box.header = cloud_msg->header;
136 
137  for (std::vector<pcl::people::PersonCluster<pcl::PointXYZRGBA> >::iterator it =
138  clusters.begin();
139  it != clusters.end(); ++it) {
140  if (it->getPersonConfidence() > min_confidence_ &&
141  it->getHeight() > people_height_threshold_) {
142  bounding_box.pose.position.x = it->getCenter()[0];
143  bounding_box.pose.position.y = it->getCenter()[1] + it->getHeight() / 2;
144  bounding_box.pose.position.z = it->getCenter()[2];
145 
146  bounding_box.pose.orientation.x = 0.0;
147  bounding_box.pose.orientation.y = 0.0;
148  bounding_box.pose.orientation.z = 0.0;
149  bounding_box.pose.orientation.w = 1.0;
150 
151  bounding_box.dimensions.x = box_width_;
152  bounding_box.dimensions.y = it->getHeight() + 0.3;
153  bounding_box.dimensions.z = box_depth_;
154 
155  bounding_box_array.boxes.push_back(bounding_box);
156  }
157  }
158  pub_box_.publish(bounding_box_array);
159  }
160 
162  const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr&
163  coefficients_msg) {
164  if (coefficients_msg->coefficients.size() >= 1) {
165  set_ground_coeffs(coefficients_msg->coefficients[0]);
166  }
167  }
168 
170  const pcl_msgs::ModelCoefficients& coefficients) {
171  boost::mutex::scoped_lock lock(mutex_);
172  for (int i = 0; i < 4; ++i) {
173  ground_coeffs_[i] = coefficients.values[i];
174  }
175  }
176 
178  const sensor_msgs::CameraInfo::ConstPtr& info_msg) {
179  boost::mutex::scoped_lock lock(mutex_);
180  latest_camera_info_ = info_msg;
181 
182  Eigen::Matrix3f rgb_intrinsics_matrix;
183  rgb_intrinsics_matrix << info_msg->K[0], info_msg->K[1], info_msg->K[2],
184  info_msg->K[3], info_msg->K[4], info_msg->K[5], info_msg->K[6],
185  info_msg->K[7], info_msg->K[8];
186  people_detector_.setIntrinsics(
187  rgb_intrinsics_matrix); // set RGB camera intrinsic parameters
188  }
189 }
190 
virtual void ground_coeffs_callback(const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients_msg)
void publish(const boost::shared_ptr< M > &message) const
void configCallback(Config &config, uint32_t level)
pcl::people::PersonClassifier< pcl::RGB > person_classifier_
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
sensor_msgs::CameraInfo::ConstPtr latest_camera_info_
virtual void set_ground_coeffs(const pcl_msgs::ModelCoefficients &coefficients)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::PeopleDetection, nodelet::Nodelet)
boost::shared_ptr< ros::NodeHandle > pnh_
pcl::people::GroundBasedPeopleDetectionApp< pcl::PointXYZRGBA > people_detector_
virtual void detect(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
#define NODELET_FATAL(...)
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
jsk_pcl_ros::PeopleDetectionConfig Config


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