People detector in RGB-D pointclouds

edit

Hello all;

I need to perform a people detector for security tasks in robot cells. My initial idea has been to make a ros node adapting the work from Matteo Munaro, from the PCL tutorials, as it fits quite well my initial idea: PCL people detector However, when I run it, I obtain continuously this error, that is, not detecting people:

[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Cluster indices have not been set!
[DEBUG] [1446646345.599799741]:  0 people found
[DEBUG] [1446646345.641495873]: Ground plane: 0.0261189 -0.995346 -0.0927586 1.17296

No groundplane update!

My pointcloud has this aspect, that complies with the requirements of the algorithm:

And my node code is also quite simple:

void initPeopleDetector(void)
{
    // Create classifier for people detection:  
  groundCoeffs.resize(4);
  personClassifier.loadSVMFromFile(clasifFile);  
  peopleDetector.setVoxelSize(voxelSize);                                                           
  Eigen::Matrix3f rgb_intrinsics_matrix;
  rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; 
  peopleDetector.setIntrinsics(rgb_intrinsics_matrix);                                          
  peopleDetector.setClassifier(personClassifier);                                                   
  peopleDetector.setHeightLimits(minPersonHeight, maxPersonHeight);                                                     
}

void scenePointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr scene(new pcl::PointCloud<pcl::PointXYZRGBA>);  
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr floor(new pcl::PointCloud<pcl::PointXYZRGBA>);  
    pcl::fromROSMsg (*msg, *scene);

    sceneMutex.lock();

    //floor segmentation
    pcl::PassThrough<pcl::PointXYZRGBA> pass;

  pass.setInputCloud (scene);
  pass.setFilterFieldName ("x");
  pass.setFilterLimits (x_min, x_max);
  pass.filter (*floor);
  pass.setInputCloud (scene);
  pass.setFilterFieldName ("y");
  pass.setFilterLimits (y_min, y_max);
  pass.filter (*floor);
  pass.setInputCloud (floor);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (z_min, z_max);
  pass.filter (*floor);

    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);
  seg.setInputCloud (floor);
  seg.segment (*inliers, *coefficients);
  groundCoeffs[0]= coefficients->values[0]; groundCoeffs[1]= coefficients->values[1];
  groundCoeffs[2]= coefficients->values[2]; groundCoeffs[3]= coefficients->values[3];
    ROS_DEBUG_STREAM("Ground plane: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3]<<" ");   

    //people detection      
    std::vector<pcl::people::PersonCluster<pcl::PointXYZRGBA> > clusters;   
  peopleDetector.setInputCloud(scene);
  peopleDetector.setGround(groundCoeffs);                                           
  peopleDetector.compute(clusters);      
  //update ground
  groundCoeffs = peopleDetector.getGround();                                            

  int i=0,j=0;
  for(std::vector<pcl::people::PersonCluster<pcl::PointXYZRGBA> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
    {
        if(it->getPersonConfidence() > minConfidence)             
        {
            //Centroid, min and max points
            Eigen::Vector3f centroid = clusters[j].getCenter();
            Eigen::Vector3f minPoints = clusters[j].getMin();
            Eigen::Vector3f maxPoints = clusters[j].getMax();
            //draw a marker
            mark_cluster(centroid, minPoints, maxPoints,scene->header.frame_id, "person" ,i, 255, 0, 0);
            i++;
        }
        j++;
    }
    ROS_DEBUG_STREAM(" "<< i << " people found" << " ");

    sceneMutex.unlock();
}

Could anyone give me a hint about what is wrong in the procedure? Is there any other method in ROS to detect people in open environments using RGB-D pointclouds? I have been looking at ROS wiki, and I cannot find any suitable solution....

Thank you all in advance,

Alberto