根据平移、旋转参数移动坐标系.cpp

图像配准的文献

/********************************************************************
//	函数名称:   Cal2DMartrixbyParam  
//	函数功能:   根据平移、旋转参数移动坐标系，
//  入口参数:   double pPara[]	：平移和旋转量，顺序为： x, y, angle 
//  返回参数:   变换矩阵
//  创建日期:	2007/04/10
//  作者:       戚春燕
//  备注：		新坐标系原点在原坐标系的位置为（x,y)
                新坐标系的x轴正方向旋转角度为angle(弧度）
				返回原坐标系到新坐标系的变换矩阵MO
*********************************************************************/
MartrixF CRServer::Cal2DMartrixbyParam(double pPara[])
{

	MartrixF TransM(3,3);
	TransM(0,0) = cos(pPara[2]);
	TransM(0,1) = sin(pPara[2]);
	TransM(0,2) = -(pPara[0] * cos(pPara[2]) + sin(pPara[2]) * pPara[1]);
	TransM(1,0) = -sin(pPara[2]);
	TransM(1,1) = cos(pPara[2]);
	TransM(1,2) = -(-pPara[0] * sin(pPara[2]) +pPara[1] * cos(pPara[2]));
	TransM(2,0) = 0;
	TransM(2,1) = 0;
	TransM(2,2) = 1;

	return TransM;

}

/*-----------------------------------------------------------------------------
* 函数名称：	void CalMatByParam(double pPara[])
* 函数功能：	根据平移、旋转参数移动坐标系
* 入口参数：	double pPara[]	：平移和旋转量，顺序为： x, y, z, rz, rx, ry 
* 返回参数：	CRegMatrix，求得的变换矩阵（新坐标系到原坐标系）
* 备注    ：	旋转指的是绕数据 (0, 0, 0) 点旋转
-----------------------------------------------------------------------------*/
MartrixF CRServer::CalMatByParam(double pPara[])
{
	MartrixF matTran(4,4);

	matTran(0, 0) = double( cos(pPara[4])*cos(pPara[5]));
	matTran(0, 1) = double(-cos(pPara[3])*sin(pPara[5]) + sin(pPara[3])*sin(pPara[4])*cos(pPara[5]));
	matTran(0, 2) = double( sin(pPara[3])*sin(pPara[5]) + cos(pPara[3])*sin(pPara[4])*cos(pPara[5]));
	matTran(0, 3) = pPara[0];

	matTran(1, 0) = double( cos(pPara[4])*sin(pPara[5]));
	matTran(1, 1) = double( cos(pPara[3])*cos(pPara[5]) + sin(pPara[3])*sin(pPara[4])*sin(pPara[5]));
	matTran(1, 2) = double(-sin(pPara[3])*cos(pPara[5]) + cos(pPara[3])*sin(pPara[4])*sin(pPara[5]));
	matTran(1, 3) = pPara[1];

	matTran(2, 0) = double(-sin(pPara[4]));
	matTran(2, 1) = double( sin(pPara[3])*cos(pPara[4]));
	matTran(2, 2) = double( cos(pPara[3])*cos(pPara[4]));
	matTran(2, 3) = pPara[2];

	//////////////////////////////////////////////////////////////////////////

	matTran(3, 0) = 0.0f;
	matTran(3, 1) = 0.0f;
	matTran(3, 2) = 0.0f;
	matTran(3, 3) = 1.0f;

	//此时matTran为变换矩阵M，通过它可以计算某点经旋转平移变换的新坐标
	//其逆矩阵为原坐标系到新坐标系下的矩阵MO，通过它可以计算原坐标系下的点在新坐标系下的坐标
	return matTran.InverMartrix();
}