reprojectImageTo3D函数

该函数将视差图，通过投影矩阵Q，得到一副映射图，图像大小与视差图相同，且每个像素具有三个通道，分别存储了该像素位置在相机坐标系下的三维点坐标在x, y, z,三个轴上的值，即每个像素的在相机坐标系下的三维坐标。

void cv::reprojectImageTo3D( 
	InputArray disparity, 	//视差图像
	OutputArray _3dImage,	//映射后存储三维坐标的图像
	InputArray Q,			//重投影矩阵 通过stereoRectify得到
	bool handleMissingValues = false, //计算得到的非正常值是否给值，如果为true则给值10000
	int ddepth = -1			//输出类型 -1 即默认为CV_32FC3 还可以是 CV_16S, CV_32S, CV_32F
)

运算如下：
$$[X,Y,Z,W{]}^T=Q\ast [x,y,disparity(x,y),1{]}^T$$
$$\_3dImage(x,y)=(X/W,Y/W,Z/W)$$

其中：
$$Q=\left[\begin{array}{cccc}1& 0& 0& -c_x\\ 0& 1& 0& -c_y\\ 0& 0& 0& f\\ 0& 0& \frac{-1}{T_x}& \frac{c_x-c_x^{\prime }}{T_x}\end{array}\right],Q\left[\begin{array}{c}x\\ y\\ d\\ 1\end{array}\right]=\left[\begin{array}{c}x-c_x\\ y-c_y\\ f\\ \frac{-d+c_x-c_x^{\prime }}{T_x}\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ Z\\ W\end{array}\right]$$
$$\frac{Z}{W}=\frac{T_{x}f}{-[d-(c_x-c_x^{\prime })]}$$

$c_x$、$c_y$为左相机主点在图像中的坐标，$f$为焦距，$T_x$为两台相机投影中心间的平移（负值），$c_x^{\prime }$是右相机主点在图像中的坐标。

这里有个地方需要注意，如果获得视差图像是CV_16S类型的，这样的视差图的每个像素值由一个16bit表示，其中低位的4位存储的是视差值得小数部分，所以真实视差值应该是该值除以16。在进行映射后应该乘以16，以获得毫米级真实位置。