点云相减去除背景改进版

改进版"/>

点云相减去除背景改进版

       注意此程序需要一个背景点云和一个背景+目标的点云，两点云坐标系要相同，博主是固定深度相机拍了一个背景和一个背景+目标的点云，用此程序成功实现了对目标点云的提取，程序是在参考程序基础上改的，可能有的注释改掉了。点云文件因特殊原因不方便提供。原程序找不到了，所以没办法给参考链接。

       程序是在一个博主基础上进行了部分改进，次程序只适用于背景+目标点云（设为B点云）中点的数量大于背景点云（设为A点云）数量的情况。

       算法原理是在以B点云为主，将B点云中每个点与A点云中的每个点相比，若在A中存在与B距离小于p（p大小自己定），则将B中这个点设为重复点及背景点。

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <vector>
#include <algorithm>
#include <ctime>
#include <pcl/visualization/pcl_plotter.h>
#include <fstream>//写入txt
#include<string>
#include <stdlib.h>//将整型转换成字符型
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>using namespace std;  // 可以加入 std 的命名空间//给定两个点云A和B，求取两个点云的不同之处，A异B表示在A中不在B中的点，B异A表示在B中不在A中的点int
main(int argc, char** argv)
{string ReviseName;cout << "是否已经修改输出文件（A异B和B异A）的名称？请输入Y或N。" << endl;//不参与计时，因此与输入时间时的人的反应有关cin >> ReviseName;if (ReviseName != "Y"){return (-1);//跳出整个程序}//-------------------------------------------------------------------------------srand(time(NULL));  //seeds rand() with the system time time_t begin, end;begin = clock();  //开始计时//-------------------------------------------------------------------------------pcl::PointCloud<pcl::PointXYZRGB>::Ptr Acloud(new pcl::PointCloud<pcl::PointXYZRGB>);	// 存放A点云，背景if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("1 - Cloud.pcd", *Acloud) == -1){                                                                           PCL_ERROR("Couldn't read that boundary pcd file\n");                            return(-1);}pcl::PointCloud<pcl::PointXYZRGB>::Ptr Bcloud(new pcl::PointCloud<pcl::PointXYZRGB>);	// 存放B点云，目标+背景if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("yv1 Cloud.pcd", *Bcloud) == -1){                                                                           PCL_ERROR("Couldn't read that outlier pcd file\n");                           return(-1);}pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;kdtree.setInputCloud(Acloud);//在Acloud中进行搜索int K = 1;float p = 2e-5;//p为距离，距离之内的设为重复点std::vector<int> pointIdxNKNSearch(K);//进行1邻域点搜索std::vector<float> pointNKNSquaredDistance(K);//设置参数int nr_Apoints = (int)Acloud->points.size();//获得Acloud的大小，用于计算时的循环int nr_Bpoints = (int)Bcloud->points.size();//获得Bcloud的大小，用于计算时的循环cout << nr_Apoints << endl;cout << nr_Bpoints << endl;std::vector<int> LA01(nr_Bpoints, 0); //存放各点有无重合点的标记。初始化为0，其中0表示无重合点，1表示有重合点std::vector<int> LB01(nr_Bpoints, 0); //存放各点有无重合点的标记。初始化为0，其中0表示无重合点，1表示有重合点std::vector<int> LA;  LA.clear();  //存放A中异于B的点，初始为空！！！,不定义长度，因此后面要用L.push_back(i)来压入数据。std::vector<int> LB;  LB.clear();  //存放B中异于A的点，初始为空！！！,不定义长度，因此后面要用L.push_back(i)来压入数据。for (int i = 0; i < nr_Bpoints; ++i)//对B中的每个点与A中点进行比较{if (kdtree.nearestKSearch(Bcloud->points[i], K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0){//cout << pointNKNSquaredDistance[0] << endl;if (pointNKNSquaredDistance[0] <= p)//如果搜索到的第一个点距小于参数p，那么这个点为重合点（因为用A搜B中的点，不存在包含自身的情况）{LB01[i] = 1;LA01[pointIdxNKNSearch[0]] = 1;}}}
for (int ii = 0; ii < nr_Apoints; ++ii){if (LA01[ii] == 0)LA.push_back(ii);}for (int jj = 0; jj < nr_Bpoints; ++jj){if (LB01[jj] == 0)LB.push_back(jj);}pcl::PointCloud<pcl::PointXYZRGB>::Ptr Arecloud(new pcl::PointCloud<pcl::PointXYZRGB>);// 创建点云Arecloud存放A中剩余（residual）的点pcl::copyPointCloud(*Acloud, LA, *Arecloud);//按照索引复制点云pcl::PointCloud<pcl::PointXYZRGB>::Ptr Brecloud(new pcl::PointCloud<pcl::PointXYZRGB>);// 创建点云Arecloud存放B中剩余（residual）的点pcl::copyPointCloud(*Bcloud, LB, *Brecloud);//按照索引复制点云//写入磁盘//pcl::io::savePCDFileASCII("basicpoint2.pcd", *Arecloud); //将点云保存到PCD文件中pcl::io::savePCDFileASCII("FISHNEEDLVBO.pcd", *Brecloud); //将点云保存到PCD文件中为物体点云int bpoints = (int)Brecloud->points.size();cout << bpoints << endl;
end = clock();  //结束计时double Times = double(end - begin) / CLOCKS_PER_SEC; //将clock()函数的结果转化为以秒为单位的量std::cout << "time: " << Times << "s" << std::endl;boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("viewer"));int v1 = 0;int v2 = 1;viewer->createViewPort(0, 0, 0.5, 1, v1);//设置窗口点云显示范围viewer->createViewPort(0.5, 0, 1, 1, v2);viewer->setBackgroundColor(0, 0, 0, v1);//设置背景颜色viewer->setBackgroundColor(0, 0, 0, v2);//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZ> rgb(Arecloud);viewer->addPointCloud<pcl::PointXYZRGB>(Bcloud, "cloud", v1);viewer->addPointCloud<pcl::PointXYZRGB>(Brecloud, "want cloud", v2);while (!viewer->wasStopped()){viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(10000));}return 0;
}

有问题在评论区问就行。