ROS上使用PCL进行三维点云读取出错

#1

最近正在学习图像处理入门,在Ros环境下读取KinectV2获取的三维点云数据时出现错误,求大神们指点,如果有这方面经验的朋友欢迎向我发送邮件kenshinxj@163.com或在评论区留言进一步交流,有偿的线上或线下帮助调试我都可以接受
以下为我学习配置使用的github项目说明
Mini-3D-Scanner

我运行的代码信息如下
----------kinect2_emulator.cpp------------------------
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

#include
#include <pcl/io/pcd_io.h>

typedef pcl::PointXYZRGBA PointT;

int
main (int argc, char **argv)
{
int start_index = std::stoi(argv[1]);
int end_index = std::stoi(argv[2]);

ros::init (argc, argv, "kinect2_emulator");
//发布节点	
ros::NodeHandle nh;
ros::Publisher emu_pub = nh.advertise<sensor_msgs::PointCloud2> ("/kinect2/qhd/points", 1);

//定义点云数据对象output和指针Ptr
sensor_msgs::PointCloud2 output;
pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT>);

std::string dir = "/home/kenshin/catkin_ws/src/scanner/data/emulator";
int cloud_index = start_index;
//std::string filename;
std::string filename = dir + std::to_string(cloud_index) + ".pcd";
//std::cout<<"save save save"<<filename<<endl;


ros::Rate loop_rate(2);
while (ros::ok())
{
    filename = dir + std::to_string(cloud_index) + ".pcd";

    pcl::io::loadPCDFile<PointT> (filename, *cloud_ptr);
    std::cout << filename << std::endl;

    //Convert the cloud to ROS message
    pcl::toROSMsg(*cloud_ptr, output);
    output.header.frame_id = "odom";

    emu_pub.publish(output);
    std::cout << "have published" << std::endl;
    cloud_index++;
    if (cloud_index > end_index)
        break;
    loop_rate.sleep();

}

return 0;

}

------------------------pcl_processing.cpp-------------------------
#include
#include
#include

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Int64.h>
#include <pcl_conversions/pcl_conversions.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/extract_indices.h>

#include <Eigen/Eigen>

typedef pcl::PointXYZRGBA PointT;
static int pcd_index = 0;
static char gotDataFlag = 0;// could use a ‘class’ to reduce this global variable

pcl::PointCloud::Ptr
cloud_filter(pcl::PointCloud::Ptr &cloud);

void callback(sensor_msgs::PointCloud2 cloud_raw)
{
// cloud_raw is PC data from Kinect V2;
// static int pcd_index = 0;
pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud);
std::string filename = “/home/kenshin/catkin_ws/src/scanner/data/” + std::to_string(pcd_index) + “.pcd”;

ROS_INFO("Processing #%i PointCloud...", pcd_index);

// change PC format from PointCloud2 to pcl::PointCloud<PointT>
pcl::fromROSMsg(cloud_raw, *cloud_ptr);

// crop, segment, filter
cloud_ptr = cloud_filter(cloud_ptr);

// save PCD file to local folder
pcl::io::savePCDFileBinary (filename, *cloud_ptr);

gotDataFlag = 1;
++pcd_index;

}

int main (int argc, char **argv)
{
ros::init (argc, argv, “pcl_processing”);

ros::NodeHandle nh; // can sub and pub use the same NodeHandle?
ros::Subscriber sub = nh.subscribe("/kinect2/qhd/points", 1 , callback);
ros::Publisher pub = nh.advertise<std_msgs::Int64> ("pcd_save_done", 1);

ros::Rate loop_rate(1);
std_msgs::Int64 number_PCDdone;
// std::stringstream ss;
while (ros::ok())
{
    /* Do something? */
    // ss.str("");
    // ss << "have saved pcd #" << pcd_index ;
    // msg.data = ss.str();
    number_PCDdone.data = pcd_index;

    // ros::spin()
    //*** only when this is run, it will get to callback
    ros::spinOnce();

    // only publish data when having got data
    if (gotDataFlag == 1){
        pub.publish(number_PCDdone);
        gotDataFlag = 0;
    }
    loop_rate.sleep();
}
return 0;

}

pcl::PointCloud::Ptr
cloud_filter(pcl::PointCloud::Ptr &cloud)
{
pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud);

//****************************************************//
// Create the filtering object - passthrough
pcl::PassThrough passz;
passz.setInputCloud (cloud);
passz.setFilterFieldName (“z”);
passz.setFilterLimits (0.75, 1.0);
// passz.setFilterLimits (0.5, 1.5);

// passz.setFilterLimits (-2.0, 4.0);
//pass.setFilterLimitsNegative (true);
passz.filter (*cloud_filtered);

pcl::PassThrough<PointT> passy;
passy.setInputCloud (cloud_filtered);
passy.setFilterFieldName ("y");
passy.setFilterLimits (-0.1, 0.22);
// passy.setFilterLimits (-0.5, 0.5);

// passy.setFilterLimits (-2.0, 2.0);
//pass.setFilterLimitsNegative (true);
passy.filter (*cloud_filtered);

pcl::PassThrough<PointT> passx;
passx.setInputCloud (cloud_filtered);
passx.setFilterFieldName ("x");
passx.setFilterLimits (-0.18, 0.18);
// passx.setFilterLimits (-0.5, 0.5);

// passx.setFilterLimits (-3.0, 3.0);
//pass.setFilterLimitsNegative (true);
passx.filter (*cloud_filtered);

//****************************************************//

//****************************************************//
// // segment ground
// pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
// pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// // Create the segmentation object
// pcl::SACSegmentation seg;
// // Optional
// seg.setOptimizeCoefficients (true);
// // Mandatory
// seg.setModelType (pcl::SACMODEL_PLANE); // plane
// seg.setMethodType (pcl::SAC_RANSAC);
// seg.setDistanceThreshold (0.010);

// seg.setInputCloud (cloud_filtered);
// seg.segment (*inliers, *coefficients);

// pcl::ExtractIndices<PointT> extract;
// extract.setInputCloud(cloud_filtered);
// extract.setIndices(inliers);
// extract.setNegative(true);
// extract.filter(*cloud_filtered);

//****************************************************//

//****************************************************//
// Create the filtering object - StatisticalOutlierRemoval filter
pcl::StatisticalOutlierRemoval sor;
sor.setInputCloud (cloud_filtered);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered);

//****************************************************//

// pcl::PointCloud<PointT>::Ptr cloud_write (new pcl::PointCloud<PointT>);
// cloud_write.width = cloud_filtered.points.size();
// cloud_write.height = 1;
// cloud_write.is_dense = false;

return cloud_filtered;

}

-------------------------下面是报错信息:-----------------------------------
SUMMARY
NODES
/
pcl_processing (scanner/pcl_processing)
rotation_dir (scanner/rotation_dir)

ROS_MASTER_URI=http://localhost:11311

process[rotation_dir-1]: started with pid [5394]
process[pcl_processing-2]: started with pid [5395]
[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!
terminate called after throwing an instance of ‘pcl::IOException’
what(): : [pcl::PCDWriter::writeBinary] Input point cloud has no data!
[pcl_processing-2] process has died [pid 5395, exit code -6, cmd /home/kenshin/catkin_ws/devel/lib/scanner/pcl_processing __name:=pcl_processing __log:=/home/kenshin/.ros/log/fa547aac-722c-11e9-a786-e4f89cbc248d/pcl_processing-2.log].
log file: /home/kenshin/.ros/log/fa547aac-722c-11e9-a786-e4f89cbc248d/pcl_processing-2*.log
[ERROR] [1557390794.889532855]: poll failed with error Interrupted system call

#2

Hmm 看起来这个问题解决之后,后续还是有不少问题在等着你。

同学你在哪个城市?

#4

我在北京,方丈在哪呀,同城的话以交流一下吗哈哈:hugs:

#5

我们有一个北京的研发中心,在知春路地铁附近,软件研究所。有兴趣可以约个时间来交流