车载环视拼接系统之 获取每个摄像头相对于坐标系原点的位置信息

在做车载环视拼接系统有时需要用到一些信息：

比如想获取4个摄像头相对于汽车中心的信息，也即摄像头高度（z轴距离），摄像头相对于车中心的横向偏移（X轴距离），摄像头相对于车中心的竖向距离（Y轴距离）。如果以 汽车中心点为世界坐标系原点，那么需要获取的信息即相当于摄像机坐标到世界坐标系的平移向量。

注意： 坐标轴定义不一样，得出的旋转向量意义也不一样。

由于求出的外参旋转平移矩阵是指 世界坐标系到相机坐标系。对该矩阵求逆矩阵 就可以得到 摄像机坐标到世界坐标系的平移向量。

世界坐标系的一点（Xw,Yw,Zw）  到相机坐标系的一点（Xc,Yc,Zc）的数学表达式如下：（具体推导过程可参考之前文章：车载环视拼接系统的设计与实现(二)：摄像机成像原理）

 

参考代码：

void TranstationVetorChange(CvMat *Transtation,CvMat *TranstationMatrix ,CvMat *Rotation_Matrix)
{

    /* RT 从世界坐标到图像坐标系的旋转平移矩阵，RT_INV为逆矩阵*/
	CvMat *RT = cvCreateMat(4,4,CV_64FC1);
	CvMat *RT_INV = cvCreateMat(4,4,CV_64FC1);
	RT->data.db[0] = Rotation_Matrix->data.db[0];
	RT->data.db[1] = Rotation_Matrix->data.db[1];
	RT->data.db[2] = Rotation_Matrix->data.db[2];
	RT->data.db[3] = TranstationMatrix->data.db[0];

	RT->data.db[4] = Rotation_Matrix->data.db[3];
	RT->data.db[5] = Rotation_Matrix->data.db[4];
	RT->data.db[6] = Rotation_Matrix->data.db[5];
	RT->data.db[7] = TranstationMatrix->data.db[1];

	RT->data.db[8] = Rotation_Matrix->data.db[6];
	RT->data.db[9] = Rotation_Matrix->data.db[7];
	RT->data.db[10] = Rotation_Matrix->data.db[8];
	RT->data.db[11] = TranstationMatrix->data.db[2];

	RT->data.db[12] = 0;
	RT->data.db[13] = 0;
	RT->data.db[14] = 0;
	RT->data.db[15] = 1;

    /* 求旋转平移逆矩阵*/
    cvInvert(RT,RT_INV);
	Transtation->data.db[0] = RT_INV->data.db[3];
	Transtation->data.db[1] = RT_INV->data.db[7];
    Transtation->data.db[2] = RT_INV->data.db[11];

	cvReleaseMat(&RT);
	cvReleaseMat(&RT_INV);
}