PCL将深度图转化为点云并存储为pcd文件

最近又开始折腾点云了，下面记录一点基础知识。
（1）深度相机采集的depth 数据如何转换为点云pcd
一般3D相机都会采集到depth 数据，以记录深度相机拍摄到的深度距离Z值。要将深度图转化成三维空间点云数据，首先需要相机的内参数据。
示例如下：

#include <iostream>
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/ndt.h>

#include <windows.h>


// 定义点云类型（不带颜色）
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

//---------相机内参---------------
const double camera_factor = 1000;//单位米
const double camera_fx = 611.716003; ;
const double camera_fy = 612.044861;
const double camera_cx = 631.656372; 
const double camera_cy = 401.639801; 

bool CreateDirectorySimple(const std::string& dir) 
{
	return CreateDirectoryA(dir.c_str(), NULL) || GetLastError() == ERROR_ALREADY_EXISTS;
}

std::vector<std::string> FindAllPngDepthImages(const std::string& image_dir) 
{
	std::vector<std::string> png_files;

	WIN32_FIND_DATAA find_data;
	HANDLE hFind = INVALID_HANDLE_VALUE;
	std::string pattern = image_dir + "*.png";

	hFind = FindFirstFileA(pattern.c_str(), &find_data);
	if (hFind == INVALID_HANDLE_VALUE) {
		std::cerr << "无法打开目录或未找到 PNG 文件: " << image_dir << std::endl;
		return png_files;
	}

	do {
		std::string filename = find_data.cFileName;

		// 排除目录
		if (find_data.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY)
			continue;

		// 添加 PNG 文件路径
		png_files.push_back(image_dir + filename);

	} while (FindNextFileA(hFind, &find_data) != 0);

	FindClose(hFind);
	return png_files;
}

bool ProcessDepthImage(const std::string& depth_path, const std::string& pcd_path) 
{
	// 读取深度图像
	std::cout << "找到 " << depth_path << std::endl;
	cv::Mat depth = cv::imread(depth_path, cv::IMREAD_UNCHANGED);
	if (depth.empty()) 
	{
		std::cerr << "无法加载深度图像: " << depth_path << std::endl;
		return false;
	}
	// 检查深度图像类型
	if (depth.type() != CV_16UC1 && depth.type() != CV_32FC1) {
		std::cerr << "深度图像类型不支持 (仅支持 CV_16UC1 和 CV_32FC1): " << depth_path << std::endl;
		return false;
	}
	// 创建点云对象
	PointCloud::Ptr cloud(new PointCloud);
	// 定义圆柱体过滤参数
	const double target_z = 0.97; // 目标距离1米
	const double z_tolerance = 0.5;// 深度容忍范围 ±0.05米
	const double radius = 0.65;//    // 圆柱体半径0.5米（直径1米）
	// 遍历每个像素
	for (int m = 0; m < depth.rows; m++)
	 {
		for (int n = 0; n < depth.cols; n++) 
		{
			// 获取深度值
			unsigned short d = depth.ptr<unsigned short>(m)[n];
			if (d == 0)
				continue; // 无效深度值，跳过
			// 计算空间坐标（单位：米）
			double z = double(d) / camera_factor;
			double x = (n - camera_cx) * z / camera_fx;
			double y = (m - camera_cy) * z / camera_fy;

			// --------过滤条件：提取位于指定ROI范围内的点云数据
			if (x <- 0.65)
				continue;
			if (sqrtf((z - target_z)*(z - target_z)+ (y-0.034) * (y - 0.034) ) > (radius * radius))//圆柱体过滤
			continue;
			
			// 创建点
			PointT p;
			p.x =  (x);
			p.y =  (y);
			p.z =  (z);			
			// 添加到点云
			cloud->points.push_back(p);
		}
	}
	// 设置点云属性
	cloud->height = 1;
	cloud->width = static_cast<uint32_t>(cloud->points.size());
	cloud->is_dense = false;
	std::cout << "点云大小 = " << cloud->points.size() << std::endl;
	if(cloud->points.size() == 0)  		
		return false;
	// 保存 PCD 文件
	//if (pcl::io::savePCDFileASCII(pcd_path, *cloud) == -1)//以ASCII 码形式吸入pcd
	if (pcl::io::savePCDFileBinary(pcd_path, *cloud) == -1)//以二进制形式写入pcd
	{
		PCL_ERROR("无法保存 PCD 文件: %s\n", pcd_path.c_str());
		return false;
	}
	std::cout << "已保存 PCD 文件: " << pcd_path << "，包含 " << cloud->points.size() << " 个点。" <<std::endl;
	return true;
}

int main()
{
// depth图像目录和固定输出 PCD 目录
std::string image_dir = "F:/Data/depth/";      //深度图数据路径
std::string pcd_dir = "F:/Data/pcd12/";     // 输出的pcdl文件路径示例
int index = 0 ;
// 创建固定输出目录（不检查是否存在）
CreateDirectorySimple(pcd_dir);

// 查找所有 PNG 深度图
std::vector<std::string> depth_images = FindAllPngDepthImages(image_dir);
std::cout << "找到 " << depth_images.size() << " 个 PNG 深度图。" << std::endl;

// 处理每个深度图
for (const auto& depth_path : depth_images) 
{
	index = index + 1;
	std::cout << "已经处理了: " << index << std::endl;
	//if (index % 2 != 1)
		//continue;
	// 提取文件名（不含路径）
	size_t pos = depth_path.find_last_of("/\\");
	std::string filename = (pos == std::string::npos) ? depth_path : depth_path.substr(pos + 1);

	// 提取基名（去除扩展名）
	size_t dot = filename.find_last_of('.');
	std::string base = (dot == std::string::npos) ? filename : filename.substr(0, dot);

	// 定义 PCD 文件路径
	std::string pcd_path = pcd_dir + base + ".pcd";
	
	// 处理并保存 PCD
	bool success = ProcessDepthImage(depth_path, pcd_path);
	if (!success) {
		std::cerr << "处理深度图失败: " << depth_path << std::endl;
	}
  }
  return 0;
}

（2）为什么用pcl写点云数据到pcd文件后用记事本打开乱码
如果写入pcd 的数据是按照ASCII码写的，其一定可以用文本文件打开，如果用二进制类型写的打开就是乱码。
上面的代码中的语句
1）if(pcl::io::savePCDFileASCII(pcd_path, *cloud) == -1)//以ASCII 码形式吸入pcd,写速度慢，文件大，但可以用文本文件查看点云坐标；
2）if (pcl::io::savePCDFileBinary(pcd_path, *cloud) == -1)//以二进制形式写入pcd，写速度快，文件小。

好了，先这样吧，pcl 博大精深，后面继续~