Unity下的ICP坐标变换

希望求得Unity场景中物体A与物体B的变换，通过ICP迭代求得。
已知：
B的局部坐标：Vector3[] vertices_model = modelObject.GetComponent<MeshFilter>().mesh.vertices;，A的局部坐标Vector3[] vertices_sample = sampleObject.GetComponent<MeshFilter>().mesh.vertices;。
将两物体的局部坐标变换为世界坐标：

Matrix4x4 localToWorld_model = modelObject.transform.localToWorldMatrix;
for (int i = 0; i < vertices_model.Length; i++)
{
    Vector3 world_v = localToWorld_model.MultiplyPoint3x4(vertices_model[i]);
    Model_points[0, i] = world_v.x; Model_points[1, i] = world_v.y; Model_points[2, i] = world_v.z;
}

Matrix4x4 localToWorld_sample = sampleObject.transform.localToWorldMatrix;
for (int i = 0; i < vertices_sample.Length; i++)
{
    Vector3 world_v = localToWorld_sample.MultiplyPoint3x4(vertices_sample[i]);
    Sample_points[0, i] = world_v.x; Sample_points[1, i] = world_v.y; Sample_points[2, i] = world_v.z;
}

使用ICP算法求得A的世界坐标到B的世界坐标的变换：Rt = ICP.ICP_run(Model_points, Sample_points, threshold, max_itr, true);

整个变换过程为：

一：B不动，保持A的局部坐标不变，变换A的局部坐标系，使物体A变换到物体B上。

该过程的含义为，将物体A的局部坐标变换到B的世界坐标上，变换矩阵为：

Matrix<double> Rt_localToWorld = m.Dense(4, 4, 0);
Matrix<double> Rt_coordinate = m.Dense(4, 4, 0);//坐标系变换
Rt_localToWorld[0, 0] = localToWorld_sample[0, 0]; Rt_localToWorld[1, 0] = localToWorld_sample[1, 0]; Rt_localToWorld[2, 0] = localToWorld_sample[2, 0]; Rt_localToWorld[3, 0] = localToWorld_sample[3, 0];
Rt_localToWorld[0, 1] = localToWorld_sample[0, 1]; Rt_localToWorld[1, 1] = localToWorld_sample[1, 1]; Rt_localToWorld[2, 1] = localToWorld_sample[2, 1]; Rt_localToWorld[3, 1] = localToWorld_sample[3, 1];
Rt_localToWorld[0, 2] = localToWorld_sample[0, 2]; Rt_localToWorld[1, 2] = localToWorld_sample[1, 2]; Rt_localToWorld[2, 2] = localToWorld_sample[2, 2]; Rt_localToWorld[3, 2] = localToWorld_sample[3, 2];
Rt_localToWorld[0, 3] = localToWorld_sample[0, 3]; Rt_localToWorld[1, 3] = localToWorld_sample[1, 3]; Rt_localToWorld[2, 3] = localToWorld_sample[2, 3]; Rt_localToWorld[3, 3] = localToWorld_sample[3, 3];
//计算Sample到Model的变换矩阵
Rt = ICP.ICP_run(Model_points, Sample_points, threshold, max_itr, true);
//计算局部坐标系的变换
Rt_coordinate = Rt.Multiply(Rt_localToWorld);

Matrix4x4 transform_coordinate = Matrix4x4.identity;
transform_coordinate[0, 0] = (float)Rt_coordinate[0, 0]; transform_coordinate[1, 0] = (float)Rt_coordinate[1, 0]; transform_coordinate[2, 0] = (float)Rt_coordinate[2, 0]; transform_coordinate[3, 0] = (float)Rt_coordinate[3, 0];
transform_coordinate[0, 1] = (float)Rt_coordinate[0, 1]; transform_coordinate[1, 1] = (float)Rt_coordinate[1, 1]; transform_coordinate[2, 1] = (float)Rt_coordinate[2, 1]; transform_coordinate[3, 1] = (float)Rt_coordinate[3, 1];
transform_coordinate[0, 2] = (float)Rt_coordinate[0, 2]; transform_coordinate[1, 2] = (float)Rt_coordinate[1, 2]; transform_coordinate[2, 2] = (float)Rt_coordinate[2, 2]; transform_coordinate[3, 2] = (float)Rt_coordinate[3, 2];
transform_coordinate[0, 3] = (float)Rt_coordinate[0, 3]; transform_coordinate[1, 3] = (float)Rt_coordinate[1, 3]; transform_coordinate[2, 3] = (float)Rt_coordinate[2, 3]; transform_coordinate[3, 3] = (float)Rt_coordinate[3, 3];

//变换物体A在Unity中的坐标
sampleObject.transform.position = new Vector3(transform_coordinate[0, 3], transform_coordinate[1, 3], transform_coordinate[2, 3]);
sampleObject.transform.rotation = transform_coordinate.rotation;

二：保持A不动，使物体B变换到物体A上。

该过程的含义为将物体B的局部坐标变换到物体A的世界坐标

下面场景中由于得到的是物体B的世界坐标，因此只计算TICP的逆矩阵即可：

Matrix<double> Rt_Inverse = Inverse(Rt);
Matrix4x4 transform_coordinate = Matrix4x4.identity;
 transform_coordinate[0, 0] = (float)Rt_Inverse[0, 0]; transform_coordinate[1, 0] = (float)Rt_Inverse[1, 0]; transform_coordinate[2, 0] = (float)Rt_Inverse[2, 0]; transform_coordinate[3, 0] = (float)Rt_Inverse[3, 0];
transform_coordinate[0, 1] = (float)Rt_Inverse[0, 1]; transform_coordinate[1, 1] = (float)Rt_Inverse[1, 1]; transform_coordinate[2, 1] = (float)Rt_Inverse[2, 1]; transform_coordinate[3, 1] = (float)Rt_Inverse[3, 1];
transform_coordinate[0, 2] = (float)Rt_Inverse[0, 2]; transform_coordinate[1, 2] = (float)Rt_Inverse[1, 2]; transform_coordinate[2, 2] = (float)Rt_Inverse[2, 2]; transform_coordinate[3, 2] = (float)Rt_Inverse[3, 2];
transform_coordinate[0, 3] = (float)Rt_Inverse[0, 3]; transform_coordinate[1, 3] = (float)Rt_Inverse[1, 3]; transform_coordinate[2, 3] = (float)Rt_Inverse[2, 3]; transform_coordinate[3, 3] = (float)Rt_Inverse[3, 3];
        
modelObject.transform.position = new Vector3(transform_coordinate[0, 3], transform_coordinate[1, 3], transform_coordinate[2, 3]);
modelObject.transform.rotation = transform_coordinate.rotation;