已知两平面对应三个点对，获取旋转矩阵

条件：知道两个圆环上对应的三个点对，计算两个圆环的旋转矩阵

1、C++代码实现

/*计算圆心变量*/
const double Pi = 3.1415926;
const double ERR_PRECISION = 0.000001;
double buff_set_Circle1foundpoint[3] = { 0 };//右侧舱段
double buff_set_Circle2foundpoint[3] = { 0 };
double buff_set_Circle3foundpoint[3] = { 0 };

double buff_set_LCircle1foundpoint[3] = { 0 };//左侧舱段
double buff_set_LCircle2foundpoint[3] = { 0 };
double buff_set_LCircle3foundpoint[3] = { 0 };

/*计算圆心变量*/

/*计算圆心函数*/
int  Cal_ARC(const double Point_sta[], const double Point_mid[], const double Point_end[], double *center, double *R, double * arc);
void CircleTest(double a[], double b[], double c[]);

double Circle_Radius,Circle_Center[3];

//计算左边圆心
double l_r,Left_Circle_center[3];

//计算右边圆心
double r_r,Right_Circle_center[3];
/*计算圆心函数*/

Mat P_LC(1,3, CV_32F);
Mat PL1(1,3, CV_32F);
Mat n_l(1,3, CV_32F);
Mat a_l(1,3, CV_32F);
Mat o_l(1,3, CV_32F);

Mat P_RC(1,3, CV_32F);
Mat PR1(1,3, CV_32F);
Mat n_r(1,3, CV_32F);
Mat a_r(1,3, CV_32F);
Mat o_r(1,3, CV_32F);

Mat R_L(3,3, CV_32F);
Mat R_R(3,3, CV_32F);
Mat ExtrinsicL(3,4, CV_32F);
Mat ExtrinsicR(3,4, CV_32F);
Mat TL(4,4, CV_32F);
Mat TR(4,4, CV_32F);
Mat R_cabin(4,4,CV_32F);

float p_lc[1][3];
float p_rc[1][3];

float pl1[1][3];
float pr1[1][3];

float normall[1][3];
float normalr[1][3];

Mat A, B;//中间变量

float add_2[1][4] = { 0, 0, 0, 1 };
Mat ADD_2(1, 4, CV_32F);

void main(void)
{

	Mat ADD_2(1, 4, CV_32F, add_2);

	//左边舱段
	buff_set_LCircle1foundpoint[0] = -50.418884;
	buff_set_LCircle2foundpoint[0] = -40.394142;
	buff_set_LCircle3foundpoint[0] = -30.644667;

	buff_set_LCircle1foundpoint[1] = -663.10748;
	buff_set_LCircle2foundpoint[1] = -558.31946;
	buff_set_LCircle3foundpoint[1] = -517.80524;

	buff_set_LCircle1foundpoint[2] = -304.63519;
	buff_set_LCircle2foundpoint[2] = -357.23822;
	buff_set_LCircle3foundpoint[2] = -488.86584;

	//右边舱段
	buff_set_Circle1foundpoint[0] = -59.06461;
	buff_set_Circle2foundpoint[0] = -50.390839;
	buff_set_Circle3foundpoint[0] = -43.202301;

	buff_set_Circle1foundpoint[1] = -660.91101;
	buff_set_Circle2foundpoint[1] = -556.76605;
	buff_set_Circle3foundpoint[1] = -529.6546;

	buff_set_Circle1foundpoint[2] = -305.25043;
	buff_set_Circle2foundpoint[2] = -358.20734;
	buff_set_Circle3foundpoint[2] = -500.07596;


	//计算左边圆心
	CircleTest(buff_set_LCircle1foundpoint, buff_set_LCircle2foundpoint, buff_set_LCircle3foundpoint);
	l_r = Circle_Radius;
	Left_Circle_center[0] = Circle_Center[0];
	Left_Circle_center[1] = Circle_Center[1];
	Left_Circle_center[2] = Circle_Center[2];

	cout<<"l_r="<<l_r<<endl;
	cout<<"Left_Circle_center[0]="<<Left_Circle_center[0]<<endl;
	cout<<"Left_Circle_center[1]="<<Left_Circle_center[1]<<endl;
	cout<<"Left_Circle_center[2]="<<Left_Circle_center[2]<<endl;

	//计算右边圆心
	CircleTest(buff_set_Circle1foundpoint, buff_set_Circle2foundpoint, buff_set_Circle3foundpoint);
	r_r = Circle_Radius;
	Right_Circle_center[0] = Circle_Center[0];
	Right_Circle_center[1] = Circle_Center[1];
	Right_Circle_center[2] = Circle_Center[2];

	cout<<"r_r="<<r_r<<endl;
	cout<<"Right_Circle_center[0]="<<Right_Circle_center[0]<<endl;
	cout<<"Right_Circle_center[1]="<<Right_Circle_center[1]<<endl;
	cout<<"Right_Circle_center[2]="<<Right_Circle_center[2]<<endl;

	//计算左边法向量
	Point3f POINT_L;
	Cal_norm(buff_set_LCircle1foundpoint,buff_set_LCircle2foundpoint,buff_set_LCircle3foundpoint,POINT_L);

	//计算右边法向量
	Point3f POINT_R;
	Cal_norm(buff_set_Circle1foundpoint,buff_set_Circle2foundpoint,buff_set_Circle3foundpoint,POINT_R);

	//计算左边x轴方向向量
	p_lc[0][0]=Left_Circle_center[0];
	p_lc[0][1]=Left_Circle_center[1];
	p_lc[0][2]=Left_Circle_center[2];

	Mat P_LC(1, 3, CV_32F, p_lc);
	cout<<"P_LC"<<P_LC<<endl;

	pl1[0][0]=buff_set_LCircle1foundpoint[0];
	pl1[0][1]=buff_set_LCircle1foundpoint[1];
	pl1[0][2]=buff_set_LCircle1foundpoint[2];

	Mat PL1(1, 3, CV_32F, pl1);
	cout<<"PL1"<<PL1<<endl;

	n_l = PL1-P_LC;
	//cout<<"n_l"<<n_l<<endl;
	n_l = n_l / norm(n_l);
	cout << "n_l" << n_l << endl;

	//计算左边z轴方向向量
	normall[0][0]=-POINT_L.x;
	normall[0][1]=-POINT_L.y;
	normall[0][2]=-POINT_L.z;

	Mat a_l(1, 3, CV_32F, normall);
	cout<<"a_l"<<a_l<<endl;

    //计算左边y轴方向向量
	o_l.at<float>(0, 0) = a_l.at<float>(0, 1)*n_l.at<float>(0, 2) - a_l.at<float>(0, 2)*n_l.at<float>(0, 1);
	o_l.at<float>(0, 1) = a_l.at<float>(0, 2)*n_l.at<float>(0, 0) - a_l.at<float>(0, 0)*n_l.at<float>(0, 2);
	o_l.at<float>(0, 2) = a_l.at<float>(0, 0)*n_l.at<float>(0, 1) - a_l.at<float>(0, 1)*n_l.at<float>(0, 0);
	cout<<"o_l"<<o_l<<endl;

	hconcat(n_l.t(), o_l.t(), A);
	cout << "A" << A << endl;
	hconcat(A, a_l.t(), R_L);
	cout << "R_L" << R_L << endl;
	
	hconcat(R_L, P_LC.t(), ExtrinsicL);
	cout << "ExtrinsicL" << ExtrinsicL << endl;
	
	vconcat(ExtrinsicL, ADD_2, TL);
	cout << "TL" << TL << endl;

	//R_cabin = 

	//计算右边x轴方向向量
	p_rc[0][0]=Right_Circle_center[0];
	p_rc[0][1]=Right_Circle_center[1];
	p_rc[0][2]=Right_Circle_center[2];

	Mat P_RC(1, 3, CV_32F, p_rc);
	cout<<"P_RC"<<P_RC<<endl;

	pr1[0][0]=buff_set_Circle1foundpoint[0];
	pr1[0][1]=buff_set_Circle1foundpoint[1];
	pr1[0][2]=buff_set_Circle1foundpoint[2];

	Mat PR1(1, 3, CV_32F, pr1);
	cout<<"PR1"<<PR1<<endl;

	n_r = PR1-P_RC;
	//cout<<"n_l"<<n_l<<endl;
	n_r = n_r / norm(n_r);
	cout << "n_r" << n_r << endl;

	//计算右边z轴方向向量
	normalr[0][0]=-POINT_R.x;
	normalr[0][1]=-POINT_R.y;
	normalr[0][2]=-POINT_R.z;

	Mat a_r(1, 3, CV_32F, normalr);
	cout<<"a_r"<<a_r<<endl;

    //计算右边y轴方向向量
	o_r.at<float>(0, 0) = a_r.at<float>(0, 1)*n_r.at<float>(0, 2) - a_r.at<float>(0, 2)*n_r.at<float>(0, 1);
	o_r.at<float>(0, 1) = a_r.at<float>(0, 2)*n_r.at<float>(0, 0) - a_r.at<float>(0, 0)*n_r.at<float>(0, 2);
	o_r.at<float>(0, 2) = a_r.at<float>(0, 0)*n_r.at<float>(0, 1) - a_r.at<float>(0, 1)*n_r.at<float>(0, 0);
	cout<<"o_r"<<o_r<<endl;

	hconcat(n_r.t(), o_r.t(), B);
	cout << "B" << B << endl;
	hconcat(B, a_r.t(), R_R);
	cout << "R_R" << R_R << endl;
	
	hconcat(R_R, P_RC.t(), ExtrinsicR);
	cout << "ExtrinsicR" << ExtrinsicR << endl;
	
	vconcat(ExtrinsicR, ADD_2, TR);
	cout << "TR" << TR << endl;

	Mat TL_V;
	invert(TL, TL_V, DECOMP_SVD);
	cout << "TL_V" << TL_V << endl;

	R_cabin = TL_V*TR;
	cout << "R_cabin" << R_cabin<< endl;

	
	waitKey(100000);
	system("pause");
}

输出如下：

2、MATLAB实现

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%左边旋转矩阵建立
% X轴红
P_LC=[-42.965,-679.286,-467.071]
PL1=[-50.418884,-663.10748,-304.63519]
n_l=(PL1-P_LC)
n_l = n_l/norm(n_l)
quiver3(0,0,0,n_l(1),n_l(2),n_l(3),'r','LineWidth',2);
xlabel('x');ylabel('y');zlabel('z');%规范x,y,z坐标轴刻度范围，及在各自坐标轴上标注字母x,y,z

%Z轴黄
a_l = -[0.996236, -0.0689124, 0.052579]
hold on;
quiver3(0,0,0,a_l(1),a_l(2),a_l(3),'y','LineWidth',2);
xlabel('x');ylabel('y');zlabel('z');%规范x,y,z坐标轴刻度范围，及在各自坐标轴上标注字母x,y,z

%acos(dot(a_l,n_l)/(norm(a_l)*norm(n_l)))*180/pi%计算两向量角度
%Y轴绿
o_l = cross(a_l,n_l)/norm(cross(a_l,n_l))
hold on;
quiver3(0,0,0,o_l(1),o_l(2),o_l(3),'g','LineWidth',2);

R_L=[n_l',o_l',a_l']


%右边旋转矩阵建立
% X轴红
P_RC=[-53.9864,-671.041,-453.935]
PR1=[-59.06461,-660.91101,-305.25043]
n_r=(PR1-P_RC)
n_r = n_r/norm(n_r)
quiver3(0,0,1,n_r(1),n_r(2),n_r(3),'r','LineWidth',3);

%Z轴黄
a_r = -[0.997241, -0.0635353, 0.0383888]
hold on;
quiver3(0,0,1,a_r(1),a_r(2),a_r(3),'y','LineWidth',2);


acos(dot(a_r,n_r)/(norm(a_r)*norm(n_r)))*180/pi%计算两向量角度
%Y轴绿
o_r = cross(a_r,n_r)/norm(cross(a_r,n_r))
hold on;
quiver3(0,0,1,o_r(1),o_r(2),o_r(3),'g','LineWidth',2);

R_R=[n_r',o_r',a_r']


ExtrinsicL =[R_L,P_LC']
ExtrinsicR =[R_R,P_RC']

TL=[ExtrinsicL;0,0,0,1]
TR=[ExtrinsicR;0,0,0,1]

X=inv(TL)*TR

输出如下：