Improved version of point cloud subtraction to remove background

       Note that this program requires a background point cloud and a background + target point cloud. The coordinate systems of the two point clouds must be the same. The blogger took a background and a background + target point cloud with a fixed depth camera. Using this program, the target point cloud was successfully extracted. The program was modified on the basis of the reference program, and some comments may have been changed. Point cloud files are inconvenient to provide due to special reasons. I can't find the original program, so I can't give a reference link.

       The program is partially improved on the basis of a blogger, and the sub-program is only applicable to the case where the number of points in the background + target point cloud (set as point cloud B) is greater than the number of points in the background point cloud (set as point cloud A).

       The principle of the algorithm is to focus on point cloud B, and compare each point in point cloud B with each point in point cloud A. If there is a distance between A and B that is less than p (the size of p is determined by yourself), then set this point in B as a repeat point and a background point.

#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;
}

Just ask any questions in the comment area.