点云与深度图像的相互转换

1. 深度图像转点云

• 深度图转点云 并进行欧式聚类分割等处理
• 主要是将相机内参替换成自己的参数，图像路径改一改就行

//图像到点云的转换

//C++ 标准库
#include 
#include 
//using namespace std;

//OpenCV 库
#include 
#include 

//PCL 库
#include 
#include 
#include 
#include 
#include 
#include 
#include //基于采样一致性分割的类的头文件
#include //去除离群点
#include 	//根据索引提取内点
#include //欧式聚类分割相关头文件
#include 

//定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBA>);
pcl::PointXYZRGBA p_min, p_max;

//定义颜色数组用于可视化
float colors[] = {
	255, 0,   0,   // red 		1
	0,   255, 0,   // green		2
	0,   0,   255, // blue		3
	255, 255, 0,   // yellow	4
	0,   255, 255, // light blue5
	255, 0,   255, // magenta   6
	255, 255, 255, // white		7
	255, 128, 0,   // orange	8
	255, 153, 255, // pink		9
	51,  153, 255, //			10
	153, 102, 51,  //			11
	128, 51,  153, //			12
	153, 153, 51,  //			13
	163, 38,  51,  //			14
	204, 153, 102, //			15
	204, 224, 255, //			16
	128, 179, 255, //			17
	206, 255, 0,   //			18
	255, 204, 204, //			19
	204, 255, 153, //			20
};

相机内参
//const double camera_factor = 1000;   //定义一个double类型的常变量（它的值不准修改）
//const double camera_cx = 312.0;
//const double camera_cy = 241.6;
//const double camera_fx = 599.9;
//const double camera_fy = 599.8;

//相机内参
const double camera_factor = 1000;   //定义一个double类型的常变量（它的值不准修改）
const double camera_cx = 1595.4377;
const double camera_cy = 1001.2092;
const double camera_fx = 2407.4058;
const double camera_fy = 2409.0186;

//直通滤波
void PassThrough(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr incloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr outcloud, float min, float max, int mode)
{
	//设置滤波器对象
	pcl::PassThrough<pcl::PointXYZRGBA> pass;
	//设置输入点云
	pass.setInputCloud(incloud);
	//设置过滤是所需要的点云类型的y字段
	switch (mode)
	{
	case 1:
		pass.setFilterFieldName("x");
		break;
	case 2:
		pass.setFilterFieldName("y");
		break;
	case 3:
		pass.setFilterFieldName("z");
		break;
	}
	//设置在过滤字段上的范围 默认保留范围内的点云
	pass.setFilterLimits(min, max);
	//不设置默认为false即保留范围内点云，true则保留范围外的点云
	//pass.setFilterLimitsNegative (true); 
	//执行滤波，保存过滤结果
	pass.filter(*outcloud);
};
void StatisticalOutlierRemoval(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr incloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr outcloud, int k, double sigma)
{
	//创建滤波器对象
	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGBA> sor;
	//设置待滤波的点云
	sor.setInputCloud(incloud);
	//设置用于平均距离估计的 KD-tree最近邻搜索点的个数
	sor.setMeanK(k);
	//设置判断是否为离群点的阀值;高斯分布标准差的倍数, 也就是 u+1*sigma,u+2*sigma,u+3*sigma 中的倍数1、2、3 
	sor.setStddevMulThresh(sigma);
	//存储滤波后的点云
	sor.filter(*outcloud);
	//下面这个参数不设置默认为false，true为取被过滤的点
	//sor.setNegative(true);
}
void RadiusOutlierRemoval(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr incloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr outcloud, double r, int n)
{
	//创建滤波器
	pcl::RadiusOutlierRemoval<pcl::PointXYZRGBA> outrem;
	//输入点云
	outrem.setInputCloud(incloud);
	//设置半径查找范围为2
	outrem.setRadiusSearch(r);
	//在半径范围内点数小于12的点滤除.
	outrem.setMinNeighborsInRadius(n);
	//保留存储半径范围内点数大于12的点
	outrem.filter(*outcloud);
}
void RANSAC(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr incloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr outcloud, pcl::PointIndices::Ptr inliers)
{
	//创建一个模型参数对象，用于记录结果
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	//inliers表示误差能容忍的点 记录的是点云的序号
	//pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	// 创建一个分割器
	pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
	// Optional设置对估计的模型做优化处理
	seg.setOptimizeCoefficients(true);
	// Mandatory-设置目标几何形状
	seg.setModelType(pcl::SACMODEL_PLANE);
	//分割方法：随机采样法
	seg.setMethodType(pcl::SAC_RANSAC);
	//设置误差容忍范围
	seg.setDistanceThreshold(0.1);
	//输入点云
	seg.setInputCloud(incloud);

	//分割点云,得到参数
	seg.segment(*inliers, *coefficients);
	pcl::copyPointCloud<pcl::PointXYZRGBA>(*incloud, *inliers, *outcloud);
}
void ExtractIndices(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr incloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr outcloud, pcl::PointIndices::Ptr index)
{
	//提取1,3,4位置处点云
	//std::vector index = { 1,3,4 };
	//boost::shared_ptr> index_ptr = boost::make_shared>(index);
	//Create the filtering object
	pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
	//Extract the inliers
	extract.setInputCloud(incloud);
	extract.setIndices(index);
	extract.setNegative(true);//如果设为true,可以提取指定index之外的点云
	extract.filter(*outcloud);

	//提取子集构成一个新的点云
	//std::vector indexs = { 1, 2, 5 };//声明索引值
	//pcl::copyPointCloud(*incloud, indexs, *outcloud);//将对应索引的点存储
}
//欧式聚类分割
void EuclideanClusterExtraction(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr incloud)
{
	//pcl::PointXYZ center;
	pcl::PCDWriter writer;
	//Creating the KdTree object for the search method of the extraction
	//pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
	//tree->setInputCloud(incloud);//将无法提取平面的点云作为cloud_filtered
	std::vector<pcl::PointIndices> cluster_indices;//保存每一种聚类，每一种聚类下还有具体的聚类的点
	pcl::EuclideanClusterExtraction<pcl::PointXYZRGBA> ec;//实例化一个欧式聚类提取对象
	//ec.setClusterTolerance(0.005); // 近邻搜索的搜索半径为2cm，重要参数
	ec.setClusterTolerance(0.02);
	ec.setMinClusterSize(99);//设置一个聚类需要的最少点数目为100
	ec.setMaxClusterSize(999999);//一个聚类最大点数目为25000
	ec.setSearchMethod(tree);//设置点云的搜索机制
	ec.setInputCloud(incloud);//设置输入点云
	ec.extract(cluster_indices);//将聚类结果保存至cluster_indices中

	//可视化写在这里是为了给每个聚类设置不同的颜色
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Point cloud processing"));
	int v1(0);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer->setBackgroundColor(0.1, 0.1, 0.1, v1);
	viewer->addText("before", 10, 10, "v1 text", v1);
	viewer->addPointCloud<pcl::PointXYZRGBA>(incloud, "cloud_in", v1);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_in", v1);
	int v2(0);
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);
	viewer->addText("after", 10, 10, "v2 text", v2);

	//my->NormalEstimation(incloud, cloud_normals);
	//其中，参数100表示整个点云中每100个点显示一个法向量（若全部显示，可设置为1，可根据自己要求设置）；  0.9表示法向量的长度
	//viewer->addPointCloudNormals(incloud, cloud_normals, 100, 66, "id");
	//迭代访问点云索引cluster_indices，直到分割出所有聚类,一个循环提取出一类点云
	int j = 0;
	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGBA>);
		//创建新的点云数据集cloud_cluster，直到分割出所有聚类
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
		{
			cloud_cluster->points.push_back(incloud->points[*pit]);
		}
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;
		std::stringstream ss;
		ss << "cloud_cluster_" << j << ".pcd";
		writer.write<pcl::PointXYZRGBA>(ss.str(), *cloud_cluster, false);
		cout << "-----" << ss.str() << "-----" << endl;
		//cout << *cloud_cluster << endl;
		std::cout << "Segmentation" << cloud_cluster->size() << endl;
		viewer->addPointCloud<pcl::PointXYZRGBA>(cloud_cluster, ss.str(), v2);
		//pcl::PointXYZ a;
		//a=Centroid(cloud_cluster);
		//string id = ss.str() + "1";
		//string id1 = ss.str() + "2";
		//viewer->addSphere(a, 0.02, 1, 1, 1, id, v2);
		//cout << "点云质心：" << a << endl;
		//my->NormalEstimation(cloud_cluster, cloud_normals);
		//viewer->addPointCloudNormals(cloud_cluster, cloud_normals, 100, 1, id1,v2);
		//设置点云大小
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, ss.str(), v2);
		//pcl::visualization::PointCloudColorHandlerCustom single_color(cloud_cluster, j*50, j*30, j);//这种不行都是同一个颜色
		//设置点云颜色 其中colors取值范围为0-1
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, colors[j * 3] / 255, colors[j * 3 + 1] / 255, colors[j * 3 + 2] / 255, ss.str(), v2);
		j++;
	}
	//Eigen::Matrix4f rotation_x = Eigen::Matrix4f::Identity();//定义绕X轴的旋转矩阵，并初始化为单位阵
	//double angle_x = M_PI / 2;//旋转90°
	//rotation_x(1, 1) = cos(angle_x);
	//rotation_x(1, 2) = -sin(angle_x);
	//rotation_x(2, 1) = sin(angle_x);
	//rotation_x(2, 2) = cos(angle_x);
	//pcl::transformPointCloud(*cloud, *cloud_transformed, rotation_x);

	//viewer->resetCamera();
	viewer->setCameraPosition(-0.7, -0.4, -1, -0.7, -0.4, 1, 0, -1, 0);
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}

void visualization(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("cloud"));

	//可以对点云设置一个统一的颜色
	//pcl::visualization::PointCloudColorHandlerCustom single_color(cloud, 0, 255, 0);
	//对点云进行渲染
	//pcl::visualization::PointCloudColorHandlerGenericField fildColor(cloud, "x");
	//pcl::visualization::PointCloudColorHandlerRGBField rgb(cloud);
	viewer->addPointCloud<pcl::PointXYZRGBA>(cloud, "example");
	//最后一个参数为添加的点云的id
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "example");

	//这个控制放大
	//viewer->setCameraFieldOfView(0.5);
	//viewer->setCameraClipDistances(0.1, 572);
	//viewer->setCameraPosition(-1, 0, 0, 0, 0, 0, 0, 0, 1);
	viewer->resetCamera();
	//viewer->spinOnce();
	while (!viewer->wasStopped()) {
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}


//主函数
int main(int argc, char** argv)
{
	// 读取./data/rgb.png和./data/depth.png，并转化为点云

	// 图像矩阵
	cv::Mat rgb, depth;
	// rgb 图像是8UC3的彩色图像
	rgb = cv::imread("E:/DataSet/lidar/translate/RGB.bmp");
	// depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
	depth = cv::imread("E:/DataSet/lidar/translate/D.tiff", -1);

	// 点云变量
	// 使用智能指针，创建一个空点云。这种指针用完会自动释放。
	PointCloud::Ptr cloud(new PointCloud);
	PointCloud::Ptr cloud1(new PointCloud);
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

	// 遍历深度图
	for (int m = 0; m < depth.rows; m++)
	{
		for (int n = 0; n < depth.cols; n++)
		{
			//获取深度图中(m,n)处的值
			//cv::Mat的ptr函数会返回指向该图像第m行数据的头指针。然后加上位移n后，这个指针指向的数据就是我们需要读取的数据.
			ushort d = depth.ptr<ushort>(m)[n];
			// d 可能没有值，若如此，跳过此点
			if (d == 0)
				continue;
			//d 存在值，则向点云增加一个点
			PointT p;

			// 计算这个点的空间坐标
			p.z = double(d) / camera_factor;
			p.x = (n - camera_cx) * p.z / camera_fx;
			p.y = (m - camera_cy) * p.z / camera_fy;

			// 从rgb图像中获取它的颜色
			// rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
			p.b = rgb.ptr<uchar>(m)[n * 3];
			p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
			p.r = rgb.ptr<uchar>(m)[n * 3 + 2];

			//把p加入到点云中
			cloud->points.push_back(p);
		}
	}

	// 设置并保存点云
	cloud->height = 1;
	cloud->width = cloud->points.size();
	pcl::getMinMax3D(*cloud, p_min, p_max);
	std::cout << "points" << cloud->size() << endl;
	std::cout << "minpoint" << p_min.x << "," << p_min.y << "," << p_min.z << endl;
	std::cout << "maxpoint" << p_max.x << "," << p_max.y << "," << p_max.z << endl;
	cloud->is_dense = false;
	pcl::io::savePCDFile("pointcloud2.pcd", *cloud);
	// 清除数据并退出
	cloud->points.clear();
	cout << "Point cloud saved." << endl;
	pcl::io::loadPCDFile("pointcloud2.pcd", *cloud);
	//RANSAC(cloud, cloud1, inliers);
	EuclideanClusterExtraction(cloud);
	//ExtractIndices(cloud, cloud1, inliers);
	//StatisticalOutlierRemoval(cloud, cloud1,500,0.05);
	//RadiusOutlierRemoval(cloud, cloud1,2,1000);
	//pcl::io::savePCDFile("pointcloud1.pcd", *cloud1);
	//visualization(cloud1);
	return 0;
}

1.1 效果图

2. 点云转深度图

• 点云转深度图 并且保存生成的深度图像

#define  _CRT_SECURE_NO_WARNINGS
#pragma warning(disable:4996)
 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
//OpenCV 库
#include 
#include 
 
void
setViewerPose(pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
	Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
	Eigen::Vector3f look_at_vector = viewer_pose.rotation() * Eigen::Vector3f(0, 0, 1) + pos_vector;
	Eigen::Vector3f up_vector = viewer_pose.rotation() * Eigen::Vector3f(0, -1, 0);
	viewer.setCameraPosition(
		pos_vector[0],
		pos_vector[1],
		pos_vector[2],
		look_at_vector[0],
		look_at_vector[1],
		look_at_vector[2],
		up_vector[0],
		up_vector[1],
		up_vector[2]);
}
 
#if 1
int
main(int argc, char** argv) {
	
	//pcl::PointCloud pointCloud;
	生成数据
	//for (float y = -0.5f; y <= 0.5f; y += 0.01f)
	//{
	//	for (float z = -0.5f; z <= 0.5f; z += 0.01f)
	//	{
	//		pcl::PointXYZ point;
	//		point.x = 2.0f - y;
	//		point.y = y;
	//		point.z = z;
	//		pointCloud.points.push_back(point);
	//	}
	//}
	//pointCloud.width = (uint32_t)pointCloud.points.size();
	//pointCloud.height = 1;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("pointcloud2.pcd", *cloud);
	pcl::PointCloud<pcl::PointXYZ> pointCloud;
	pointCloud = *cloud;
	//以1度为角分辨率，从上面创建的点云创建深度图像。
	float angularResolution = (float)(1.0f * (M_PI / 180.0f));  // 1度转弧度
	//模拟深度传感器在水平方向的最大采样角度:360°视角。
	float maxAngleWidth = (float)(360.0f * (M_PI / 180.0f));  // 360.0度转弧度
	//模拟深度传感器在垂直方向的最大采样角度:180°视角。
	float maxAngleHeight = (float)(180.0f * (M_PI / 180.0f)); // 180.0度转弧度
	/*
	sensorPose定义模拟深度传感器的自由度位置：
	横滚角roll、俯仰角pitch、偏航角yaw，默认初始值均为0。
	*/
	Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
	/*
	定义坐标轴正方向：
	CAMERA_FRAME：X轴向右，Y轴向下，Z轴向前；
	LASER_FRAME：X轴向前，Y轴向左，Z轴向上；
	*/
	pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
	/*
	noiseLevel设置周围点对当前点深度值的影响：
	如 noiseLevel=0.05，可以理解为，深度距离值是通过查询点半径为 Scm 的圆内包含的点用来平均计算而得到的。
	*/
	float noiseLevel = 0.00;
	/*
	min_range设置最小的获取距离，小于最小获取距离的位置为盲区
	*/
	float minRange = 0.0f;
	/*border_size获得深度图像的边缘的宽度：在裁剪图像时，如果 borderSize>O ，将在图像周围留下当前视点不可见点的边界*/
	int borderSize = 1;
 
	boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
	pcl::RangeImage& range_image = *range_image_ptr;
	range_image.createFromPointCloud(pointCloud, angularResolution, maxAngleWidth, maxAngleHeight, sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
	std::cout << range_image << "\n";
 
	//创建3D视图并且添加点云进行显示
	pcl::visualization::PCLVisualizer viewer("3D Viewer");
	viewer.setBackgroundColor(0, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler(range_image_ptr, 255, 255, 255);
	viewer.addPointCloud(range_image_ptr, range_image_color_handler, "range image");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
	viewer.resetCamera();
	viewer.initCameraParameters();
	setViewerPose(viewer, range_image.getTransformationToWorldSystem());

	float* ranges = range_image.getRangesArray();
	unsigned char* rgb_image = pcl::visualization::FloatImageUtils::getVisualImage(ranges, range_image.width, range_image.height);
	pcl::io::saveRgbPNGFile("ha.png", rgb_image, range_image.width, range_image.height);
 
	//显示深度图像
	pcl::visualization::RangeImageVisualizer range_image_widget("Range image");
	//range_image_widget.setRangeImage(range_image);           
	range_image_widget.showRangeImage(range_image); //图像可视化方式显示深度图像
	Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
	//主循环
	while (!viewer.wasStopped())
	{
		range_image_widget.spinOnce();
		viewer.spinOnce();
		//pcl_Sleep(0.01);
		if (1)
		{
			scene_sensor_pose = viewer.getViewerPose();
			range_image.createFromPointCloud(pointCloud,
				angularResolution, pcl::deg2rad(360.0f),
				pcl::deg2rad(180.0f),
				scene_sensor_pose,
				pcl::RangeImage::LASER_FRAME,
				noiseLevel,
				minRange,
				borderSize);
			range_image_widget.showRangeImage(range_image);
		}
	}
 
}
#endif

2.2 效果图

• 嗯生成的深度图非常的小
  

3. 无法打开输入文件“vtkViewsInfovisopencv_calib3d.lib”

• 之前同时配置PCL和opencv时就会报无法打开输入文件“vtkViewsInfovisopencv_calib3d.lib”这个错，每次都手动去链接器里删除这个lib，如下图。
• 今天我尝试着修改了下CMakeLists.txt才发现，之前是因为CMakeLists.txt中的TARGET_LINK_LIBRARIES里面的${PCL_LIBRARIES} ${OpenCV_LIBS}写的时候两个之间没有空格导致报错，加上空格就不会生成这个奇怪的lib和错误了。