少有人走的路

数学模型

已知两个坐标系在各方向上尺度缩放比例一致，两个坐标系的转换关系可以用7个参数来表示，3个旋转参数，3个平移参数，1个比例参数。已知三点在A、B两个坐标系中的坐标，那么这7个参数可以唯一确定。

坐标转换的数学模型为：

其中，λ是比例参数，R是旋转矩阵，Δ是平移向量，A、B分别是两个坐标系中的坐标。

比例参数λ最容易计算

旋转矩阵R是一个3x3的正交矩阵，有3个自由度。可利用反对称矩阵S来构造旋转矩阵R：

那么

其中I是单位矩阵，这里R只有a、b、c三个变量，解出a、b、c即可确定旋转矩阵R。

这样(3)式可写为

整理可得

把a、b、c带入(2)式可到旋转矩阵R，把任意一点坐标带入(1)式可得Δ。

swift算法实现

在ARKit中，SCNNode.transform是一个4x4的变换矩阵，

下面代码中的m_Translation为λT，m_Rotation为λR

声明变量

    var point1onA: float3 = float3(0,0,0)
    var point2onA: float3 = float3(0,0,0)
    var point3onA: float3 = float3(0,0,0)
    
    var point1onB: float3 = float3(0,0,0)
    var point2onB: float3 = float3(0,0,0)
    var point3onB: float3 = float3(0,0,0)
    
    var m_ratio: Float = 0.0 // 比例参数λ
    var m_Translation: float3 = float3(0,0,0) // λx平移向量Δ，3个参数
    var m_Rotation: matrix_float3x3！ // λx旋转矩阵R ，3个参数abc，计算时解算的是abc

计算比例参数λ，为了减少误差取平均值

func caliratio(){
        var ratiotemp:[Float] = [0.0,0.0,0.0]
        var temp1:Float = 0.0
        var temp2:Float = 0.0
        
        temp1 = distance(point1onA, point2onA)
        temp2 = distance(point1onB, point2onB)
        ratiotemp[0] = temp1/temp2
        
        temp1 = distance(point2onA, point3onA)
        temp2 = distance(point2onB, point3onB)
        ratiotemp[1] = temp1/temp2
        
        temp1 = distance(point1onA, point3onA)
        temp2 = distance(point1onB, point3onB)
        ratiotemp[2] = temp1/temp2
        
        m_ratio = (ratiotemp[0]+ratiotemp[1]+ratiotemp[2])/3
    }

计算旋转矩阵R

func caliRotation(){ 
        //2点减1点
        let m12 = (point2onA.z - point1onA.z) + m_ratio*(point2onB.z - point1onB.z)
        
        let m32 = (point2onA.x - point1onA.x) + m_ratio*(point2onB.x - point1onB.x)

        let m31 = (point2onA.y - point1onA.y) + m_ratio*(point2onB.y - point1onB.y)
    
        let m21 = m12        
        //3点减1点
        let m23 = (point3onA.x - point1onA.x) + m_ratio*(point3onB.x - point1onB.x)
        
        let m13 = (point3onA.y - point1onA.y) + m_ratio*(point3onB.y - point1onB.y)
        
        
        let J = matrix_float3x3(float3(0,-m12,m13),float3(-m21,0,m23),float3(-m31,m32,0))

        let K = J.inverse      
        
        var xyz = float3(0,0,0)
        
        xyz.x = -1 * m_ratio*(point2onA.x - point1onA.x) + (point2onB.x - point1onB.x)
        
        xyz.y = -1 * m_ratio*(point2onA.y - point1onA.y) + (point2onB.y - point1onB.y)
        
        xyz.z = -1 * m_ratio*(point3onA.z - point1onA.z) + (point3onB.z - point1onB.z)
        
        let abc = K * xyz        
 
        let S = matrix_float3x3(float3(0, abc.z, abc.y), float3(-1*abc.z, 0, abc.x), float3(-1*abc.y, -1*abc.x, 0))
        
        let I = matrix_float3x3(float3(1,0,0), float3(0,1,0), float3(0,0,1))
        
        
         m_Rotation = (I + S) * ((I - S).inverse) * m_ratio    }

计算偏移量

 func caliTranslation(){
       m_Translation = point3onA - m_Rotation * point3onB    }

测试

func test(){
    
     point1onA = float3(1,0,0)
     point2onA = float3(0,1,0)
     point3onA = float3(0,0,1)
    
     let node = SCNNode()
     node.position = SCNVector3(0.1, 0.2, 0.3)
     node.eulerAngles = SCNVector3(0.1, 0.2, 0.3)
     node.scale = SCNVector3(1.2, 1.2, 1.2)
        
     let newA = node.convertPosition(SCNVector3(point1onA), from: nil)
     let newB = node.convertPosition(SCNVector3(point2onA), from: nil)
     let newC = node.convertPosition(SCNVector3(point3onA), from: nil)
        
     point1onB = float3(newA.x, newA.y, newA.z)
     point2onB = float3(newB.x, newB.y, newB.z)
     point3onB = float3(newC.x, newC.y, newC.z)
        
     caliratio()
     caliRotation()
     caliTranslation()
        
     print("比例参数λ:\(m_ratio)")
     print("平移向量Δ:\(m_Translation)")
     print("旋转矩阵R:\(m_Rotation)")
     print("变换矩阵真值:node.transform")

    }