36 #define BOOST_PARAMETER_MAX_ARITY 7 41 #include <pcl/people/ground_based_people_detection_app.h> 45 DiagnosticNodelet::onInit();
55 if (trained_filename_ ==
"") {
62 Eigen::Matrix3f rgb_intrinsics_matrix;
63 rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0,
67 rgb_intrinsics_matrix);
80 srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
81 dynamic_reconfigure::Server<Config>::CallbackType
f =
89 advertise<jsk_recognition_msgs::BoundingBoxArray>(*
pnh_,
"boxes", 1);
120 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
122 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr input_cloud(
123 new pcl::PointCloud<pcl::PointXYZRGBA>);
126 std::vector<pcl::people::PersonCluster<pcl::PointXYZRGBA> >
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;
137 for (std::vector<pcl::people::PersonCluster<pcl::PointXYZRGBA> >::iterator it =
139 it != clusters.end(); ++it) {
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];
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;
152 bounding_box.dimensions.y = it->getHeight() + 0.3;
155 bounding_box_array.boxes.push_back(bounding_box);
162 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr&
164 if (coefficients_msg->coefficients.size() >= 1) {
170 const pcl_msgs::ModelCoefficients& coefficients) {
172 for (
int i = 0; i < 4; ++i) {
178 const sensor_msgs::CameraInfo::ConstPtr& info_msg) {
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];
187 rgb_intrinsics_matrix);
virtual void ground_coeffs_callback(const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients_msg)
void publish(const boost::shared_ptr< M > &message) const
ros::Subscriber sub_cloud_
void configCallback(Config &config, uint32_t level)
std::string trained_filename_
pcl::people::PersonClassifier< pcl::RGB > person_classifier_
Eigen::VectorXf ground_coeffs_
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)
double people_height_threshold_
ros::Subscriber sub_info_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void unsubscribe()
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::PeopleDetection, nodelet::Nodelet)
pcl::people::GroundBasedPeopleDetectionApp< pcl::PointXYZRGBA > people_detector_
virtual void detect(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
#define NODELET_FATAL(...)
ros::Subscriber sub_coefficients_
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
jsk_pcl_ros::PeopleDetectionConfig Config