[ros-users] Issues with Principal Curvatures Estimation

Dear all,

I am trying to estimate the curvature values of a point cloud using the
principal curvates estimation from PCL in the following way:

1. Get a point cloud.
2. Compute the normals.
3. Estimate the curvatures

But my program hangs in step 3 and the compute() function my program calls
to estimate the curvatures does not return. I have attached the cpp file
herewith. Looking forward to your suggestions.


Cheers,

Hozefa
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

#include "pcl/kdtree/kdtree_ann.h" //for the kdtree

#include <pcl/features/principal_curvatures.h> //for estimating curvatures
#include <pcl/features/normal_3d_tbb.h>

class CurvatureTest
{
  public:
    CurvatureTest();
    ~CurvatureTest();
    void curvatureTestCallBack(const sensor_msgs::PointCloud& msg);
    pcl::PointCloud<pcl::PointXYZ> convertFromMsgToPointCloud(const sensor_msgs::PointCloud& pointcloud_msg);
    pcl::PointCloud<pcl::PrincipalCurvatures> getCurvatureEstimates(pcl::PointCloud<pcl::PointXYZ> &pointcloud);
    void run();

  private:
    ros::NodeHandle nh_;
    std::string subscribe_pointcloud_topic_, frame_id_;
    ros::Subscriber pointcloud_subscriber_;

    bool firstCloudReceived_;
    pcl::PointCloud<pcl::PointXYZ> pointcloud2_current_;
};

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

pcl::PointCloud<pcl::PointXYZ> CurvatureTest::convertFromMsgToPointCloud(const sensor_msgs::PointCloud& pointcloud_msg)
{
sensor_msgs::PointCloud2 pointcloud2_msg;
pcl::PointCloud<pcl::PointXYZ> pointcloud_pcl;

// Converting from PointCloud msg format to PointCloud2 msg format
sensor_msgs::convertPointCloudToPointCloud2(pointcloud_msg, pointcloud2_msg);

// Converting from PointCloud2 msg format to pcl pointcloud format
point_cloud::fromMsg(pointcloud2_msg, pointcloud_pcl);

return (pointcloud_pcl);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

pcl::PointCloud<pcl::PrincipalCurvatures> CurvatureTest::getCurvatureEstimates(pcl::PointCloud<pcl::PointXYZ> &pointcloud)
{
vector<int> indices;
pcl::PointCloud<pcl::Normal> normals;
pcl::KdTreeANN<pcl::PointXYZ>::Ptr tree_ptr_;
pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> pc;
pcl::PointCloud<pcl::PrincipalCurvatures> pcs; //Object

tree_ptr_ = boost::make_shared<pcl::KdTreeANN<pcl::PointXYZ> > ();
tree_ptr_->setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (pointcloud));

ROS_INFO("getCurvatureEstimates: tree_ptr_ set.");

  indices.resize (pointcloud.points.size ());
  for (size_t i = 0; i < indices.size (); ++i)
  {
    indices[i] = i;
  }
  ROS_INFO("getCurvatureEstimates: indices resized.");

  // Estimate normals first
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
  // set parameters
  n.setInputCloud (boost::make_shared <const pcl::PointCloud<pcl::PointXYZ> > (pointcloud));
  n.setIndices (boost::make_shared <vector<int> > (indices));
  n.setSearchMethod (tree_ptr_);
  n.setKSearch (10);    // Use 10 nearest neighbors to estimate the normals
  // estimate
  n.compute (normals);
  ROS_INFO("getCurvatureEstimates: normals computed");

// set parameters
pc.setInputNormals (boost::make_shared<const pcl::PointCloud<pcl::Normal> > (normals));
pc.setInputCloud (boost::make_shared <const pcl::PointCloud<pcl::PointXYZ> > (pointcloud));
pc.setIndices (boost::make_shared <vector<int> > (indices));
pc.setSearchMethod (tree_ptr_);
pc.setKSearch (indices.size ());
ROS_INFO("getCurvatureEstimates: parameters set.");
// estimate
pc.compute (pcs);

ROS_INFO("getCurvatureEstimates: estimates computed");

return (pcs);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

CurvatureTest::CurvatureTest(): nh_("~")
{
nh_.param("subscribe_pointcloud_topic", subscribe_pointcloud_topic_, std::string("/shoulder_cloud"));
nh_.param("frame_id", frame_id_, std::string("/map"));
firstCloudReceived_ = false;
ROS_INFO("curvature_test node is up and running.");

run();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

CurvatureTest::~CurvatureTest()
{
ROS_INFO("Shutting down curvature_test node!.");
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void CurvatureTest::run()
{
pointcloud_subscriber_ = nh_.subscribe(subscribe_pointcloud_topic_, 100, &CurvatureTest::curvatureTestCallBack, this);

ros::spin();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void CurvatureTest::curvatureTestCallBack(const sensor_msgs::PointCloud& pointcloud_msg)
{

  if( firstCloudReceived_ == false)
  {
    ROS_INFO("Received first point cloud.");
    pointcloud2_current_ = convertFromMsgToPointCloud(pointcloud_msg);
    pcl::PointCloud<pcl::PrincipalCurvatures> pointcloud_curvatures;
    pointcloud_curvatures = getCurvatureEstimates(pointcloud2_current_);
    firstCloudReceived_ = true;
    ROS_INFO("Curvatures Estimated");

}

}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

int main(int argc, char** argv)
{
    ros::init(argc, argv, "curvature_test");
    CurvatureTest curvature_test;
    return(0);
}