c#实现SVD奇异值分解求解旋转平移矩阵

VS->工具->NuGet包添加Accord库，代码使用halcon进行矩阵计算，可以自行使用opencv中Mat进行矩阵计算。

using System;	
using System.Linq;
using Accord.Math.Decompositions;  //  添加方法

namespace SVD
{
    class Program
    {
        static void Main(string[] args)
        {
            //  模拟数据
            List<Point> a = new List<Point>()
            {
                new Point(100, 0),
                new Point(0, 100),
                new Point(0, -100),
                new Point(-100, 0),
             };
             List<Point> b = new List<Point>()
             {
                new Point(100, 10),
                new Point(0, 110),
                new Point(0, -90),
                new Point(-100, 10),
             };

            // 转数组进行运算
            double[] ax = new double[a.Count];
            double[] ay = new double[a.Count];
            double[] bx = new double[a.Count];
            double[] by = new double[a.Count];
            for (int i = 0; i < a.Count; ++i)
            {
                ax[i] = a[i].X;
                ay[i] = a[i].Y;
                bx[i] = b[i].X;
                by[i] = b[i].Y;
            }
            
            // 中心化
            Point centerA = new Point(ax.Average(), ay.Average());
            Point centerB = new Point(bx.Average(), by.Average());
            HOperatorSet.TupleGenConst(4, 0, out HTuple tmp);
            HOperatorSet.TupleSub(ax, centerA.X, out HTuple axx);
            HOperatorSet.TupleSub(ay, centerA.Y, out HTuple ayy);
            HOperatorSet.TupleSub(bx,centerB.X, out HTuple bxx);
            HOperatorSet.TupleSub(by, centerB.Y, out HTuple byy);
            HOperatorSet.VectorToHomMat2d(bxx, byy, axx, ayy, out HTuple mat2D);

            // Create a matrix double[,]  变换矩阵转齐次变换矩阵
            double[,] matrix =
            {
                { mat2D[0], mat2D[1], mat2D[2] }, 
                { mat2D[3], mat2D[4], mat2D[5] }, 
                { 0, 0, 1 }
            };

            // Perform SVD
            SingularValueDecomposition svd = new SingularValueDecomposition(matrix);
            // 获取 u, s, V matrices
            double[,] U = svd.LeftSingularVectors;
            double[,] S = svd.DiagonalMatrix;
            double[,] V = svd.RightSingularVectors;

            int elementCount = U.GetLength(0) * U.GetLength(1);
            double[] matU = new double[elementCount];
            double[] matV = new double[elementCount];
            double[] matS = new double[elementCount];
            for (int i = 0; i < U.GetLength(0); i++)
            {
                for (int j = 0; j < U.GetLength(1); j++)
                {
                    matU[i* U.GetLength(0) + j] = U[i, j];
                    matV[i* V.GetLength(0) + j] = V[i, j];
                    matS[i* S.GetLength(0) + j] = S[i, j];
                }
            }

            // 生成USV矩阵
            HOperatorSet.CreateMatrix(3, 3, 0, out HTuple matrixU);
            HOperatorSet.CreateMatrix(3, 3, 0, out HTuple matrixS);
            HOperatorSet.CreateMatrix(3, 3, 0, out HTuple matrixV);
            HOperatorSet.SetFullMatrix(matrixU, matU);
            HOperatorSet.SetFullMatrix(matrixS, matS);
            HOperatorSet.SetFullMatrix(matrixV, matV);

            //验证USV是否正确  H = U * S * V.T()
            HOperatorSet.MultMatrix(matrixU, matrixS, "AB", out HTuple matrixUS);
            HOperatorSet.MultMatrix(matrixUS, matrixV, "ABT", out HTuple matrixResult);
            HOperatorSet.GetFullMatrix(matrixResult, out HTuple values);

            // 求旋转矩阵   R = V * U.T()
            HOperatorSet.MultMatrix(matrixV, matrixU, "ABT", out HTuple matrixR);
            HOperatorSet.GetFullMatrix(matrixR, out HTuple valuesR);

            // 求平移矩阵 T = B - R * A
            double[] A = { centerA.X, centerA.Y, 1 };
            double[] B = { centerB.X, centerB.Y, 1 };
            HOperatorSet.CreateMatrix(3, 1, A, out HTuple matrixA);
            HOperatorSet.CreateMatrix(3, 1, B, out HTuple matrixB);
            HOperatorSet.MultMatrix(matrixR, matrixA, "AB", out HTuple matrixAB);
            HOperatorSet.SubMatrix(matrixB, matrixAB, out HTuple matrixSub);
            HOperatorSet.GetFullMatrix(matrixSub, out HTuple valueT);

            Console.ReadLine();
        }
    }
}

通过SingularValueDecomposition类进行SVD计算。然后，我们分别获取左奇异向量矩阵U，对角矩阵S和右奇异向量矩阵V，通过奇异矩阵计算旋转矩阵，再通过旋转矩阵和重心计算平移矩阵。