template <typename PointT> Eigen::VectorXf
open_ptrack::detection::GroundBasedPeopleDetectionApp<PointT>::rotateGround(Eigen::VectorXf ground_coeffs, Eigen::Affine3f transform){
Eigen::VectorXf ground_coeffs_new;
// Create a cloud with three points on the input ground plane:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr dummy (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB first = pcl::PointXYZRGB(0.0,0.0,0.0);
first.x = 1.0;
pcl::PointXYZRGB second = pcl::PointXYZRGB(0.0,0.0,0.0);
second.y = 1.0;
pcl::PointXYZRGB third = pcl::PointXYZRGB(0.0,0.0,0.0);
third.x = 1.0;
third.y = 1.0;
dummy->points.push_back( first );
dummy->points.push_back( second );
dummy->points.push_back( third );
for(uint8_t i = 0; i < dummy->points.size(); i++ )
{ // Find z given x and y:
dummy->points[i].z = (double) ( -ground_coeffs_(3) -(ground_coeffs_(0) * dummy->points[i].x) - (ground_coeffs_(1) * dummy->points[i].y) ) / ground_coeffs_(2);
}
// Rotate them:
dummy = rotateCloud(dummy, transform);
// Compute new ground coeffs:
std::vector<int> indices;
for(unsigned int i = 0; i < dummy->points.size(); i++)
{
indices.push_back(i);
}
pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> model_plane(dummy);
model_plane.computeModelCoefficients(indices, ground_coeffs_new);
return ground_coeffs_new;
}
template <typename PointInT, typename PointOutT> void
pcl::ROPSEstimation <PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
if (triangles_.size () == 0)
{
output.points.clear ();
return;
}
buildListOfPointsTriangles ();
//feature size = number_of_rotations * number_of_axis_to_rotate_around * number_of_projections * number_of_central_moments
unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5;
unsigned int number_of_points = static_cast <unsigned int> (indices_->size ());
output.points.resize (number_of_points, PointOutT ());
for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
{
std::set <unsigned int> local_triangles;
std::vector <int> local_points;
getLocalSurface (input_->points[(*indices_)[i_point]], local_triangles, local_points);
Eigen::Matrix3f lrf_matrix;
computeLRF (input_->points[(*indices_)[i_point]], local_triangles, lrf_matrix);
PointCloudIn transformed_cloud;
transformCloud (input_->points[(*indices_)[i_point]], lrf_matrix, local_points, transformed_cloud);
PointInT axis[3];
axis[0].x = 1.0f; axis[0].y = 0.0f; axis[0].z = 0.0f;
axis[1].x = 0.0f; axis[1].y = 1.0f; axis[1].z = 0.0f;
axis[2].x = 0.0f; axis[2].y = 0.0f; axis[2].z = 1.0f;
std::vector <float> feature;
for (unsigned int i_axis = 0; i_axis < 3; i_axis++)
{
float theta = step_;
do
{
//rotate local surface and get bounding box
PointCloudIn rotated_cloud;
Eigen::Vector3f min, max;
rotateCloud (axis[i_axis], theta, transformed_cloud, rotated_cloud, min, max);
//for each projection (XY, XZ and YZ) compute distribution matrix and central moments
for (unsigned int i_proj = 0; i_proj < 3; i_proj++)
{
Eigen::MatrixXf distribution_matrix;
distribution_matrix.resize (number_of_bins_, number_of_bins_);
getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix);
std::vector <float> moments;
computeCentralMoments (distribution_matrix, moments);
feature.insert (feature.end (), moments.begin (), moments.end ());
}
theta += step_;
} while (theta < 90.0f);
}
float norm = 0.0f;
for (unsigned int i_dim = 0; i_dim < feature_size; i_dim++)
norm += std::abs (feature[i_dim]);
if (norm < std::numeric_limits <float>::epsilon ())
norm = 1.0f;
else
norm = 1.0f / norm;
for (unsigned int i_dim = 0; i_dim < feature_size; i_dim++)
output.points[i_point].histogram[i_dim] = feature[i_dim] * norm;
}
}
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);
}
