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;
}
