基于三维点云机器人杆件目标识别与抓取（一）

从本文开始，我将近一年的课题学习与研究，已分片的形式来记录和展示。将通过这几篇博文的介绍，展示自己的学习和研究过程中采用的方法及其相关结果显示。
下图所示为基于位置的机器人视觉伺服反馈控制方法，本课题侧重于视觉伺服反馈部分中的场景超长杆件的目标识别及杆件的位姿估计。

本篇博文将介绍数据的采集以及数据的预处理等相关内容。
一、场景数据采集

点云数据通常由深度图生成，本文基于双目视觉成像原理获取深度图，红外点阵投射器将结构光投射到物体表面，左右红外相机获取左右图像，根据三角测量原理得到视差图，由视差图进而生成深度图[35-37]。如图2-1 所示为双目视觉成像原理，图中image_left与image_right 分别表示深度相机获取的左右图像， left O 和right O 分别表示左右相机的光轴，B表示光轴间的基线距离， Oleft与Oright分别表示目标点P(xc,yc,zc) 在左右图像中成像的像素位置，P(xc,yc,zc) 表示为实际空间中的一点。
通过双目视觉成像原理，得到其场景图像如下所示：

二、场景数据滤波处理
在获取场景点云数据时，因数据采集设备的精度及外界干扰的影响，造成获取的原始场景点云数据中存在大量的噪声点，该噪声点严重影响场景目标识别与位姿估计算法的实施效果，因此需要对获取的原始场景数据进行滤波处理操作，结合PCL（PointCloud Library）数据库中的滤波算子，减少数据中的离群点和噪声点，改善数据质量，提高后续数据处理算法实施的准确率。
相关滤波算法，这里以图片的形式展示：
1.体素化网格滤波
经深度相机获取的场景点云数据的数据量较大，为了减少数据所占内存以及算法实施耗时，在保证算法实施有效性的前提下，需要对获取的原始场景点云数据进行数据压缩。本文采用体素化网格滤波算法实现原始点云数据的压缩。
体素化网格滤波算法能够在保证点云数据外形特征的基础上实现点云数据的压缩。如图2-3中a所示，通过创建三维体素网格，使网格内的重心点近似代表网格内的所有点，假设网格内存在m个点，该网格的重心点 如式(2-3)所示：

2.直通滤波与半径滤波
原始点云数据经体素化网格滤波算法处理后，点云数据的数据量大幅减少，但压缩后的点云数据仍存在一定的噪声点与离群点，需要利用点云数据滤波算法对数据进行去噪和优化。本文用到的数据滤波算法包括直通滤波、半径滤波以及统计滤波等分别对数据进行去噪处理。
直通滤波算法对点云数据坐标轴特定范围内进行保留和删除操作。在使用直通滤波算法前，需要确定有效点云在空间内的大致区域，从而确定有效的数据范围，保留有效范围内的数据。
半径滤波算法以某点为中心创建一个范围，统计该范围内的点数，当点数大于给定阈值时保留，反之删除。如图2-3中b所示，当设定半径范围内点数为3时， 和 点将被删除， 点因满足其要求而保留。

3.统计滤波：
统计滤波算法主要是剔除离群噪声点，对每个点进行统计，分析并去除不满足设定阈值的点。根据数据中心与相邻点之间的距离分布，给定范围并删除不在范围内的点。 表示距离均值如式(2-4)， 表示距离均值方差如式(2-5)：

三、点云数据滤波处理实验与结果分析

如上图所示，通过深度相机获取的场景深度图得到原始场景点云数据。原始场景点云数据中的每一个点均具有位置和RGB信息，为了减少数据信息量，需要去除每个点的RGB信息，得到无RGB信息的点云数据。对无RGB信息的点云数据进行下采样，大幅减少数据量。针对下采样压缩后的点云数据依次进行直通滤波、统计滤波、半径滤波等算法的滤波处理，进一步改善了数据质量。如图2-4所示为点云数据滤波算法实施可视化，①为原始场景点云数据；②为无RGB信息点云数据；③为下采样压缩点云数据；④为直通滤波处理后点云数据；⑤为统计滤波处理后点云数据；⑥为半径滤波处理后点云数据。

如表2-1为点云数据预处理参数设置以及点云数量信息统计，各种数据滤波算法中设定的阈值参数由数据处理后的实际效果确定。原始场景点云数据的数据量将近17万个点，如此庞大的数据量用于后期识别和位姿估计部分的算法实施存在两个方面的问题：一是影响算法实施效果；二是大量数据的处理将会消耗大量内存，导致计算时间过长。本文经体素化下采样压缩算法的实施，数据量减少了大约20倍，通过对压缩后的点云数据实施滤波算法，剔除了部分离群点及噪声点，减少了数据量并提高了数据质量。

数据滤波部分相关代码：

#include<iostream>
#include<pcl/point_types.h>
#include<pcl/filters/passthrough.h>                   //直通滤波器头文件
#include<pcl/filters/voxel_grid.h>                   //体素滤波器头文件
#include<pcl/filters/statistical_outlier_removal.h> //统计滤波器头文件
#include <pcl/filters/conditional_removal.h>       //条件滤波器头文件
#include <pcl/filters/radius_outlier_removal.h>   //半径滤波器头文件
#include<pcl/io/pcd_io.h>                        //pcd读写类相关头文件
#include <pcl/visualization/cloud_viewer.h>     //

void viewerOneOff(pcl::visualization::PCLVisualizer& viewer)    //设置背景颜色
{
	viewer.setBackgroundColor(1.0f, 0.7f, 1.0f);
}


int main(int argc, char** argv)
{



	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

	//
	//*打开点云文件
	//if (pcl::io::loadPCDFile<pcl::PointXYZ>("C://Users//Administrator//Desktop//test1//Project2//rabbit.pcd", *cloud) == -1)  
	//F://3D points//Filter//filter//filter//testdata//scene_truss_rgb.pcd
	if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("F://3D points//Filter//filter//filter//testdata//scene_truss_rgb.pcd", *cloud) == -1)
		//根据自己pcd文件的路径进行添加
	{
		PCL_ERROR("Couldn't read file rabbit.pcd\n");
		return(-1);
	}

	std::cout << "原始点云数据点数：" << cloud->points.size() << std::endl << std::endl;  //打印所有点到标准错误输出

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud0(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointXYZ p;
	//删除背景的颜色信息

	for (int i = 0; i < cloud->points.size(); i++)
	{
		//删除坐标均为0的点
		if (cloud->points[i].x == 0 && cloud->points[i].y == 0 && cloud->points[i].z == 0)
		{
			continue;
		}
		else
		{
			p.x = cloud->points[i].x;
			p.y = cloud->points[i].y;
			p.z = cloud->points[i].z;
			cloud0->points.push_back(p);
		}
	}
	//设置点云为无序点云
	cloud0->width = cloud0->points.size();
	cloud0->height = 1;

	std::cout << "去除无数据点的原始点云数据点数：" << cloud0->points.size() << std::endl << std::endl;

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
	cloud1 = cloud0;


		/****************************
				数据滤波
	******************************/

	//场景数据压缩
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_voxelgrid(new pcl::PointCloud<pcl::PointXYZ>);//
	pcl::VoxelGrid<pcl::PointXYZ> voxelgrid;  //创建滤波对象
	voxelgrid.setInputCloud(cloud1);  //设置需要过滤的点云
	voxelgrid.setLeafSize(0.006f, 0.006f, 0.006f);//长宽高
	voxelgrid.filter(*cloud_after_voxelgrid);  //执行滤波处理后，存储输出cloud_after_voxelgrid
	std::cout << "体素化网格方法后点云数据点数：" << cloud_after_voxelgrid->points.size() << std::endl;

	//直通滤波
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pass(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_new(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud(cloud_after_voxelgrid);
	pass.setFilterFieldName("z");
	pass.setFilterLimits(0, 0.7);
	pass.filter(*cloud_pass);
	std::cerr << " 直通滤波方法后的点云数据: " << cloud_pass->points.size() << " 数据点" << std::endl;

	//统计数据滤波
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_StatisticalRemoval(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> Statistical;  //创建滤波器对象
	Statistical.setInputCloud(cloud_pass);   //设置待滤波的点云
	Statistical.setMeanK(150);//取平均值的临近点数
	Statistical.setStddevMulThresh(3); //设置是否为离群点的阈值：超过平均距离一个标准差以上，该点记为离群点，将其移除
	Statistical.filter(*cloud_after_StatisticalRemoval);  //执行滤波处理，保存内点到cloud_after_StatisticalRemoval-
	std::cout << "统计数据滤波后点云数据：" << cloud_after_StatisticalRemoval->points.size() << std::endl;
	//cloud_new = cloud_after_StatisticalRemoval;

	//半径滤波
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_Radius(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::RadiusOutlierRemoval<pcl::PointXYZ> radiusoutlier;  //创建滤波器
	radiusoutlier.setInputCloud(cloud_after_StatisticalRemoval);    //设置输入点云
	radiusoutlier.setRadiusSearch(0.01);     //设置半径为（）的范围内找临近点
	radiusoutlier.setMinNeighborsInRadius(6); //设置查询点的邻域点集数小于（）的删除
	radiusoutlier.filter(*cloud_after_Radius);
	std::cout << "半径滤波后点云数据点数：" << cloud_after_Radius->points.size() << std::endl;
	//cloud_new = cloud_after_Radius;
	//保存滤波处理后的点云数据
	pcl::io::savePCDFile<pcl::PointXYZ>("F://3D points//Filter//filter//filter//testdata//scene_truss.pcd", *cloud_after_Radius);


	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("点云数据滤波可视化")); // 创建一个窗口,名为3D Viewer通过智能指针创建
	viewer->initCameraParameters();
	//设置窗口的尺寸
	int v1(0);
	viewer->createViewPort(0.0, 0.5, 0.33, 1.0, v1);
	viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "cloud", v1);
	int v2(0);
	viewer->createViewPort(0.33, 0.5, 0.66, 1.0, v2);
	viewer->addPointCloud<pcl::PointXYZ>(cloud1, "cloud_no_rgb", v2);
	int v3(0);
	viewer->createViewPort(0.66, 0.5, 1, 1, v3);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_after_voxelgrid, "cloud_after_voxelgrid", v3);
	int v4(0);
	viewer->createViewPort(0.0, 0.0, 0.33, 0.5, v4);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_pass, "cloud_pass", v4);
	int v5(0);
	viewer->createViewPort(0.33, 0.0, 0.66, 0.5, v5);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_after_StatisticalRemoval, "cloud_after_StatisticalRemoval", v5);
	int v6(0);
	viewer->createViewPort(0.66, 0.0, 1.0, 0.5, v6);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_after_Radius, "cloud_after_Radius", v6);
	//设置可视化背景颜色
	viewer->setBackgroundColor(0.0, 0.0, 0.0, v1);
	viewer->setBackgroundColor(0.1, 0, 0.1, v2);
	viewer->setBackgroundColor(0.1, 0.1, 0, v3);
	viewer->setBackgroundColor(0.1, 0, 0.1, v4);
	viewer->setBackgroundColor(0.1, 0.1, 0, v5);
	viewer->setBackgroundColor(0.0, 0.1, 0.1, v6);


	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);

	}

}