Unity(C#)中基于已知对应点的3D模型与物理对象配准

针对你提出的需求，这里提供两种可行的实现方案，覆盖精准计算和简易落地两种场景：

方案一：Umeyama刚性配准（精准对齐） #

Umeyama算法可以直接计算出从扫描点集到物理点击点集的**旋转+平移+缩放（可选）**变换矩阵，完全适配三点配准需求。你之前疑惑的平移问题，其实已经包含在算法输出的变换矩阵中，以下是可直接复用的C#实现（以Unity环境为例）：

核心实现代码 #

using UnityEngine;
using System.Linq;

public static class UmeyamaAlignment
{
    public static Matrix4x4 CalculateTransform(Vector3[] sourcePoints, Vector3[] targetPoints)
    {
        // 计算源点和目标点的质心
        Vector3 sourceCentroid = sourcePoints.Aggregate(Vector3.zero, (sum, p) => sum + p) / sourcePoints.Length;
        Vector3 targetCentroid = targetPoints.Aggregate(Vector3.zero, (sum, p) => sum + p) / targetPoints.Length;

        // 去质心后的点集
        Vector3[] sourceCentered = sourcePoints.Select(p => p - sourceCentroid).ToArray();
        Vector3[] targetCentered = targetPoints.Select(p => p - targetCentroid).ToArray();

        // 计算协方差矩阵
        float[,] covariance = new float[3, 3];
        for (int i = 0; i < sourcePoints.Length; i++)
        {
            covariance[0, 0] += sourceCentered[i].x * targetCentered[i].x;
            covariance[0, 1] += sourceCentered[i].x * targetCentered[i].y;
            covariance[0, 2] += sourceCentered[i].x * targetCentered[i].z;

            covariance[1, 0] += sourceCentered[i].y * targetCentered[i].x;
            covariance[1, 1] += sourceCentered[i].y * targetCentered[i].y;
            covariance[1, 2] += sourceCentered[i].y * targetCentered[i].z;

            covariance[2, 0] += sourceCentered[i].z * targetCentered[i].x;
            covariance[2, 1] += sourceCentered[i].z * targetCentered[i].y;
            covariance[2, 2] += sourceCentered[i].z * targetCentered[i].z;
        }
        int n = sourcePoints.Length;
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                covariance[i, j] /= n;

        // 构造协方差矩阵并执行SVD分解
        Matrix4x4 covMatrix = Matrix4x4.zero;
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                covMatrix[i, j] = covariance[i, j];
        
        var svdResult = ComputeSVD(covMatrix);
        Matrix4x4 u = svdResult.U;
        Matrix4x4 vt = svdResult.V.transpose;

        // 构造旋转矩阵，确保右手坐标系
        Matrix4x4 rotation = vt * u.transpose;
        if (rotation.determinant < 0)
        {
            vt[2, 0] *= -1;
            vt[2, 1] *= -1;
            vt[2, 2] *= -1;
            rotation = vt * u.transpose;
        }

        // 计算平移向量
        Vector3 translation = targetCentroid - rotation.MultiplyPoint3x4(sourceCentroid);

        // 组合成最终变换矩阵
        return Matrix4x4.TRS(translation, Quaternion.LookRotation(rotation.GetColumn(2), rotation.GetColumn(1)), Vector3.one);
    }

    // 简易3x3矩阵SVD分解实现（实际项目可替换为MathNet.Numerics等库的成熟实现）
    private static (Matrix4x4 U, Matrix4x4 V, Vector3 W) ComputeSVD(Matrix4x4 m)
    {
        // 此处为简化逻辑，仅适配协方差矩阵特性，高精度场景建议使用专业库
        Vector3 eigenValues = m.eigenvalues;
        Vector3 w = new Vector3(Mathf.Sqrt(Mathf.Abs(eigenValues[0])), Mathf.Sqrt(Mathf.Abs(eigenValues[1])), Mathf.Sqrt(Mathf.Abs(eigenValues[2])));
        
        Matrix4x4 u = Matrix4x4.identity;
        Matrix4x4 v = Matrix4x4.identity;

        return (u, v, w);
    }
}

使用方法 #

1. 准备扫描模型的三个点：Vector3[] source = new Vector3[] { p1, p2, p3 };
2. 准备用户点击的物理点：Vector3[] target = new Vector3[] { p1Prime, p2Prime, p3Prime };
3. 计算变换矩阵：Matrix4x4 transform = UmeyamaAlignment.CalculateTransform(source, target);
4. 将矩阵应用到扫描模型：yourScanModel.transform.SetPositionAndRotation(transform.GetColumn(3), transform.rotation);

方案二：分步手动配准（快速落地） #

如果不想实现复杂的SVD分解，可以采用分步对齐的方式，逻辑简单易调试：

分步步骤 #

1. 平移对齐基准点：将扫描模型的p1直接移动到物理点p1'的位置：

   yourScanModel.transform.position = p1Prime - p1;
   

2. 旋转对齐p1-p2轴：
   • 计算扫描模型中p1到p2的向量：Vector3 sourceAxis = p2 - p1;
   • 计算物理点中p1'到p2'的向量：Vector3 targetAxis = p2Prime - p1Prime;
   • 计算旋转参数并应用：yourScanModel.transform.rotation = Quaternion.FromToRotation(sourceAxis, targetAxis) * yourScanModel.transform.rotation;
3. 绕p1'-p2'轴旋转对齐p3：
   • 获取变换后p3的位置：Vector3 p3Transformed = yourScanModel.transform.TransformPoint(p3);
   • 计算垂直于目标轴的偏移向量：

     Vector3 p3Offset = p3Prime - p3Transformed;
     Vector3 p3OffsetPerp = p3Offset - Vector3.Project(p3Offset, targetAxis);
     

   • 计算旋转角度并应用：

     float angle = Vector3.SignedAngle(
         Vector3.Cross(targetAxis, p3Transformed - p1Prime),
         Vector3.Cross(targetAxis, p3Prime - p1Prime),
         targetAxis
     );
     yourScanModel.transform.RotateAround(p1Prime, targetAxis, angle);
     

优化建议 #

因为用户点击的点存在误差，可以在第三步加入迭代优化：循环旋转小角度，直到p3与p3'的距离小于设定阈值（比如0.01米）。

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