求解两组3D点云间变换参数：缩放、旋转与平移计算问题

核心变换与线性化 #

基础变换公式为：
P₂ = s·R·P₁ + T
其中：

• P₁=(x₁,y₁,z₁)：原坐标系下的点
• P₂=(x₂,y₂,z₂)：目标坐标系下的点
• s：全局缩放因子
• R：3×3旋转矩阵（由ω、φ、κ三个旋转角分别对应X、Y、Z轴旋转定义）
• T=(Tₓ,Tᵧ,T_z)：平移向量

旋转矩阵的线性化（小角度近似） #

当旋转角较小时，旋转矩阵可近似为：

R ≈ [1   -κ    φ
     κ    1   -ω
    -φ    ω    1]

代入变换公式展开各坐标分量，得到每个点的误差方程（v为残差）：

vₓᵢ = s·x₁ᵢ - s·κ·y₁ᵢ + s·φ·z₁ᵢ + Tₓ - x₂ᵢ
vᵧᵢ = s·κ·x₁ᵢ + s·y₁ᵢ - s·ω·z₁ᵢ + Tᵧ - y₂ᵢ
v_zᵢ = -s·φ·x₁ᵢ + s·ω·y₁ᵢ + s·z₁ᵢ + T_z - z₂ᵢ

将待求参数设为修正量 Δs, Δω, Δφ, Δκ, ΔTₓ, ΔTᵧ, ΔT_z，初始值设为 s₀=1, ω₀=φ₀=κ₀=0, Tₓ₀=Tᵧ₀=T_z₀=0，将误差方程整理为矩阵形式 V = B·X - L，其中X是参数修正量向量。

最小二乘迭代步骤（保证收敛的关键） #

• 重心化预处理
  对两组点分别计算重心：
  P₁̄ = (1/n)ΣP₁ᵢ, P₂̄ = (1/n)ΣP₂ᵢ
  将所有点转换为重心化坐标：
  p₁ᵢ = P₁ᵢ - P₁̄, p₂ᵢ = P₂ᵢ - P₂̄
  这一步能彻底解耦平移与缩放/旋转参数，是避免迭代发散的核心操作。

• 初始参数估计

  • 初始缩放因子：
    s₀ = (Σ(p₂ₓᵢp₁ₓᵢ + p₂ᵧᵢp₁ᵧᵢ + p₂zᵢp₁zᵢ)) / Σ(p₁ₓᵢ² + p₁ᵧᵢ² + p₁zᵢ²)
  • 初始旋转矩阵：
    构造协方差矩阵 M = Σ(p₁ᵢ·p₂ᵢᵀ)，对M做SVD分解得 M=UΣVᵀ，则 R₀=VUᵀ；若det(R₀)=-1，将V的第三列取反后再计算R₀。
  • 初始平移向量：
    T₀ = P₂̄ - s₀·R₀·P₁̄

• 迭代修正

  • 用当前参数计算预测点：P₂'ᵢ = s·R·P₁ᵢ + T
  • 计算残差向量：Lᵢ = P₂ᵢ - P₂'ᵢ
  • 构建设计矩阵B，基于线性化后的误差方程
  • 求解法方程：(BᵀB)X = BᵀL，得到参数修正量X
  • 更新参数：
    • s += Δs
    • 用修正后的旋转角构建增量旋转矩阵ΔR，更新R = ΔR·R（或用四元数复合更新）
    • T += ΔT
  • 重复迭代，直到残差平方和的变化量小于阈值（如1e-8）或迭代次数达到上限（如20次）

常见收敛失败原因及解决 #

• 未做重心化：原始坐标下平移与缩放/旋转参数强耦合，迭代极易发散，必须先做重心化。
• 初始值不合理：若旋转角较大，小角度近似不成立，必须用SVD获取可靠初始旋转矩阵，不能直接用0初始值。
• 旋转参数更新错误：欧拉角直接累加会导致奇异性（万向锁），建议改用四元数表示旋转，或用旋转矩阵复合方式更新。
• 误差方程方向错误：确保残差是观测值 - 预测值，反向会导致法方程求解错误。

四元数优化方案（避免欧拉角奇异性） #

用四元数q=(q₀,q₁,q₂,q₃)表示旋转（满足q₀²+q₁²+q₂²+q₃²=1），旋转矩阵由四元数推导：

R = [1-2q₂²-2q₃²  2q₁q₂-2q₀q₃    2q₁q₃+2q₀q₂
     2q₁q₂+2q₀q₃  1-2q₁²-2q₃²    2q₂q₃-2q₀q₁
     2q₁q₃-2q₀q₂  2q₂q₃+2q₀q₁    1-2q₁²-2q₂²]

将四元数作为待求参数线性化，迭代收敛性更稳定，无万向锁问题。

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