MATLAB 单目相机内外参数已知，进行坐标系建立代码参考

以下是一个 MATLAB 示例代码，用于根据单目相机的内外参数建立坐标系：

% 单目相机内外参数
K = [fx 0 cx; 0 fy cy; 0 0 1]; % 相机内参数矩阵
R = [r11 r12 r13; r21 r22 r23; r31 r32 r33]; % 相机旋转矩阵
t = [tx; ty; tz]; % 相机平移向量

% 世界坐标系中的三个点
P1 = [x1; y1; z1];
P2 = [x2; y2; z2];
P3 = [x3; y3; z3];

% 将三个点投影到图像平面上
p1 = K * [R t] * [P1; 1];
p2 = K * [R t] * [P2; 1];
p3 = K * [R t] * [P3; 1];

% 将图像平面中的点转换为归一化的相机坐标系下的点
pc1 = inv(K) * p1;
pc2 = inv(K) * p2;
pc3 = inv(K) * p3;

% 通过三个相机坐标系下的点计算出相机坐标系到世界坐标系的变换矩阵
v1 = P2 - P1;
v2 = P3 - P1;
v3 = cross(v1, v2);
Rc = [v1/norm(v1), v2/norm(v2), v3/norm(v3)];
tc = P1;
Tcw = [Rc tc; 0 0 0 1];

% 将相机坐标系下的点转换为世界坐标系下的点
pw1 = Tcw * [pc1; 1];
pw2 = Tcw * [pc2; 1];
pw3 = Tcw * [pc3; 1];

% 建立坐标系
origin = [0 0 0];
x_axis = [1 0 0];
y_axis = [0 1 0];
z_axis = [0 0 1];

% 绘制三个坐标轴和坐标系原点
plot3([origin(1) x_axis(1)], [origin(2) x_axis(2)], [origin(3) x_axis(3)], 'r');
hold on;
plot3([origin(1) y_axis(1)], [origin(2) y_axis(2)], [origin(3) y_axis(3)], 'g');
plot3([origin(1) z_axis(1)], [origin(2) z_axis(2)], [origin(3) z_axis(3)], 'b');
scatter3(origin(1), origin(2), origin(3), 'filled');

% 绘制三个点和它们在世界坐标系下的位置
scatter3(P1(1), P1(2), P1(3), 'filled');
scatter3(P2(1), P2(2), P2(3), 'filled');
scatter3(P3(1), P3(2), P3(3), 'filled');
scatter3(pw1(1), pw1(2), pw1(3), 'filled');
scatter3(pw2(1), pw2(2), pw2(3), 'filled');
scatter3(pw3(1), pw3(2), pw3(3), 'filled');

% 设置坐标轴标签和标题
xlabel('x');
ylabel('y');
zlabel('z');
title('Camera Coordinate System');

在这个示例代码中，我们首先定义了单目相机的内外参数，然后定义了世界坐标系中的三个点，并将它们投影到图像平面上。接下来，我们将图像平面中的点转换为归一化的相机坐标系下的点，并通过三个相机坐标系下的点计算出相机坐标系到世界坐标系的变换矩阵。最后，我们将相机坐标系下的点转换为世界坐标系下的点，并绘制出坐标系和三个点在世界坐标系下的位置。

注意，这个示例代码仅适用于世界坐标系中的三个点不共线的情况。如果三个点共线，或者不满足其他条件，可能需要进行适当的修改。