PCL 姿态估计 RANSAC + SVD（基于特征匹配）

1、问题定义

假设有两组点云：

2、数学原理

Step 1：RANSAC

随机选 3 个对应点对 (pi,qi)(p_i,q_i)(pi,qi)
用 SVD 求解当前候选刚体变换 (R,t)(R,t)(R,t)
计算 所有匹配点误差 ：

4.判断 inlier：

5.保存 inlier 数最多的 RANSAC 结果

Step 2：SVD 刚体求解（Kabsch 算法）

3、PCL 对应关系

MATLAB 逻辑	PCL 对应类
随机选 3 个匹配	`SampleConsensusModelRegistration`
误差判断 inlier	`SACSegmentation`
SVD 求 R,t	`TransformationEstimationSVD`
全 inlier 精修	`estimateRigidTransformationSVD`

4、MATLAB 完整实现

cpp 复制代码

clc; clear; close all;

%% 模拟点云
N = 100;
P = rand(N,3);  % 原始点云
R_true = axang2rotm([0 0 1 pi/6]); % 旋转
t_true = [0.5 0.2 0.3];
Q = (R_true*P' + t_true')'; % 变换后的点云
Q = Q + 0.01*randn(size(Q)); % 添加噪声

%% 假设已经有特征匹配，这里全匹配
matches = [(1:N)' (1:N)'];

%% RANSAC 参数
maxIter = 200;
threshold = 0.02;
bestInliers = [];
bestR = eye(3);
bestT = zeros(1,3);

for iter = 1:maxIter
    % 随机 3 个匹配点
    idx = randperm(N,3);
    Pi = P(matches(idx,1),:);
    Qi = Q(matches(idx,2),:);

    % SVD 求解旋转和平移
    cp = mean(Pi,1); cq = mean(Qi,1);
    X = Pi - cp; Y = Qi - cq;
    H = X' * Y;
    [U,~,V] = svd(H);
    R = V*U';
    if det(R)<0
        V(:,3) = -V(:,3);
        R = V*U';
    end
    t = cq - (R*cp')';

    % 计算误差
    Qp = (R*P(matches(:,1),:)' + t')';
    errs = sqrt(sum((Qp - Q(matches(:,2),:)).^2,2));
    inliers = find(errs < threshold);

    if length(inliers) > length(bestInliers)
        bestInliers = inliers;
        bestR = R;
        bestT = t;
    end
end

fprintf('RANSAC inliers = %d\n', length(bestInliers));

%% 用所有 inliers 再精确求解一次 SVD
Pi = P(matches(bestInliers,1),:);
Qi = Q(matches(bestInliers,2),:);
cp = mean(Pi,1); cq = mean(Qi,1);
X = Pi - cp; Y = Qi - cq;
H = X' * Y;
[U,~,V] = svd(H);
R_final = V*U';
if det(R_final)<0
    V(:,3) = -V(:,3);
    R_final = V*U';
end
t_final = cq - (R_final*cp')';

disp('Estimated R:'); disp(R_final);
disp('Estimated t:'); disp(t_final);

%% 可视化
figure; hold on; axis equal;
scatter3(P(:,1),P(:,2),P(:,3),36,'b','filled');
scatter3(Q(:,1),Q(:,2),Q(:,3),36,'r','filled');

% RANSAC 对齐后
Q_aligned = (R_final*P' + t_final')';
scatter3(Q_aligned(:,1),Q_aligned(:,2),Q_aligned(:,3),36,'g');

legend('Source P','Target Q','Aligned P');
title('RANSAC + SVD Point Cloud Registration');
xlabel X; ylabel Y; zlabel Z;
grid on;

5、流程

cpp 复制代码

        ┌──────────────┐
        │ 输入特征匹配 │
        └──────┬──────┘
               │
   ┌───────────▼────────────┐
   │       RANSAC 主循环     │
   └───────────┬────────────┘
               │
 ┌─────────────▼──────────────┐
 │  随机选 3 对匹配样本 (Pi,Qi) │
 └─────────────┬──────────────┘
               │
 ┌─────────────▼──────────────┐
 │    SVD 求解当前模型 (R,t)   │
 └─────────────┬──────────────┘
               │
 ┌─────────────▼──────────────┐
 │    计算所有匹配误差 & Inlier │
 └─────────────┬──────────────┘
               │
     ┌────────▼───────────┐
     │ 更新最佳模型 (max inlier) │
     └────────┬───────────┘
              │
    ┌─────────▼─────────┐
    │  使用所有 inlier 重新 SVD │
    └─────────┬─────────┘
              │
      ┌───────▼────────┐
      │ 输出最终 R, t   │
      └─────────────────┘