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