点云相减去除背景改进版

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

       程序是在一个博主基础上进行了部分改进，次程序只适用于背景+目标点云（设为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;
}

有问题在评论区问就行。