3D点云投影至图像平面的技术实现问题问询

看起来你已经搭好了点云投影的代码框架，不过目前还没完成从相机坐标系到图像平面的关键步骤，我来帮你补全并优化整个流程：

先补全你当前的循环代码 #

你的mtimes调用还没写完，在Matlab里矩阵乘法可以直接用*运算符，还要注意维度匹配——你构造的b是列向量，相乘后需要转成行向量才能赋值给xyz_c的行：

Td=[0.994700168903275 0.0521802707917803 0.0885934158044077 -1.09405637454607/1000;
-0.0536097887263755 0.998466155670681 0.0138320834629078 24.2728498290288/1000;
-0.0877357654352521 -0.0185082500606608 0.995971826982662 9.35239973330908/1000;
0 0 0 1];
xd=xyz(:,1);
yd=xyz(:,2);
zd=xyz(:,3);
xyz_c=zeros(76800,4);
for i=1:76800
    b=[xd(i); yd(i); zd(i); 1];
    % 补全矩阵乘法，转成行向量赋值
    xyz_c(i,:) = (Td * b)';
end

更高效的向量化实现（替代循环） #

Matlab里循环处理大量点效率很低，建议直接用矩阵运算批量处理所有点，速度会快很多：

% 构造点云的齐次坐标矩阵（每一列对应一个点的[x,y,z,1]）
xyz_hom = [xyz, ones(size(xyz,1), 1)]';
% 批量计算相机坐标系下的齐次点
xyz_c_hom = Td * xyz_hom;
% 转成N×3的相机坐标系点（去掉齐次项）
xyz_c = xyz_c_hom(1:3,:)';

关键步骤：相机内参与透视投影 #

你当前只完成了世界坐标系→相机坐标系的变换，要得到图像平面的像素坐标，还需要用相机内参矩阵K做透视投影：

% 替换成你实际相机标定得到的内参参数
fx = 1000; % 焦距x方向
fy = 1000; % 焦距y方向
cx = 512;  % 图像主点x坐标
cy = 384;  % 图像主点y坐标

% 构造内参矩阵K
K = [fx  0  cx;
     0  fy cy;
     0   0  1];

% 计算图像平面的齐次坐标
uv_hom = K * xyz_c_hom(1:3,:);
% 透视除法：将齐次坐标转换为像素坐标
u = uv_hom(1,:) ./ uv_hom(3,:);
v = uv_hom(2,:) ./ uv_hom(3,:);

% 可选：转换为整数像素坐标（用于图像索引）
u_pixel = round(u);
v_pixel = round(v);

重要注意事项 #

• 单位一致性：你的Td平移项都除以了1000，要确保点云xyz的单位（比如毫米）和Td的单位（米）一致，否则投影结果会出现严重偏差。
• 过滤无效点：如果相机坐标系下的Z值（xyz_c(:,3)）为负，说明点在相机后方，这类点无法投影到图像上，需要过滤掉：

  valid_idx = xyz_c(:,3) > 0;
  u_valid = u(valid_idx);
  v_valid = v(valid_idx);
  

• 内参准确性：内参矩阵K必须通过相机标定获得，不能随便用示例值，否则投影坐标会完全错误。

内容的提问来源于stack exchange，提问作者Kuchx