嘿，你已经搞定了最核心的立体标定环节，接下来的视差计算和点云上色其实都是基于标定得到的内外参做坐标转换，我给你分两种场景拆解具体步骤：

一、从ToF深度图推导视差图 #

视差本质是同一场景点在两台相机像素平面的坐标差，而ToF已经直接给出了深度值，我们可以用标定参数直接转换，不用像传统双目那样做特征匹配：

1. 提取标定核心参数
   从Matlab stereoCalibration 的输出结果中，你需要拿到：

   • RGB相机的内参 rgbIntrinsics（包含焦距、主点等）
   • ToF相机到RGB相机的旋转矩阵 R 和平移向量 T（注意标定的相机顺序，别搞反参考系）
   • 计算基线长度：B = norm(T)（平移向量的模长就是两台相机的物理间距）
   • RGB相机的焦距：f = rgbIntrinsics.FocalLength(1)（针孔相机x/y焦距通常近似相等）

2. 深度图转视差图的代码实现

   % 加载ToF深度图（注意单位：比如mm转m，要和T的单位一致）
   depthMap = imread('tof_depth.png');
   depthMap = depthMap ./ 1000; % 假设深度图单位是mm，转成m
   
   % 计算视差图，过滤无效深度值
   disparityMap = (f * B) ./ depthMap;
   disparityMap(depthMap <= 0 | isinf(disparityMap)) = NaN; % 剔除无效像素
   
   % 可选：如果ToF和RGB分辨率不同，先对齐图像
   % 用标定的畸变参数校正图像，再缩放匹配分辨率
   undistortedDepth = undistortImage(depthMap, tofIntrinsics);
   alignedDepth = imresize(undistortedDepth, [size(rgbImage,1), size(rgbImage,2)]);
   

   这里要注意：如果两台相机分辨率差异大，一定要先做图像对齐，否则视差图的像素位置会和RGB图像不对应。

二、配准ToF点云与RGB图像，生成带色彩的点云 #

这个场景更实用，直接把RGB的颜色贴到ToF点云上，步骤如下：

方法1：基于标定参数的直接投影配准（首选） #

这是最准确的方式，完全依赖你已经完成的立体标定结果：

% 1. 加载必要数据
tofDepthMap = imread('tof_depth.png');
rgbImage = imread('rgb_frame.png');
% 从stereoCalibration结果中取出参数
tofIntrinsics = stereoParams.CameraParameters2; % 假设ToF是相机2
rgbIntrinsics = stereoParams.CameraParameters1; % RGB是相机1
R = stereoParams.RotationOfCamera2;
T = stereoParams.TranslationOfCamera2;

% 2. 从深度图生成ToF点云
tofPointCloud = depthToPointCloud(tofDepthMap, tofIntrinsics);

% 3. 将ToF点云转换到RGB相机坐标系
tform = rigid3d(R, T);
transformedPC = pctransform(tofPointCloud, tform);

% 4. 将转换后的点云投影到RGB图像，获取对应颜色
[rgbPixels, ~] = projectPoints(transformedPC.Location, rgbIntrinsics, eye(3), zeros(3,1));
% 过滤超出RGB图像边界的点
imageHeight = size(rgbImage, 1);
imageWidth = size(rgbImage, 2);
validIdx = (rgbPixels(:,1) >= 1) & (rgbPixels(:,1) <= imageWidth) ...
          & (rgbPixels(:,2) >= 1) & (rgbPixels(:,2) <= imageHeight);
% 提取对应RGB颜色（注意像素坐标的行/列对应）
validPixels = round(rgbPixels(validIdx, :));
colorIndices = sub2ind([imageHeight, imageWidth], validPixels(:,2), validPixels(:,1));
colors = rgbImage(colorIndices, :);

% 5. 创建并显示带色彩的点云
coloredPC = pointCloud(transformedPC.Location(validIdx,:), 'Color', colors);
pcshow(coloredPC);
axis tight;

方法2：ICP精配准（针对标定有轻微误差的情况） #

如果直接投影后颜色匹配有偏差，可以用ICP算法做精调：

% 先从RGB图像生成稀疏参考点云（比如用特征匹配）
% 或者用ToF点云做初始配准，再用ICP优化
[optimizedTform, ~] = pcregistericp(transformedPC, rgbReferencePC, ...
                                   'MaxIterations', 50, 'Metric', 'pointToPoint');
finalPC = pctransform(transformedPC, optimizedTform);
% 之后再按照方法1的步骤贴颜色即可

常见坑点提示 #

• 分辨率不匹配：一定要先对深度图/RGB图像做畸变校正（undistortImage），再缩放对齐分辨率，否则投影的像素位置会偏移。
• 单位不一致：深度图的单位（mm/m）要和标定得到的T的单位一致，否则计算出的视差或点云位置会完全错误。
• 标定顺序：确认RotationOfCamera2和TranslationOfCamera2对应的是ToF到RGB的变换，别搞反参考系。

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