该案例主要包括(点云序列获取、滤波模块、平面检测及删除模块、聚类分割模块、运动分割模块、ICP配准模块、可视化模块)
1.点云序列读取
第三方库boost,实现同一个文件内所有pcd文件的读取
std::vector<std::string> pcd_files_; //点云序列
std::vector<boost::filesystem::path> pcd_paths_; //储存文件夹下的路径序列
boost::filesystem::directory_iterator end_itr;
std::string dir_ = "F:\\test";
if (boost::filesystem::is_directory(dir_)) //用于判断传入的dir_是否为目录
{
for (boost::filesystem::directory_iterator itr(dir_); itr != end_itr; ++itr) //结合for循环可以遍历文件path的所有内容
{
std::string ext = itr->path().extension().string(); //获得文件后缀
if (ext.compare(".pcd") == 0)
{
pcd_files_.push_back(itr->path().string());
pcd_paths_.push_back(itr->path());
}
else
{
PCL_DEBUG("[PCDVideoPlayer::selectFolderButtonPressed] : found a different file\n");
}
}
}
else
{
PCL_ERROR("Path is not a directory\n");
exit(-1);
}
读取第一个点云图:
通过三维扫描设备采集得到的原始散乱点云普遍存在不均匀采样的情况,点云下采样处理能得到分布均匀的点云。使用Voxel滤波器对场景点云下采样。
double voxel_size = 0.01;
pcl::VoxelGrid<pcl::PointXYZRGB> vg;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered1(new pcl::PointCloud<pcl::PointXYZRGB>);
vg.setInputCloud(back_cloud);
vg.setLeafSize(voxel_size, voxel_size, voxel_size);
vg.filter(*cloud_filtered1);
*cloud_filtered = *cloud_filtered1;
std::cout << "show the filtered data!" << endl;
showCloudstmp(cloud_filtered);
利用采样一致性算法检测平面点云,并将其删除。
pcl::PointIndices::Ptr tmpinliers(new pcl::PointIndices);
double distance = 0.06, degree = 25, max = 10000;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Ncloud_ground_plane(new pcl::PointCloud<pcl::PointXYZRGB>());
Eigen::VectorXf coefficients;
pcl::SampleConsensusModelPerpendicularPlane<pcl::PointXYZRGB>::Ptr model(new pcl::SampleConsensusModelPerpendicularPlane<pcl::PointXYZRGB>(cloud_filtered));
model->setAxis(Eigen::Vector3f(0.0, 1.0, 0.0)); //设置所搜索平面垂直的轴
model->setEpsAngle(pcl::deg2rad(degree)); //设置待检测的平面模型和上述轴的最大角度
pcl::RandomSampleConsensus<pcl::PointXYZRGB> ransac(model);
ransac.setMaxIterations(max); //最大迭代次数
ransac.setDistanceThreshold(distance); //距离阈值
ransac.computeModel();
ransac.getInliers(tmpinliers->indices);
ransac.getModelCoefficients(coefficients);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(tmpinliers);
extract.setNegative(true);
extract.filter(*Ncloud_ground_plane);
*cloud_filtered = *Ncloud_ground_plane;
std::cout << "show the data after deleting ground plane!" << endl;
showCloudstmp(cloud_filtered);
重点描述:
类SampleConsensusModelPerpendicularPlane的函数setAxis用于设置所搜索平面垂直的轴,函数setEpsAngle用于设置待检测的平面模型和上述设置轴的最大角度。在这里设置(0,1,0)即Y轴,则寻找其垂直平面X-Y平面上的平面。
去除所有平面:
double distance = 0.02, ratio = 0.8;
int maxitter = 10000;
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
pcl::PointIndices::Ptr tmpinliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>());
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(maxitter);
seg.setDistanceThreshold(distance);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZRGB>);
int nr_points = (int)cloud_filtered->points.size();
for (int i = 0; i < 1; i++)
{
seg.setInputCloud(cloud_filtered);
seg.segment(*tmpinliers, *coefficients);
std::cout << &