MATLAB 相机标定中标定板角点像素坐标系到世界坐标系的转换

matlab 做相机标定后，想将第一张（任意一张都行）标定板角点所对应的像素坐标转换到世界坐标系下，标定板角点的像素坐标真值与世界坐标真值都非常容易获得，但是我通过内外参矩阵将像素坐标转换到世界坐标有很大的误差，如下

close all;
clear all;
clc;
load ('6mm_matlab.mat')
% 相机标定基本参数
M = cameraParams.IntrinsicMatrix';
R = cameraParams.RotationMatrices(:,:,1);
T = cameraParams.TranslationVectors(1,:)';
UV = cameraParams.ReprojectedPoints(:,:,1);
% 将标定板角点像素坐标转换成齐次
for i = 1:size(UV,1)
    UV_H(i,:) = [UV(i,:),1];
end
% 将像素坐标系转换到像素坐标系,公式参考: https://blog.csdn.net/qq_43222384/article/details/101516807
leftMatrix = inv(R)*inv(M)*UV_H';
rightMatrix = inv(R)*T;
rightMatrix_H = repmat(rightMatrix(3),[1,size(UV,1)]);
temp_s = rightMatrix_H./leftMatrix(3,:);
N_rightMatrix = repmat(rightMatrix,1,size(UV,1));
CB_World = temp_s.*leftMatrix - N_rightMatrix;

像素坐标到世界坐标之间的转换可参考如下公式

已知像素坐标 $\left[ \begin{array}{c} u\\ v\\ 1\\ \end{array} \right]$ ，世界坐标 $\left[ \begin{array}{c} X_w\\ Y_w\\ 0\\ \end{array} \right]$ ，内参矩阵 M，旋转矩阵 R，平移向量 T。

则有： $s\left[ \begin{array}{c} u\\ v\\ 1\\ \end{array} \right] =MR\left[ \begin{array}{c} X_w\\ Y_w\\ 0\\ \end{array} \right] +T$

$sR^{-1}M^{-1}\left[ \begin{array}{c} u\\ v\\ 1\\ \end{array} \right] =\left[ \begin{array}{c} X_w\\ Y_w\\ 0\\ \end{array} \right] +R^{-1}T$

记 $\left[ \begin{array}{c} a\\ b\\ c\\ \end{array} \right] =R^{-1}M^{-1}\left[ \begin{array}{c} u\\ v\\ 1\\ \end{array} \right]$ ， $\left[ \begin{array}{c} t_1\\ t_2\\ t_3\\ \end{array} \right] =R^{-1}T$

解得 $s=\frac{t_3}{c}$

所以 $\left[ \begin{array}{c} X_w\\ Y_w\\ 0\\ \end{array} \right] =\frac{t_3}{c}\left[ \begin{array}{c} a\\ b\\ c\\ \end{array} \right] -\left[ \begin{array}{c} t_1\\ t_2\\ t_3\\ \end{array} \right]$

Matlab 标定后的相机参数如下：

内参矩阵： $M=\left[ \begin{matrix} 1768& 0& 749.7\\ 0& 1768& 586\\ 0& 0& 1\\ \end{matrix} \right]$ ，旋转矩阵： $R=\left[ \begin{matrix} -0.9900& 0.1411& 0.0046\\ -0.1403& -0.9868& 0.0803\\ 0.0159& 0.0788& 0.9968\\ \end{matrix} \right]$ ,

平移向量： $T\ =\ \left[ \begin{array}{c} 92.755\\ 52.092\\ 422.884\\ \end{array} \right]$

世界坐标系下的点到像素坐标系下的转化关系可表示为：

$s\left( \begin{array}{c} u_1\\ v_1\\ 1\\ \end{array} \right) =\left[ \begin{matrix} 1768& 0& 749.7\\ 0& 1768& 586\\ 0& 0& 1\\ \end{matrix}\begin{array}{c} 0\\ 0\\ 0\\ \end{array} \right] \left[ \begin{matrix} -0.9900& 0.1411& 0.0046& 92.755\\ -0.1403& -0.9868& 0.0803& 52.092\\ 0.0159& 0.0788& 0.9968& 422.884\\ 0& 0& 0& 1\\ \end{matrix} \right] \left( \begin{array}{c} X_w\\ Y_w\\ 0\\ 1\\ \end{array} \right)$