[pcl::PCDReader::read] Number of points read (3643177) is different than expected (7335701) couldn’t load the pcd file
引子
lego-loam和lio-sam在跑完建图之后,可以选择是否导出特征点地图和fullMap的pcd文件。lego-loam导出pcd文件的代码附在最后(有所改动,详情可参考源码),pcd文件最后存到/tmp目录下。该段代码是放在visualizeMapThread中的,前面有一个ros::ok的循环。当ctrl+c结束后,开始执行后面的导出pcd程序,但如果在一定时间内没有执行完,进程强制退出,窗口抛出"escalating to SIGTERM"错误。此时存储的pcd文件可能是不完全的,当使用pcl::io::loadPCDFile或者pcl::PCDReader::read读取的时候,可能会报"Number of points read (3643177) is different than expected (7335701)" 错误,这是由于pcl::PointCloud的field中记录的点数和实际点数不一致引起的,可以通过以下两种方式解决:
1、修改field —— 使用文本编辑软件打开pcd文件,直接修改field中的WIDTH和POINTS的数值。该方法具有一般性,当然还是应该查一下为什么点数会不一致。下图是使用vscode打开Xpcd文件效果截图。
2、针对"escalating to SIGTERM"1引起的点云数目不一致问题,修改/opt/ros/
kinetic(ros版本)/lib/python2.7/dist-package/roslaunch/nodeprocess.py文件中的"_TIMEOUT_SIGINT = 15.0 #seconds"为自己期望的运行时间,从而保证实际点数和field中记录的值相同。当然,这只针对由"escalating to SIGTERM"引起的点数不一致问题,不具有一般性。
附
void visualizeGlobalMapThread() {
ros::Rate rate(0.2);
while (ros::ok()){
rate.sleep();
publishGlobalMap();
}
std::cout << "save point cloud" << std::endl;
// save final point cloud
// 如果publishGlobalMap()不起作用,下面的这一句会抛出异常
// pcl::io::savePCDFileASCII(fileDirectory+"finalCloud.pcd", *globalMapKeyFramesDS);
string cornerMapString = "/tmp/cornerMap.pcd";
string surfaceMapString = "/tmp/surfaceMap.pcd";
string trajectoryString = "/tmp/trajectory.pcd";
pcl::PointCloud<PointType>::Ptr cornerMapCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr cornerMapCloudDS(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfaceMapCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfaceMapCloudDS(new pcl::PointCloud<PointType>());
// 添加了fullMap
pcl::PointCloud<PointType>::Ptr fullMapCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr fullMapCloudDS(new pcl::PointCloud<PointType>());
for(int i = 0; i < cornerCloudKeyFrames.size(); i++) {
*cornerMapCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
*surfaceMapCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
*surfaceMapCloud += *transformPointCloud(outlierCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
}
*fullMapCloud += *cornerMapCloud;
*fullMapCloud += *surfaceMapCloud;
downSizeFilterCorner.setInputCloud(cornerMapCloud);
downSizeFilterCorner.filter(*cornerMapCloudDS);
downSizeFilterSurf.setInputCloud(surfaceMapCloud);
downSizeFilterSurf.filter(*surfaceMapCloudDS);
downSizeFilterCorner.setInputCloud(fullMapCloud);
downSizeFilterCorner.filter(*fullMapCloudDS);
std::cout << cornerMapCloud->points.size() << std::endl;
std::cout << surfaceMapCloud->points.size() << std::endl;
pcl::io::savePCDFileASCII(fileDirectory+"cornerMap.pcd", *cornerMapCloudDS);
pcl::io::savePCDFileASCII(fileDirectory+"surfaceMap.pcd", *surfaceMapCloudDS);
pcl::io::savePCDFileASCII(fileDirectory+"trajectory.pcd", *cloudKeyPoses3D);
pcl::io::savePCDFileASCII(fileDirectory+"fullMap.pcd", *fullMapCloudDS);
}
Reference
https://blog.csdn.net/zack_liu/article/details/78456467 ↩︎