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,:);%leftMatrix和rightMatrix_H表示相机内参矩阵和外参矩阵， %UV是像素坐标系下的二维点，CB_World是相机坐标系下的三维点。 N_rightMatrix = repmat(rightMatrix,1,size(UV,1)); CB_World = temp_s.*leftMatrix - N_rightMatrix; WorldPoint = cameraParams.WorldPoints';已知质心像素坐标求实际坐标

假设你已知质心在图像上的像素坐标为 (u,v)，可以使用相机的标定参数将其转换为相机坐标系下的三维点坐标。

首先，将像素坐标转换为齐次坐标形式：

UV_H = [u, v, 1];

然后，根据相机的内参矩阵和外参矩阵，可以将像素坐标系下的点转换为相机坐标系下的点：

M = cameraParams.IntrinsicMatrix';
R = cameraParams.RotationMatrices(:,:,1);
T = cameraParams.TranslationVectors(1,:)';
leftMatrix = inv(R) * inv(M) * UV_H';
rightMatrix = inv(R) * T;
rightMatrix_H = repmat(rightMatrix(3), [1, size(UV_H, 2)]);
temp_s = rightMatrix_H ./ leftMatrix(3,:);
N_rightMatrix = repmat(rightMatrix, 1, size(UV_H, 2));
CB_Camera = temp_s .* leftMatrix - N_rightMatrix;

最后，再将相机坐标系下的点转换为世界坐标系下的点，即：

WorldPoint = cameraParams.WorldPoints';
CB_World = R' * (CB_Camera - T);

其中，`cameraParams.WorldPoints` 是标定板上角点的世界坐标系下的坐标，`CB_World` 是相机坐标系下的三维点，`R` 和 `T` 是相机的旋转矩阵和平移向量。