people_detection_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 #include "jsk_pcl_ros/people_detection.h"
00038 #include "jsk_recognition_utils/pcl_conversion_util.h"
00039 #include "jsk_recognition_utils/pcl_util.h"
00040 
00041 #include <pcl/people/ground_based_people_detection_app.h>
00042 
00043 namespace jsk_pcl_ros {
00044   void PeopleDetection::onInit() {
00045     DiagnosticNodelet::onInit();
00046 
00047     pnh_->param("people_height_threshold", people_height_threshold_, 0.5);
00048     pnh_->param("min_confidence", min_confidence_, -1.5);
00049     pnh_->param("queue_size", queue_size_, 1);
00050     pnh_->param("voxel_size", voxel_size_, 0.03);
00051     pnh_->param("box_width", box_width_, 0.5);
00052     pnh_->param("box_depth", box_depth_, 0.5);
00053     pnh_->param<std::string>("filename", trained_filename_, "");
00054 
00055     if (trained_filename_ == "") {
00056       NODELET_FATAL("Please set svm file name");
00057     }
00058 
00059     person_classifier_.loadSVMFromFile(trained_filename_);  // load trained SVM
00060     people_detector_.setVoxelSize(voxel_size_);  // set the voxel size
00061 
00062     Eigen::Matrix3f rgb_intrinsics_matrix;
00063     rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0,
00064       1.0;  // Kinect RGB camera intrinsics
00065 
00066     people_detector_.setIntrinsics(
00067       rgb_intrinsics_matrix);  // set RGB camera intrinsic parameters
00068     people_detector_.setClassifier(
00069       person_classifier_);  // set person classifier
00070 
00071     ground_coeffs_.resize(4);
00072     ground_coeffs_[0] = 0.0;
00073     ground_coeffs_[1] = 0.0;
00074     ground_coeffs_[2] = 1.0;
00075     ground_coeffs_[3] = 0.0;
00076 
00078     // dynamic reconfigure
00080     srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
00081     dynamic_reconfigure::Server<Config>::CallbackType f =
00082       boost::bind(&PeopleDetection::configCallback, this, _1, _2);
00083     srv_->setCallback(f);
00084 
00086     // Publisher
00088     pub_box_ =
00089       advertise<jsk_recognition_msgs::BoundingBoxArray>(*pnh_, "boxes", 1);
00090 
00091     onInitPostProcess();
00092   }
00093 
00094   void PeopleDetection::configCallback(Config& config, uint32_t level) {
00095     boost::mutex::scoped_lock lock(mutex_);
00096     voxel_size_ = config.voxel_size;
00097     min_confidence_ = config.min_confidence;
00098     people_height_threshold_ = config.people_height_threshold;
00099     box_width_ = config.box_width;
00100     box_depth_ = config.box_depth;
00101 
00102     people_detector_.setVoxelSize(voxel_size_);  // set the voxel size
00103   }
00104 
00105   void PeopleDetection::subscribe() {
00106     sub_info_ =
00107       pnh_->subscribe("input/info", 1, &PeopleDetection::infoCallback, this);
00108     sub_cloud_ = pnh_->subscribe("input", 1, &PeopleDetection::detect, this);
00109     sub_coefficients_ = pnh_->subscribe(
00110       "input/coefficients", 1, &PeopleDetection::ground_coeffs_callback, this);
00111   }
00112 
00113   void PeopleDetection::unsubscribe() {
00114     sub_cloud_.shutdown();
00115     sub_coefficients_.shutdown();
00116     sub_info_.shutdown();
00117   }
00118 
00119   void PeopleDetection::detect(
00120     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
00121     boost::mutex::scoped_lock lock(mutex_);
00122     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr input_cloud(
00123       new pcl::PointCloud<pcl::PointXYZRGBA>);
00124     pcl::fromROSMsg(*cloud_msg, *input_cloud);
00125 
00126     std::vector<pcl::people::PersonCluster<pcl::PointXYZRGBA> >
00127       clusters;  // vector containing persons clusters
00128     people_detector_.setInputCloud(input_cloud);
00129     people_detector_.setGround(ground_coeffs_);  // set floor coefficients
00130     people_detector_.compute(clusters);          // perform people detection
00131 
00132     jsk_recognition_msgs::BoundingBoxArray bounding_box_array;
00133     bounding_box_array.header = cloud_msg->header;
00134     jsk_recognition_msgs::BoundingBox bounding_box;
00135     bounding_box.header = cloud_msg->header;
00136 
00137     for (std::vector<pcl::people::PersonCluster<pcl::PointXYZRGBA> >::iterator it =
00138              clusters.begin();
00139          it != clusters.end(); ++it) {
00140       if (it->getPersonConfidence() > min_confidence_ &&
00141           it->getHeight() > people_height_threshold_) {
00142         bounding_box.pose.position.x = it->getCenter()[0];
00143         bounding_box.pose.position.y = it->getCenter()[1] + it->getHeight() / 2;
00144         bounding_box.pose.position.z = it->getCenter()[2];
00145 
00146         bounding_box.pose.orientation.x = 0.0;
00147         bounding_box.pose.orientation.y = 0.0;
00148         bounding_box.pose.orientation.z = 0.0;
00149         bounding_box.pose.orientation.w = 1.0;
00150 
00151         bounding_box.dimensions.x = box_width_;
00152         bounding_box.dimensions.y = it->getHeight() + 0.3;
00153         bounding_box.dimensions.z = box_depth_;
00154 
00155         bounding_box_array.boxes.push_back(bounding_box);
00156       }
00157     }
00158     pub_box_.publish(bounding_box_array);
00159   }
00160 
00161   void PeopleDetection::ground_coeffs_callback(
00162     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr&
00163       coefficients_msg) {
00164     if (coefficients_msg->coefficients.size() >= 1) {
00165       set_ground_coeffs(coefficients_msg->coefficients[0]);
00166     }
00167   }
00168 
00169   void PeopleDetection::set_ground_coeffs(
00170     const pcl_msgs::ModelCoefficients& coefficients) {
00171     boost::mutex::scoped_lock lock(mutex_);
00172     for (int i = 0; i < 4; ++i) {
00173       ground_coeffs_[i] = coefficients.values[i];
00174     }
00175   }
00176 
00177   void PeopleDetection::infoCallback(
00178     const sensor_msgs::CameraInfo::ConstPtr& info_msg) {
00179     boost::mutex::scoped_lock lock(mutex_);
00180     latest_camera_info_ = info_msg;
00181 
00182     Eigen::Matrix3f rgb_intrinsics_matrix;
00183     rgb_intrinsics_matrix << info_msg->K[0], info_msg->K[1], info_msg->K[2],
00184       info_msg->K[3], info_msg->K[4], info_msg->K[5], info_msg->K[6],
00185       info_msg->K[7], info_msg->K[8];
00186     people_detector_.setIntrinsics(
00187       rgb_intrinsics_matrix);  // set RGB camera intrinsic parameters
00188   }
00189 }
00190 
00191 #include <pluginlib/class_list_macros.h>
00192 PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::PeopleDetection, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:44