template <typename PointT> Eigen::VectorXf open_ptrack::detection::GroundplaneEstimation<PointT>::computeFromTF (tf::Transform worldToCamTransform) { // Select 3 points in world reference frame: pcl::PointCloud<pcl::PointXYZ>::Ptr ground_points (new pcl::PointCloud<pcl::PointXYZ>); ground_points->points.push_back(pcl::PointXYZ(0.0, 0.0, 0.0)); ground_points->points.push_back(pcl::PointXYZ(1.0, 0.0, 0.0)); ground_points->points.push_back(pcl::PointXYZ(0.0, 1.0, 0.0)); // Transform points to camera reference frame: for (unsigned int i = 0; i < ground_points->points.size(); i++) { tf::Vector3 current_point(ground_points->points[i].x, ground_points->points[i].y, ground_points->points[i].z); current_point = worldToCamTransform(current_point); ground_points->points[i].x = current_point.x(); ground_points->points[i].y = current_point.y(); ground_points->points[i].z = current_point.z(); } // Compute ground equation: std::vector<int> ground_points_indices; for (unsigned int i = 0; i < ground_points->points.size(); i++) ground_points_indices.push_back(i); pcl::SampleConsensusModelPlane<pcl::PointXYZ> model_plane(ground_points); Eigen::VectorXf ground_coeffs; model_plane.computeModelCoefficients(ground_points_indices,ground_coeffs); std::cout << "Ground plane coefficients obtained from calibration: " << ground_coeffs(0) << ", " << ground_coeffs(1) << ", " << ground_coeffs(2) << ", " << ground_coeffs(3) << "." << std::endl; return ground_coeffs; }
template <typename PointT> Eigen::VectorXf open_ptrack::detection::GroundplaneEstimation<PointT>::computeFromTF (std::string camera_frame, std::string ground_frame) { // Select 3 points in world reference frame: pcl::PointCloud<pcl::PointXYZ>::Ptr ground_points (new pcl::PointCloud<pcl::PointXYZ>); ground_points->points.push_back(pcl::PointXYZ(0.0, 0.0, 0.0)); ground_points->points.push_back(pcl::PointXYZ(1.0, 0.0, 0.0)); ground_points->points.push_back(pcl::PointXYZ(0.0, 1.0, 0.0)); // Read transform between world and camera reference frame: tf::TransformListener tfListener; tf::StampedTransform worldToCamTransform; try { tfListener.waitForTransform(camera_frame, ground_frame, ros::Time(0), ros::Duration(3.0), ros::Duration(0.01)); tfListener.lookupTransform(camera_frame, ground_frame, ros::Time(0), worldToCamTransform); } catch (tf::TransformException ex) { ROS_ERROR("%s",ex.what()); } // Transform points to camera reference frame: for (unsigned int i = 0; i < ground_points->points.size(); i++) { tf::Vector3 current_point(ground_points->points[i].x, ground_points->points[i].y, ground_points->points[i].z); current_point = worldToCamTransform(current_point); ground_points->points[i].x = current_point.x(); ground_points->points[i].y = current_point.y(); ground_points->points[i].z = current_point.z(); } // Compute ground equation: std::vector<int> ground_points_indices; for (unsigned int i = 0; i < ground_points->points.size(); i++) ground_points_indices.push_back(i); pcl::SampleConsensusModelPlane<pcl::PointXYZ> model_plane(ground_points); Eigen::VectorXf ground_coeffs; model_plane.computeModelCoefficients(ground_points_indices,ground_coeffs); std::cout << "Ground plane coefficients obtained from calibration: " << ground_coeffs(0) << ", " << ground_coeffs(1) << ", " << ground_coeffs(2) << ", " << ground_coeffs(3) << "." << std::endl; return ground_coeffs; }
int main (int argc, char** argv) { if(pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h")) return print_help(); // Algorithm parameters: std::string svm_filename = "../../people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml"; float min_confidence = -1.5; float min_height = 1.3; float max_height = 2.3; float voxel_size = 0.06; Eigen::Matrix3f rgb_intrinsics_matrix; rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics // Read if some parameters are passed from command line: pcl::console::parse_argument (argc, argv, "--svm", svm_filename); pcl::console::parse_argument (argc, argv, "--conf", min_confidence); pcl::console::parse_argument (argc, argv, "--min_h", min_height); pcl::console::parse_argument (argc, argv, "--max_h", max_height); // Read Kinect live stream: PointCloudT::Ptr cloud (new PointCloudT); bool new_cloud_available_flag = false; pcl::Grabber* interface = new pcl::OpenNIGrabber(); boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind (&cloud_cb_, _1, cloud, &new_cloud_available_flag); interface->registerCallback (f); interface->start (); // Wait for the first frame: while(!new_cloud_available_flag) boost::this_thread::sleep(boost::posix_time::milliseconds(1)); new_cloud_available_flag = false; cloud_mutex.lock (); // for not overwriting the point cloud // Display pointcloud: pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud"); viewer.setCameraPosition(0,0,-2,0,-1,0,0); // Add point picking callback to viewer: struct callback_args cb_args; PointCloudT::Ptr clicked_points_3d (new PointCloudT); cb_args.clicked_points_3d = clicked_points_3d; cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer); viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args); std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl; // Spin until 'Q' is pressed: viewer.spin(); std::cout << "done." << std::endl; cloud_mutex.unlock (); // Ground plane estimation: Eigen::VectorXf ground_coeffs; ground_coeffs.resize(4); std::vector<int> clicked_points_indices; for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++) clicked_points_indices.push_back(i); pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d); model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs); std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl; // Initialize new viewer: pcl::visualization::PCLVisualizer viewer("PCL Viewer"); // viewer initialization viewer.setCameraPosition(0,0,-2,0,-1,0,0); // Create classifier for people detection: pcl::people::PersonClassifier<pcl::RGB> person_classifier; person_classifier.loadSVMFromFile(svm_filename); // load trained SVM // People detection app initialization: pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector; // people detection object people_detector.setVoxelSize(voxel_size); // set the voxel size people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters people_detector.setClassifier(person_classifier); // set person classifier people_detector.setHeightLimits(min_height, max_height); // set person classifier // people_detector.setSensorPortraitOrientation(true); // set sensor orientation to vertical // For timing: static unsigned count = 0; static double last = pcl::getTime (); // Main loop: while (!viewer.wasStopped()) { if (new_cloud_available_flag && cloud_mutex.try_lock ()) // if a new cloud is available { new_cloud_available_flag = false; // Perform people detection on the new cloud: std::vector<pcl::people::PersonCluster<PointT> > clusters; // vector containing persons clusters people_detector.setInputCloud(cloud); people_detector.setGround(ground_coeffs); // set floor coefficients people_detector.compute(clusters); // perform people detection ground_coeffs = people_detector.getGround(); // get updated floor coefficients // Draw cloud and people bounding boxes in the viewer: viewer.removeAllPointClouds(); viewer.removeAllShapes(); pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud"); unsigned int k = 0; for(std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it) { if(it->getPersonConfidence() > min_confidence) // draw only people with confidence above a threshold { // draw theoretical person bounding box in the PCL viewer: it->drawTBoundingBox(viewer, k); k++; } } std::cout << k << " people found" << std::endl; viewer.spinOnce(); // Display average framerate: if (++count == 30) { double now = pcl::getTime (); std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" << std::endl; count = 0; last = now; } cloud_mutex.unlock (); } } return 0; }
template <typename PointT> Eigen::VectorXf open_ptrack::detection::GroundplaneEstimation<PointT>::compute () { Eigen::VectorXf ground_coeffs; ground_coeffs.resize(4); // Manual mode: if (ground_estimation_mode_ == 0) { std::cout << "Manual mode for ground plane estimation." << std::endl; // Initialize viewer: pcl::visualization::PCLVisualizer viewer("Pick 3 points"); // Create XYZ cloud: pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointXYZRGB xyzrgb_point; cloud_xyzrgb->points.resize(cloud_->width * cloud_->height, xyzrgb_point); cloud_xyzrgb->width = cloud_->width; cloud_xyzrgb->height = cloud_->height; cloud_xyzrgb->is_dense = false; for (int i=0; i<cloud_->height; i++) { for (int j=0; j<cloud_->width; j++) { cloud_xyzrgb->at(j,i).x = cloud_->at(j,i).x; cloud_xyzrgb->at(j,i).y = cloud_->at(j,i).y; cloud_xyzrgb->at(j,i).z = cloud_->at(j,i).z; } } //#if (XYZRGB_CLOUDS) // pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_); // viewer.addPointCloud<pcl::PointXYZRGB> (cloud_, rgb, "input_cloud"); //#else // viewer.addPointCloud<pcl::PointXYZ> (cloud_, "input_cloud"); //#endif pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> rgb(cloud_xyzrgb, 255, 255, 255); viewer.addPointCloud<pcl::PointXYZRGB> (cloud_xyzrgb, rgb, "input_cloud"); viewer.setCameraPosition(0,0,-2,0,-1,0,0); // Add point picking callback to viewer: struct callback_args_color cb_args; //#if (XYZRGB_CLOUDS) // PointCloudPtr clicked_points_3d (new PointCloud); //#else // pcl::PointCloud<pcl::PointXYZ>::Ptr clicked_points_3d (new pcl::PointCloud<pcl::PointXYZ>); //#endif pcl::PointCloud<pcl::PointXYZRGB>::Ptr clicked_points_3d (new pcl::PointCloud<pcl::PointXYZRGB>); cb_args.clicked_points_3d = clicked_points_3d; cb_args.viewerPtr = &viewer; viewer.registerPointPickingCallback (GroundplaneEstimation::pp_callback, (void*)&cb_args); // Spin until 'Q' is pressed: viewer.spin(); viewer.setSize(1,1); // resize viewer in order to make it disappear viewer.spinOnce(); viewer.close(); // close method does not work std::cout << "done." << std::endl; // Keep only the last three clicked points: while(clicked_points_3d->points.size()>3) { clicked_points_3d->points.erase(clicked_points_3d->points.begin()); } // Ground plane estimation: std::vector<int> clicked_points_indices; for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++) clicked_points_indices.push_back(i); // pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d); pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> model_plane(clicked_points_3d); model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs); std::cout << "Ground plane coefficients: " << ground_coeffs(0) << ", " << ground_coeffs(1) << ", " << ground_coeffs(2) << ", " << ground_coeffs(3) << "." << std::endl; } // Semi-automatic mode: if (ground_estimation_mode_ == 1) { std::cout << "Semi-automatic mode for ground plane estimation." << std::endl; // Normals computation: pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setMaxDepthChangeFactor (0.03f); ne.setNormalSmoothingSize (20.0f); pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>); ne.setInputCloud (cloud_); ne.compute (*normal_cloud); // Multi plane segmentation initialization: std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers (500); mps.setAngularThreshold (2.0 * M_PI / 180); mps.setDistanceThreshold (0.2); mps.setInputNormals (normal_cloud); mps.setInputCloud (cloud_); mps.segmentAndRefine (regions); std::cout << "Found " << regions.size() << " planar regions." << std::endl; // Color planar regions with different colors: pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud (new pcl::PointCloud<pcl::PointXYZRGB>); colored_cloud = colorRegions(regions); if (regions.size()>0) { // Viewer initialization: pcl::visualization::PCLVisualizer viewer("PCL Viewer"); pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(colored_cloud); viewer.addPointCloud<pcl::PointXYZRGB> (colored_cloud, rgb, "input_cloud"); viewer.setCameraPosition(0,0,-2,0,-1,0,0); // Add point picking callback to viewer: struct callback_args_color cb_args; typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr clicked_points_3d (new pcl::PointCloud<pcl::PointXYZRGB>); cb_args.clicked_points_3d = clicked_points_3d; cb_args.viewerPtr = &viewer; viewer.registerPointPickingCallback (GroundplaneEstimation::pp_callback, (void*)&cb_args); std::cout << "Shift+click on a floor point, then press 'Q'..." << std::endl; // Spin until 'Q' is pressed: viewer.spin(); viewer.setSize(1,1); // resize viewer in order to make it disappear viewer.spinOnce(); viewer.close(); // close method does not work std::cout << "done." << std::endl; // Find plane closest to clicked point: unsigned int index = 0; float min_distance = FLT_MAX; float distance; float X = cb_args.clicked_points_3d->points[clicked_points_3d->points.size() - 1].x; float Y = cb_args.clicked_points_3d->points[clicked_points_3d->points.size() - 1].y; float Z = cb_args.clicked_points_3d->points[clicked_points_3d->points.size() - 1].z; for(unsigned int i = 0; i < regions.size(); i++) { float a = regions[i].getCoefficients()[0]; float b = regions[i].getCoefficients()[1]; float c = regions[i].getCoefficients()[2]; float d = regions[i].getCoefficients()[3]; distance = (float) (fabs((a*X + b*Y + c*Z + d)))/(sqrtf(a*a+b*b+c*c)); if(distance < min_distance) { min_distance = distance; index = i; } } ground_coeffs[0] = regions[index].getCoefficients()[0]; ground_coeffs[1] = regions[index].getCoefficients()[1]; ground_coeffs[2] = regions[index].getCoefficients()[2]; ground_coeffs[3] = regions[index].getCoefficients()[3]; std::cout << "Ground plane coefficients: " << regions[index].getCoefficients()[0] << ", " << regions[index].getCoefficients()[1] << ", " << regions[index].getCoefficients()[2] << ", " << regions[index].getCoefficients()[3] << "." << std::endl; } } // Automatic mode: if ((ground_estimation_mode_ == 2) || (ground_estimation_mode_ == 3)) { std::cout << "Automatic mode for ground plane estimation." << std::endl; // Normals computation: // pcl::NormalEstimation<PointT, pcl::Normal> ne; //// ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); //// ne.setMaxDepthChangeFactor (0.03f); //// ne.setNormalSmoothingSize (20.0f); // pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>); // pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ()); // ne.setSearchMethod (tree); // ne.setRadiusSearch (0.2); // ne.setInputCloud (cloud_); // ne.compute (*normal_cloud); pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setMaxDepthChangeFactor (0.03f); ne.setNormalSmoothingSize (20.0f); pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>); ne.setInputCloud (cloud_); ne.compute (*normal_cloud); // std::cout << "Normals estimated!" << std::endl; // // // Multi plane segmentation initialization: // std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; // pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; // mps.setMinInliers (500); // mps.setAngularThreshold (2.0 * M_PI / 180); // mps.setDistanceThreshold (0.2); // mps.setInputNormals (normal_cloud); // mps.setInputCloud (cloud_); // mps.segmentAndRefine (regions); // Multi plane segmentation initialization: std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers (500); mps.setAngularThreshold (2.0 * M_PI / 180); mps.setDistanceThreshold (0.2); mps.setInputNormals (normal_cloud); mps.setInputCloud (cloud_); mps.segmentAndRefine (regions); // std::cout << "Found " << regions.size() << " planar regions." << std::endl; // Removing planes not compatible with camera roll ~= 0: unsigned int i = 0; while(i < regions.size()) { // Check on the normal to the plane: if(fabs(regions[i].getCoefficients()[1]) < 0.70) { regions.erase(regions.begin()+i); } else ++i; } // Order planar regions according to height (y coordinate): std::sort(regions.begin(), regions.end(), GroundplaneEstimation::planeHeightComparator); // Color selected planar region in red: pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud (new pcl::PointCloud<pcl::PointXYZRGB>); colored_cloud = colorRegions(regions, 0); // If at least a valid plane remained: if (regions.size()>0) { ground_coeffs[0] = regions[0].getCoefficients()[0]; ground_coeffs[1] = regions[0].getCoefficients()[1]; ground_coeffs[2] = regions[0].getCoefficients()[2]; ground_coeffs[3] = regions[0].getCoefficients()[3]; std::cout << "Ground plane coefficients: " << regions[0].getCoefficients()[0] << ", " << regions[0].getCoefficients()[1] << ", " << regions[0].getCoefficients()[2] << ", " << regions[0].getCoefficients()[3] << "." << std::endl; // Result visualization: if (ground_estimation_mode_ == 2) { // Viewer initialization: pcl::visualization::PCLVisualizer viewer("PCL Viewer"); pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(colored_cloud); viewer.addPointCloud<pcl::PointXYZRGB> (colored_cloud, rgb, "input_cloud"); viewer.setCameraPosition(0,0,-2,0,-1,0,0); // Spin until 'Q' is pressed: viewer.spin(); viewer.setSize(1,1); // resize viewer in order to make it disappear viewer.spinOnce(); viewer.close(); // close method does not work } } else { std::cout << "ERROR: no valid ground plane found!" << std::endl; } } return ground_coeffs; }
int main (int argc, char** argv) { //ROS Initialization ros::init(argc, argv, "detecting_people"); ros::NodeHandle nh; ros::Rate rate(13); ros::Subscriber state_sub = nh.subscribe("followme_state", 5, &stateCallback); ros::Publisher people_pub = nh.advertise<frmsg::people>("followme_people", 5); frmsg::people pub_people_; CloudConverter* cc_ = new CloudConverter(); while (!cc_->ready_xyzrgb_) { ros::spinOnce(); rate.sleep(); if (!ros::ok()) { printf("Terminated by Control-c.\n"); return -1; } } // Input parameter from the .yaml std::string package_path_ = ros::package::getPath("detecting_people") + "/"; cv::FileStorage* fs_ = new cv::FileStorage(package_path_ + "parameters.yml", cv::FileStorage::READ); // Algorithm parameters: std::string svm_filename = package_path_ + "trainedLinearSVMForPeopleDetectionWithHOG.yaml"; std::cout << svm_filename << std::endl; float min_confidence = -1.5; float min_height = 1.3; float max_height = 2.3; float voxel_size = 0.06; Eigen::Matrix3f rgb_intrinsics_matrix; rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics // Read if some parameters are passed from command line: pcl::console::parse_argument (argc, argv, "--svm", svm_filename); pcl::console::parse_argument (argc, argv, "--conf", min_confidence); pcl::console::parse_argument (argc, argv, "--min_h", min_height); pcl::console::parse_argument (argc, argv, "--max_h", max_height); // Read Kinect live stream: PointCloudT::Ptr cloud_people (new PointCloudT); cc_->ready_xyzrgb_ = false; while ( !cc_->ready_xyzrgb_ ) { ros::spinOnce(); rate.sleep(); } pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud = cc_->msg_xyzrgb_; // Display pointcloud: pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud"); viewer.setCameraPosition(0,0,-2,0,-1,0,0); // Add point picking callback to viewer: struct callback_args cb_args; PointCloudT::Ptr clicked_points_3d (new PointCloudT); cb_args.clicked_points_3d = clicked_points_3d; cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer); viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args); std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl; // Spin until 'Q' is pressed: viewer.spin(); std::cout << "done." << std::endl; //cloud_mutex.unlock (); // Ground plane estimation: Eigen::VectorXf ground_coeffs; ground_coeffs.resize(4); std::vector<int> clicked_points_indices; for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++) clicked_points_indices.push_back(i); pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d); model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs); std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl; // Initialize new viewer: pcl::visualization::PCLVisualizer viewer("PCL Viewer"); // viewer initialization viewer.setCameraPosition(0,0,-2,0,-1,0,0); // Create classifier for people detection: pcl::people::PersonClassifier<pcl::RGB> person_classifier; person_classifier.loadSVMFromFile(svm_filename); // load trained SVM // People detection app initialization: pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector; // people detection object people_detector.setVoxelSize(voxel_size); // set the voxel size people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters people_detector.setClassifier(person_classifier); // set person classifier people_detector.setHeightLimits(min_height, max_height); // set person classifier // people_detector.setSensorPortraitOrientation(true); // set sensor orientation to vertical // For timing: static unsigned count = 0; static double last = pcl::getTime (); int people_count = 0; histogram* first_hist; int max_people_num = (int)fs_->getFirstTopLevelNode()["max_people_num"]; // Main loop: while (!viewer.wasStopped() && ros::ok() ) { if ( current_state == 1 ) { if ( cc_->ready_xyzrgb_ ) // if a new cloud is available { // std::cout << "In state 1!!!!!!!!!!" << std::endl; std::vector<float> x; std::vector<float> y; std::vector<float> depth; cloud = cc_->msg_xyzrgb_; PointCloudT::Ptr cloud_new(new PointCloudT(*cloud)); cc_->ready_xyzrgb_ = false; // Perform people detection on the new cloud: std::vector<pcl::people::PersonCluster<PointT> > clusters; // vector containing persons clusters people_detector.setInputCloud(cloud_new); people_detector.setGround(ground_coeffs); // set floor coefficients people_detector.compute(clusters); // perform people detection ground_coeffs = people_detector.getGround(); // get updated floor coefficients // Draw cloud and people bounding boxes in the viewer: viewer.removeAllPointClouds(); viewer.removeAllShapes(); pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud"); unsigned int k = 0; std::vector<pcl::people::PersonCluster<PointT> >::iterator it; std::vector<pcl::people::PersonCluster<PointT> >::iterator it_min; float min_z = 10.0; float histo_dist_min = 2.0; int id = -1; for(it = clusters.begin(); it != clusters.end(); ++it) { if(it->getPersonConfidence() > min_confidence) // draw only people with confidence above a threshold { x.push_back((it->getTCenter())[0]); y.push_back((it->getTCenter())[1]); depth.push_back(it->getDistance()); // draw theoretical person bounding box in the PCL viewer: /*pcl::copyPointCloud( *cloud, it->getIndices(), *cloud_people); if ( people_count == 0 ) { first_hist = calc_histogram_a( cloud_people ); people_count++; it->drawTBoundingBox(viewer, k); } else if ( people_count <= 10 ) { histogram* hist_tmp = calc_histogram_a( cloud_people ); add_hist( first_hist, hist_tmp ); it->drawTBoundingBox(viewer, k); people_count++; }*/ pcl::copyPointCloud( *cloud, it->getIndices(), *cloud_people); if ( people_count < 11 ) { if ( it->getDistance() < min_z ) { it_min = it; min_z = it->getDistance(); } } else if ( people_count == 11 ) { normalize_histogram( first_hist ); people_count++; histogram* hist_tmp = calc_histogram( cloud_people ); float tmp = histo_dist_sq( first_hist, hist_tmp ); std::cout << "The histogram distance is " << tmp << std::endl; histo_dist_min = tmp; it_min = it; id = k; } else { histogram* hist_tmp = calc_histogram( cloud_people ); float tmp = histo_dist_sq( first_hist, hist_tmp ); std::cout << "The histogram distance is " << tmp << std::endl; if ( tmp < histo_dist_min ) { histo_dist_min = tmp; it_min = it; id = k; } } k++; //std::cout << "The data of the center is " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].x << " " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].y << " " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].z << std::endl; //std::cout << "The size of the people cloud is " << cloud_people->points.size() << std::endl; std::cout << "The " << k << " person's distance is " << it->getDistance() << std::endl; } } pub_people_.x = x; pub_people_.y = y; pub_people_.depth = depth; if ( k > 0 ) { if ( people_count <= 11 ) { pcl::copyPointCloud( *cloud, it_min->getIndices(), *cloud_people); if ( people_count == 0) { first_hist = calc_histogram_a( cloud_people ); people_count++; it_min->drawTBoundingBox(viewer, 1); } else if ( people_count < 11 ) { histogram* hist_tmp = calc_histogram_a( cloud_people ); add_hist( first_hist, hist_tmp ); it_min->drawTBoundingBox(viewer, 1); people_count++; } } else { pub_people_.id = k-1; if ( histo_dist_min < 1.3 ) { it_min->drawTBoundingBox(viewer, 1); std::cout << "The minimum distance of the histogram is " << histo_dist_min << std::endl; std::cout << "The vector is " << it_min->getTCenter() << std::endl << "while the elements are " << (it_min->getTCenter())[0] << " " << (it_min->getTCenter())[1] << std::endl; } else { pub_people_.id = -1; } } } else { pub_people_.id = -1; } pub_people_.header.stamp = ros::Time::now(); people_pub.publish(pub_people_); std::cout << k << " people found" << std::endl; viewer.spinOnce(); ros::spinOnce(); // Display average framerate: if (++count == 30) { double now = pcl::getTime (); std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" << std::endl; count = 0; last = now; } //cloud_mutex.unlock (); } } else { viewer.spinOnce(); ros::spinOnce(); // std::cout << "In state 0!!!!!!!!!" << std::endl; } } return 0; }