OpenCVSharp 三维点计算刚体变换的旋转平移矩阵(RT矩阵)

---

前言

在3D相机和机器人标定的时候，需要将相机的坐标系转换为机器人的坐标系，Halcon 通过vector_to_hom_mat3d可以很容易计算出对应的矩阵，Python 可以调用numpy也可以计算到对应的矩阵，但是OpenCV好像没有对应的计算方式，需要编写来计算，以下为参考的文章：
https://blog.csdn.net/sunshine_zoe/article/details/78852482
https://blog.csdn.net/qq_35565669/article/details/102748505
以上的是基于C++和python 实现，以下为采用OpenCVSharp在C# .net上的实现。

---

一、旋转平移矩阵是什么

任意的两个三维笛卡尔坐标系之间的旋转都可以用一个3x3的矩阵来表示，通过以下的公式可以将相机的坐标系转换为机器人的坐标系
P rot = R∗ P cam+ t
R为3x3的转换矩阵，t为3x1的位移变换向量，这里点坐标均为3x1的列向量。
只要给定至少3个点，就能计算出R和t，一般来说点越多，计算出来的转换就越精确。

二、计算原理：

参考《Estimating 3-D Rigid Body Transformations: A Comparison of Four Major Algorithms》，可以使用SVD方法计算R和t。

详细的计算步骤为：

1.计算源和目标点的平均中心

2.计算质心

3.SVD求解

4.计算R矩阵

5.计算t矩阵

三、示例代码：

        private void rigid_transform_3D_CV(Point3f[] srcPoints, Point3f[] dstPoints) 
        {
            float srcSumX = 0.0f;
            float srcSumY = 0.0f;
            float srcSumZ = 0.0f;

            float dstSumX = 0.0f;
            float dstSumY = 0.0f;
            float dstSumZ = 0.0f;
            int pointsNum = srcPoints.Length;
            Point3f centerSrc = new Point3f(), centerDst = new Point3f();

            for (int i = 0; i < pointsNum; ++i)
            {
                srcSumX += srcPoints[i].X;
                srcSumY += srcPoints[i].Y;
                srcSumZ += srcPoints[i].Z;
                
                dstSumX += dstPoints[i].X;
                dstSumY += dstPoints[i].Y;
                dstSumZ += dstPoints[i].Z;
            }
            centerSrc.X =( float)(srcSumX / pointsNum);
            centerSrc.Y = (float)(srcSumY / pointsNum);
            centerSrc.Z = (float)(srcSumZ / pointsNum);
            centerDst.X = (float)(dstSumX / pointsNum);
            centerDst.Y = (float)(dstSumY / pointsNum);
            centerDst.Z = (float)(dstSumZ / pointsNum);
            float[] srcDat =new float[pointsNum*3];
            float[] dstDat = new float[pointsNum * 3];
            for (int i = 0; i < pointsNum; ++i)
            {
                srcDat[i*3] = srcPoints[i].X - centerSrc.X;
                srcDat[i*3+1] = srcPoints[i].Y - centerSrc.Y;
                srcDat[i * 3 + 2] = srcPoints[i].Z - centerSrc.Z;

                dstDat[i*3] = dstPoints[i].X - centerDst.X;
                dstDat[i*3 + 1] = dstPoints[i].Y - centerDst.Y;
                dstDat[i * 3 + 2] = dstPoints[i].Z - centerDst.Z;
            }
            Mat srcMat = new Mat(pointsNum,3, MatType.CV_32FC1, srcDat);
            Mat dstMat = new Mat(pointsNum,3, MatType.CV_32FC1,dstDat);
            Mat matH = srcMat.T().Multiply( dstMat);
            Mat w = new Mat(), u = new Mat(), vt = new Mat();
            Cv2.SVDecomp(matH, w, u, vt);
            Mat matTemp = u * vt;
            double det = Cv2.Determinant(matTemp);
            float[] datM = { 1, 0, 0, 0, 1, 0, 0, 0, (float)det };
            Mat matM = new Mat(3, 3, MatType.CV_32FC1, datM);
            Mat R = vt.T() * matM * u.T();
            Mat centroid_A = new Mat(3, 1, MatType.CV_32FC1, new float[] { centerSrc.X, centerSrc.Y, centerSrc.Z });
            Mat centroid_B = new Mat(3, 1, MatType.CV_32FC1, new float[] { centerDst.X, centerDst.Y, centerDst.Z });
            Mat t = -R * centroid_A + centroid_B;


        }

四、结果比较：

和halcon结果对比：

和Python结果对比：

---

总结

计算该变换矩阵的时候，参考了很多的方式，和halcon的计算对比都不对，参考了python的munpy库，看到有numshape这个.net版本的，哪知道这个是一个巨坑，没用SVD求解的功能不说，连tile这个功能都没有，还说什么.net版本的深度学习库。。。。
就是一个大坑，毫无用处，最后还是用opencv来实现，opencvsharp还是比较厉害的。