Intel realsense SR300深度图获取点云和点云投影至图像全流程

从深度图获取点云

之前用的方法一直都是利用深度图的像素坐标（x,y）和像素值z自行计算点云，代码如下

for (int m = 0; m < depth.rows; m++)
	{
		for (int n = 0; n < depth.cols; n++)
		{
			// 获取深度图中(m,n)处的值
			uchar d = depth.ptr<uchar>(m)[n];
			// d 可能没有值，若如此，跳过此点
			if (d == 0)
				continue;
			// d 存在值，则向点云增加一个点
			PointT p;
			// 计算这个点的空间坐标/ camera_factor
			p.z = (300-double(d))*4;
			p.x = (m - camera_cx) * p.z / camera_fx;
			p.y = (n - camera_cy) * p.z / camera_fy;
			// 从rgb图像中获取它的颜色
			//彩色图是三通道的BGR格式图，所以按下面的顺序获取颜色

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

但这样计算出来的点云有一些问题，表现为图片中的直角转成点云后会变成45度的角，原因不明，查阅资料后发现可以使用librealsense自带的API完成深度图到点云的转换

主要参考官方example，利用example里面实现的pcl_ptr points_to_pcl(const rs2::points& points)这个函数就可以完成从realsenseSDK中的点云到PCL点云的转换，几行代码就可以实现，比较简单

// Declare pointcloud object, for calculating pointclouds and texture mappings
rs2::pointcloud pc;
// We want the points object to be persistent so we can display the last cloud when a frame drops
rs2::points points;
    
auto frames = pipe.wait_for_frames();

auto depth = frames.get_depth_frame();

 // Generate the pointcloud and texture mappings
points = pc.calculate(depth);
auto pcl_points = points_to_pcl(points);

如果使用了rs2::align来对齐深度和彩色帧，将对齐后的depth_frame传入calculate函数同样可以使用

将点云中的点投影回深度/彩色图

注意：realsense直接获得的深度图和彩色图是不重合的，需要使用rs2::align后才能对齐
用法如下

	rs2::pipeline pipe;     //Contruct a pipeline which abstracts the device
	rs2::config cfg;    //Create a configuration for configuring the pipeline with a non default profile

	cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30);
	cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
	rs2_stream align_to = RS2_STREAM_COLOR;
	rs2::align align(align_to);


	while(true)
	{
		rs2::frameset frames;
		frames = pipe.wait_for_frames();
        rs2::pointcloud pc;
        rs2::points points;

		auto processed = align.process(frames);
		
		//Get each frame
		auto color_frame = processed.get_color_frame();
		auto depth_frame = processed.get_depth_frame();
	}

接下来的内容主要参考官方wiki

要将点云中的三维点投影回图像，需要用到void rs2_project_point_to_pixel(float pixel[2], const rs2_intrinsics* intrin, const float point[3])
pixel 是传入的空数组，程序会将计算出的xy坐标放在里面
intrin 是相机内参
point 是点云中的点，顺序是x,y,z
realsense就不能搞个OpenCV一样规整一点的文档吗，真的找啥也难

获取相机内参
rs2_intrinsics是一个结构体，可以通过`get_intrinsics()``函数获取
具体用法如下（接上方align的代码）

		auto color_frame = processed.get_color_frame();
		auto depth_frame = processed.get_depth_frame();
        points = pc.calculate(depth_frame);
        auto prof=depth_frame.get_profile().as<rs2::video_stream_profile>();
        auto i=prof.get_intrinsics();

如果前面用过align，那么一定要用对齐后的frame来获取内参矩阵
如果没做对齐，这个get_intrinsics()是rs2::video_stream_profile类的成员函数，为了获取这个类，首先需要利用rs2::depth_frame类的depth_frame.get_profile()函数获得rs2::stream_profile类的对象，再利用rs2::stream_profile类的成员函数as()获得rs2::video_stream_profile类，然后就可以用这个函数了
我也不知道librealsense在干什么

rs2_intrinsics的具体定义在这里，考虑到内参矩阵不变，可以获取之后打印出来记下，然后自己构建一个rs2_intrinsics的实例直接拿来用