Matlab实现保存的图片深度图像与彩色图像坐标对齐

在使用realsense或者Kinect的时候，可以选择获取深度对齐到彩色空间的数据流保存，或者彩色对齐到深度空间的数据流

然而如果拿到的数据是别人做的数据集，只有没有配准的图像对，以及红外和RGB的相机内参，外参。需要自己来做深度与彩色图像对齐的工作。

原理就是把深度和彩色的图像坐标系下的点转换到相机坐标系下，然后根据红外和彩色镜头的光心外参来进行转换，从而找出深度图上的点与彩色图上的点的对应关系。

原理：

https://www.cnblogs.com/cv-pr/p/5769617.html

验证了一下代码，有一些问题，解决中

#include <stdint.h>
#include <iostream>
#include <opencv.hpp>
#include <opencv2\highgui.hpp>			

using namespace std;
using namespace cv;

void Depth2RGB(cv::Mat &src, cv::Mat &dst, const float *W)
{

	double  z;
	uint16_t u, v, d;
	uint16_t u_rgb, v_rgb;
	cv::Mat newdepth(dst.rows, dst.cols, CV_16UC1, cv::Scalar(0));
	for (v = 0; v < src.rows; v++)
	{
		for (u = 0; u < src.cols; u++)
		{
			d = src.at<uint16_t>(v, u);
			z = (double)d;
			u_rgb = (uint16_t)((W[0] * (double)u + W[1] * (double)v + W[2] + W[3] / z));
			v_rgb = (uint16_t)((W[4] * (double)u + W[5] * (double)v + W[6] + W[7] / z));
			if (u_rgb < 0 && u_rgb >= newdepth.cols && v_rgb < 0 && v_rgb >= newdepth.rows)
			{
				uint16_t *val = (uint16_t *)newdepth.ptr<uchar>(v_rgb)+u_rgb; 
				*val = d;
			}
		}
	}
	dst = newdepth;
	imshow("d2c", dst);
	imwrite("d2c.png", dst);
	waitKey(0);
}

void main()
{
	const float W[] = { 2.93606726333117, -0.00837188856195977, 11.5181662036237, 39521.9782777880,
		0.0126790367599438, 2.94109063708170, -188.560867868575, 3812.55148602000,
		-2.04121061076977e-06, 5.77401359963446e-06, 0.999224387130484, 3.90200000000000,
		0, 0, 0, 1 };
	Mat dpt, rgb;
	dpt = imread("F:\\DataSet\\FHADatset\\Video_files\\Subject_1\\put_salt\\1\\depth\\depth_0000.png",-1);
	rgb = imread("F:\\DataSet\\FHADatset\\Video_files\\Subject_1\\put_salt\\1\\color\\color_0000.jpeg",-1);
	//imshow("depth", dpt);
	//imshow("color