/**
* @function downsampling
*/
pcl::PointCloud<PointT>::Ptr downsampling( const pcl::PointCloud<PointT>::Ptr &_input,
const double &_voxelSize ) {
pcl::PointCloud<PointT>::Ptr cloud_downsampled( new pcl::PointCloud<PointT>() );
// Create the filtering object
pcl::VoxelGrid< PointT > downsampler;
// Set input cloud
downsampler.setInputCloud( _input );
// Set size of voxel
downsampler.setLeafSize( _voxelSize, _voxelSize, _voxelSize );
// Downsample
downsampler.filter( *cloud_downsampled );
return cloud_downsampled;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr ObjectDetector::downsampleCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ>);
ROS_INFO("Before voxel filtering: %d", cloud->width * cloud->height);
// Perform the voxel downsampling
pcl::VoxelGrid<pcl::PointXYZ> vox_grid;
vox_grid.setInputCloud (cloud);
vox_grid.setLeafSize (voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
vox_grid.filter (*cloud_downsampled);
ROS_INFO("After voxel filtering: %d", cloud_downsampled->width * cloud_downsampled->height);
return cloud_downsampled;
}
// this function gets called every time new pcl data comes in
void cloudcb(const sensor_msgs::PointCloud2ConstPtr &scan)
{
ROS_INFO("Kinect point cloud receieved");
ros::Time start_time = ros::Time::now();
ros::Time tcur = ros::Time::now();
Eigen::Vector4f centroid;
geometry_msgs::Point point;
pcl::PassThrough<pcl::PointXYZ> pass;
Eigen::VectorXf lims(6);
sensor_msgs::PointCloud2::Ptr
ros_cloud (new sensor_msgs::PointCloud2 ()),
ros_cloud_filtered (new sensor_msgs::PointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
// set a parameter telling the world that I am tracking the robot
ros::param::set("/tracking_robot", true);
ROS_DEBUG("finished declaring vars : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ROS_DEBUG("About to transform cloud");
// New sensor message for holding the transformed data
sensor_msgs::PointCloud2::Ptr scan_transformed
(new sensor_msgs::PointCloud2());
try{
pcl_ros::transformPointCloud("/oriented_optimization_frame",
tf, *scan, *scan_transformed);
}
catch(tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
}
scan_transformed->header.frame_id = "/oriented_optimization_frame";
// Convert to pcl
ROS_DEBUG("Convert cloud to pcd from rosmsg");
pcl::fromROSMsg(*scan_transformed, *cloud);
ROS_DEBUG("cloud transformed and converted to pcl : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// set time stamp and frame id
ros::Time tstamp = ros::Time::now();
// run through pass-through filter to eliminate tarp and below robots.
ROS_DEBUG("Pass-through filter");
pass_through(cloud, cloud_filtered, robot_limits);
ROS_DEBUG("done with pass-through : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// now let's publish that filtered cloud
ROS_DEBUG("Converting and publishing cloud");
pcl::toROSMsg(*cloud_filtered, *ros_cloud_filtered);
ros_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
cloud_pub[0].publish(ros_cloud_filtered);
ROS_DEBUG("published filtered cloud : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// Let's do a downsampling before doing cluster extraction
pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setInputCloud (cloud_filtered);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_downsampled);
ROS_DEBUG("finished downsampling : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ROS_DEBUG("Begin extraction filtering");
// build a KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree
(new pcl::search::KdTree<pcl::PointXYZ> ());
tree->setInputCloud (cloud_downsampled);
ROS_DEBUG("done with KdTree initialization : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// create a vector for storing the indices of the clusters
std::vector<pcl::PointIndices> cluster_indices;
// setup extraction:
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // cm
ec.setMinClusterSize (50);
ec.setMaxClusterSize (3000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_downsampled);
// now we can perform cluster extraction
ec.extract (cluster_indices);
ROS_DEBUG("finished extraction : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// run through the indices, create new clouds, and then publish them
int j=1;
int number_clusters=0;
geometry_msgs::PointStamped pt;
puppeteer_msgs::Robots robots;
std::vector<int> num;
for (std::vector<pcl::PointIndices>::const_iterator
it=cluster_indices.begin();
it!=cluster_indices.end (); ++it)
{
number_clusters = (int) cluster_indices.size();
ROS_DEBUG("Number of clusters found: %d",number_clusters);
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator
pit = it->indices.begin ();
pit != it->indices.end (); pit++)
{
cloud_cluster->points.push_back(cloud_downsampled->points[*pit]);
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// convert to rosmsg and publish:
ROS_DEBUG("Publishing extracted cloud");
pcl::toROSMsg(*cloud_cluster, *ros_cloud_filtered);
ros_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
if(j < MAX_CLUSTERS+1)
cloud_pub[j].publish(ros_cloud_filtered);
j++;
// compute centroid and add to Robots:
pcl::compute3DCentroid(*cloud_cluster, centroid);
pt.point.x = centroid(0);
pt.point.y = centroid(1);
pt.point.z = centroid(2);
pt.header.stamp = tstamp;
pt.header.frame_id = "/oriented_optimization_frame";
robots.robots.push_back(pt);
// add number of points in cluster to num:
num.push_back(cloud_cluster->points.size());
}
ROS_DEBUG("finished creating and publishing clusters : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
if (number_clusters > number_robots)
{
ROS_WARN("Number of clusters found is greater "
"than the number of robots");
// pop minimum cluster count
remove_least_likely(&robots, &num);
}
robots.header.stamp = tstamp;
robots.header.frame_id = "/oriented_optimization_frame";
robots.number = number_clusters;
robots_pub.publish(robots);
ROS_DEBUG("removed extra clusters, and published : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ros::Duration d = ros::Time::now() - start_time;
ROS_DEBUG("End of cloudcb; time elapsed = %f (%f Hz)",
d.toSec(), 1/d.toSec());
}
template <typename PointT> bool
pcl::people::GroundBasedPeopleDetectionApp<PointT>::compute (std::vector<pcl::people::PersonCluster<PointT> >& clusters)
{
// Check if all mandatory variables have been set:
if (!ground_coeffs_set_)
{
PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n");
return (false);
}
if (cloud_ == NULL)
{
PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n");
return (false);
}
if (!intrinsics_matrix_set_)
{
PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n");
return (false);
}
if (!person_classifier_set_flag_)
{
PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n");
return (false);
}
// Fill rgb image:
rgb_image_->points.clear(); // clear RGB pointcloud
extractRGBFromPointCloud(cloud_, rgb_image_); // fill RGB pointcloud
// Downsample of sampling_factor in every dimension:
if (sampling_factor_ != 1)
{
PointCloudPtr cloud_downsampled(new PointCloud);
cloud_downsampled->width = (cloud_->width)/sampling_factor_;
cloud_downsampled->height = (cloud_->height)/sampling_factor_;
cloud_downsampled->points.resize(cloud_downsampled->height*cloud_downsampled->width);
cloud_downsampled->is_dense = cloud_->is_dense;
for (uint32_t j = 0; j < cloud_downsampled->width; j++)
{
for (uint32_t i = 0; i < cloud_downsampled->height; i++)
{
(*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i);
}
}
(*cloud_) = (*cloud_downsampled);
}
applyTransformationPointCloud();
filter();
// Ground removal and update:
pcl::IndicesPtr inliers(new std::vector<int>);
boost::shared_ptr<pcl::SampleConsensusModelPlane<PointT> > ground_model(new pcl::SampleConsensusModelPlane<PointT>(cloud_filtered_));
ground_model->selectWithinDistance(ground_coeffs_transformed_, 2 * voxel_size_, *inliers);
no_ground_cloud_ = PointCloudPtr (new PointCloud);
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(cloud_filtered_);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*no_ground_cloud_);
if (inliers->size () >= (300 * 0.06 / voxel_size_ / std::pow (static_cast<double> (sampling_factor_), 2)))
ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_transformed_, ground_coeffs_transformed_);
else
PCL_INFO ("No groundplane update!\n");
// Euclidean Clustering:
std::vector<pcl::PointIndices> cluster_indices;
typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud(no_ground_cloud_);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance(2 * voxel_size_);
ec.setMinClusterSize(min_points_);
ec.setMaxClusterSize(max_points_);
ec.setSearchMethod(tree);
ec.setInputCloud(no_ground_cloud_);
ec.extract(cluster_indices);
// Head based sub-clustering //
pcl::people::HeadBasedSubclustering<PointT> subclustering;
subclustering.setInputCloud(no_ground_cloud_);
subclustering.setGround(ground_coeffs_transformed_);
subclustering.setInitialClusters(cluster_indices);
subclustering.setHeightLimits(min_height_, max_height_);
subclustering.setMinimumDistanceBetweenHeads(heads_minimum_distance_);
subclustering.setSensorPortraitOrientation(vertical_);
subclustering.subcluster(clusters);
// Person confidence evaluation with HOG+SVM:
if (vertical_) // Rotate the image if the camera is vertical
{
swapDimensions(rgb_image_);
}
for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
{
//Evaluate confidence for the current PersonCluster:
Eigen::Vector3f centroid = intrinsics_matrix_transformed_ * (it->getTCenter());
centroid /= centroid(2);
Eigen::Vector3f top = intrinsics_matrix_transformed_ * (it->getTTop());
top /= top(2);
Eigen::Vector3f bottom = intrinsics_matrix_transformed_ * (it->getTBottom());
bottom /= bottom(2);
it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_));
}
return (true);
}
void PlaneSegmentationPclRgbAlgorithm::getBiggestPlaneInliersDownsampling(pcl::PointIndices::Ptr inliers,
pcl::ModelCoefficients::Ptr coefficients,
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_seg (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr ground_hull (new pcl::PointCloud<pcl::PointXYZ>);
// downsampling
pcl::VoxelGrid<pcl::PointXYZ> grid_objects_;
grid_objects_.setLeafSize (pointcloud_downsample_size, pointcloud_downsample_size, pointcloud_downsample_size);
grid_objects_.setDownsampleAllData (false);
grid_objects_.setInputCloud (cloud);
grid_objects_.filter (*cloud_downsampled);
// segment plane
if (choose_plane_by_distance)
getNearestBigPlaneInliers(inliers, coefficients, cloud_downsampled);
else
getBiggestPlaneInliers(inliers, coefficients, cloud_downsampled);
// check if the plane exists
if (inliers->indices.size() == 0) {
// if plane doesn't exist a black image will be returned
ROS_WARN_STREAM("Plane segmentation: couldn't find plane.");
return;
}
// copy inliers
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(cloud_downsampled);
proj.setModelCoefficients(coefficients);
proj.setIndices (inliers);
proj.filter(*cloud_seg);
// remove NaN
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud_seg);
pass.filter (*cloud_seg);
// get biggest cluster
if (plane_clustering)
cloud_seg = getBiggestClusterPC(cloud_seg);
// Create a Convex Hull representation of the projected inliers
pcl::ConvexHull<pcl::PointXYZ> chull;
chull.setInputCloud(cloud_seg);
chull.setDimension(2);
chull.reconstruct(*ground_hull);
// Extract only those outliers that lie inside the ground plane's convex hull
pcl::PointIndices::Ptr object_indices (new pcl::PointIndices);
pcl::ExtractPolygonalPrismData<pcl::PointXYZ> hull_limiter;
hull_limiter.setInputCloud(cloud);
hull_limiter.setInputPlanarHull(ground_hull);
hull_limiter.setHeightLimits(plane_min_height, plane_max_height);
hull_limiter.segment(*object_indices);
*inliers = *object_indices;
}
template <typename PointT> bool
open_ptrack::detection::GroundBasedPeopleDetectionApp<PointT>::compute (std::vector<pcl::people::PersonCluster<PointT> >& clusters)
{
frame_counter_++;
// Define if debug info should be written or not for this frame:
bool debug_flag = false;
if ((frame_counter_ % 60) == 0)
{
debug_flag = true;
}
// Check if all mandatory variables have been set:
if (debug_flag)
{
if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_)
{
PCL_ERROR ("[open_ptrack::detection::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n");
return (false);
}
if (cloud_ == NULL)
{
PCL_ERROR ("[open_ptrack::detection::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n");
return (false);
}
if (intrinsics_matrix_(0) == 0)
{
PCL_ERROR ("[open_ptrack::detection::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n");
return (false);
}
if (!person_classifier_set_flag_)
{
PCL_ERROR ("[open_ptrack::detection::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n");
return (false);
}
}
if (!dimension_limits_set_) // if dimension limits have not been set by the user
{
// Adapt thresholds for clusters points number to the voxel size:
max_points_ = int(float(max_points_) * std::pow(0.06/voxel_size_, 2));
if (voxel_size_ > 0.06)
min_points_ = int(float(min_points_) * std::pow(0.06/voxel_size_, 2));
}
// Fill rgb image:
rgb_image_->points.clear(); // clear RGB pointcloud
extractRGBFromPointCloud(cloud_, rgb_image_); // fill RGB pointcloud
// Downsample of sampling_factor in every dimension:
if (sampling_factor_ != 1)
{
PointCloudPtr cloud_downsampled(new PointCloud);
cloud_downsampled->width = (cloud_->width)/sampling_factor_;
cloud_downsampled->height = (cloud_->height)/sampling_factor_;
cloud_downsampled->points.resize(cloud_downsampled->height*cloud_downsampled->width);
cloud_downsampled->is_dense = cloud_->is_dense;
cloud_downsampled->header = cloud_->header;
for (int j = 0; j < cloud_downsampled->width; j++)
{
for (int i = 0; i < cloud_downsampled->height; i++)
{
(*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i);
}
}
(*cloud_) = (*cloud_downsampled);
}
if (use_rgb_)
{
// Compute mean luminance:
int n_points = cloud_->points.size();
double sumR, sumG, sumB = 0.0;
for (int j = 0; j < cloud_->width; j++)
{
for (int i = 0; i < cloud_->height; i++)
{
sumR += (*cloud_)(j,i).r;
sumG += (*cloud_)(j,i).g;
sumB += (*cloud_)(j,i).b;
}
}
mean_luminance_ = 0.3 * sumR/n_points + 0.59 * sumG/n_points + 0.11 * sumB/n_points;
// mean_luminance_ = 0.2126 * sumR/n_points + 0.7152 * sumG/n_points + 0.0722 * sumB/n_points;
}
// Voxel grid filtering:
PointCloudPtr cloud_filtered(new PointCloud);
pcl::VoxelGrid<PointT> voxel_grid_filter_object;
voxel_grid_filter_object.setInputCloud(cloud_);
voxel_grid_filter_object.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
voxel_grid_filter_object.setFilterFieldName("z");
voxel_grid_filter_object.setFilterLimits(0.0, max_distance_);
voxel_grid_filter_object.filter (*cloud_filtered);
// Ground removal and update:
pcl::IndicesPtr inliers(new std::vector<int>);
boost::shared_ptr<pcl::SampleConsensusModelPlane<PointT> > ground_model(new pcl::SampleConsensusModelPlane<PointT>(cloud_filtered));
ground_model->selectWithinDistance(ground_coeffs_, voxel_size_, *inliers);
no_ground_cloud_ = PointCloudPtr (new PointCloud);
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*no_ground_cloud_);
if (inliers->size () >= (300 * 0.06 / voxel_size_ / std::pow (static_cast<double> (sampling_factor_), 2)))
ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_, ground_coeffs_);
else
{
if (debug_flag)
{
PCL_INFO ("No groundplane update!\n");
}
}
// Background Subtraction (optional):
if (background_subtraction_)
{
PointCloudPtr foreground_cloud(new PointCloud);
for (unsigned int i = 0; i < no_ground_cloud_->points.size(); i++)
{
if (not (background_octree_->isVoxelOccupiedAtPoint(no_ground_cloud_->points[i].x, no_ground_cloud_->points[i].y, no_ground_cloud_->points[i].z)))
{
foreground_cloud->points.push_back(no_ground_cloud_->points[i]);
}
}
no_ground_cloud_ = foreground_cloud;
}
if (no_ground_cloud_->points.size() > 0)
{
// Euclidean Clustering:
std::vector<pcl::PointIndices> cluster_indices;
typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud(no_ground_cloud_);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance(2 * 0.06);
ec.setMinClusterSize(min_points_);
ec.setMaxClusterSize(max_points_);
ec.setSearchMethod(tree);
ec.setInputCloud(no_ground_cloud_);
ec.extract(cluster_indices);
// Sensor tilt compensation to improve people detection:
PointCloudPtr no_ground_cloud_rotated(new PointCloud);
Eigen::VectorXf ground_coeffs_new;
if(sensor_tilt_compensation_)
{
// We want to rotate the point cloud so that the ground plane is parallel to the xOz plane of the sensor:
Eigen::Vector3f input_plane, output_plane;
input_plane << ground_coeffs_(0), ground_coeffs_(1), ground_coeffs_(2);
output_plane << 0.0, -1.0, 0.0;
Eigen::Vector3f axis = input_plane.cross(output_plane);
float angle = acos( input_plane.dot(output_plane)/ ( input_plane.norm()/output_plane.norm() ) );
transform_ = Eigen::AngleAxisf(angle, axis);
// Setting also anti_transform for later
anti_transform_ = transform_.inverse();
no_ground_cloud_rotated = rotateCloud(no_ground_cloud_, transform_);
ground_coeffs_new.resize(4);
ground_coeffs_new = rotateGround(ground_coeffs_, transform_);
}
else
{
transform_ = transform_.Identity();
anti_transform_ = transform_.inverse();
no_ground_cloud_rotated = no_ground_cloud_;
ground_coeffs_new = ground_coeffs_;
}
// To avoid PCL warning:
if (cluster_indices.size() == 0)
cluster_indices.push_back(pcl::PointIndices());
// Head based sub-clustering //
pcl::people::HeadBasedSubclustering<PointT> subclustering;
subclustering.setInputCloud(no_ground_cloud_rotated);
subclustering.setGround(ground_coeffs_new);
subclustering.setInitialClusters(cluster_indices);
subclustering.setHeightLimits(min_height_, max_height_);
subclustering.setMinimumDistanceBetweenHeads(heads_minimum_distance_);
subclustering.setSensorPortraitOrientation(vertical_);
subclustering.subcluster(clusters);
// for (unsigned int i = 0; i < rgb_image_->points.size(); i++)
// {
// if ((rgb_image_->points[i].r < 0) | (rgb_image_->points[i].r > 255) | isnan(rgb_image_->points[i].r))
// {
// std::cout << "ERROR in RGB data!" << std::endl;
// }
// }
if (use_rgb_) // if RGB information can be used
{
// Person confidence evaluation with HOG+SVM:
if (vertical_) // Rotate the image if the camera is vertical
{
swapDimensions(rgb_image_);
}
for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
{
//Evaluate confidence for the current PersonCluster:
Eigen::Vector3f centroid = intrinsics_matrix_ * (anti_transform_ * it->getTCenter());
centroid /= centroid(2);
Eigen::Vector3f top = intrinsics_matrix_ * (anti_transform_ * it->getTTop());
top /= top(2);
Eigen::Vector3f bottom = intrinsics_matrix_ * (anti_transform_ * it->getTBottom());
bottom /= bottom(2);
it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_));
}
}
else
{
for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
{
it->setPersonConfidence(-100.0);
}
}
}
return (true);
}
