【Unity, Open3d】Unityで取得した深度画像とカラー画像から作成した点群を用いたOpen3dでの位置合わせ

実現したいこと

Unityで取得した深度画像とカラー画像から作成した点群とそのオブジェクトのメッシュから作成した点群の位置合わせ

前提

Open3dでUnityで指定した回転と同じ回転を行うにはどのようにしたらよいのでしょうか？

試したこと・発生している問題

Unityで位置を[x, y, z] = [3, 4, 3]，回転を[x, y, z] = [30, 40, 50]に指定し深度画像とカラー画像を取得し，それをもとにOpen3dで作成した点群（pcd_scan）と，オブジェクトのメッシュから作成した点群（pcd_cad）の位置合わせをしたいです．

まずはUnityとOpen3dの座標系の違いを調べました．Unityは左手系，Open3dは右手系になっており，Unityで位置は原点のまま，回転もしていないpcd_scanとpcd_cadを何もせずそのまま表示すると次の画像のようになりました．

上にあるのがpcd_cad，下にあるのがpcd_scanでそれぞれの座標系も示しました．（pcd_scanの方はUnity画面で確認して手書き）
この図からUnityのx軸y軸z軸はOpen3dではそれぞれx軸z軸y軸に対応すると考えました．

また，各軸の回転角の向きを調べるためにUnityで位置を[x, y, z] = [0, 0, 0]，回転を[x, y, z] = [10, 0, 0], [0, 10, 0], [0, 0, 10]に指定した3つの点群を作成しOpen3dでpcd_cadをx軸z軸y軸に±10度回転させ，Open3dでは各軸正負のどちらに回転させればUnityの回転を実現できるか調べました．

その結果が次のようになります．ピンクの点群がOpen3dでpcd_cadを10度回転させた点群，青の点群がOpen3dでpcd_cadを-10度回転させた点群です．

↑　Unityでx軸に10度回転させた点群とOpen3dでpcd_cadをx軸に±10度回転させた様子

↑　Unityでy軸に10度回転させた点群とOpen3dでpcd_cadをz軸に±10度回転させた様子

↑　Unityでz軸に10度回転させた点群とOpen3dでpcd_cadをy軸に±10度回転させた様子

この結果から各軸の回転について次のような対応があると考えました
Unity　　　　　Open3d
x軸に10度回転⇔x軸に-10度回転
y軸に10度回転⇔z軸に10度回転
z軸に10度回転⇔y軸に-10度回転

そして，Unityのマニュアルに

Unity では、Z 軸、X 軸、Y 軸の順にオイラー角回転を行います。

との記述があったのでopen3dでは回転をy軸, x軸, z軸の順に行うことにしました．
https://docs.unity3d.com/ja/2021.3/Manual/QuaternionAndEulerRotationsInUnity.html

そうして位置合わせを行った結果が次の画像です．

かなりずれが生じていることが確認できます．このずれが生じる原因の検討がつかないのが現状です．

該当のソースコード

Unityで位置と回転の情報を取得するためのスクリプトとopen3dで実行したpythonの2つです

SavePosQuaToText.cs
1using System;
2using System.Collections;
3using System.Collections.Generic;
4using System.IO;
5using UnityEngine;
6
7public class SavePosQuaToText : MonoBehaviour
8{
9    // Start is called before the first frame update
10    void Start()
11    {
12        
13    }
14
15    // Update is called once per frame
16    void Update()
17    {
18        if (Input.GetKeyDown(KeyCode.Space))
19        {
20            SaveToText();
21        }
22    }
23
24    void SaveToText()
25    {
26        Debug.Log("START Append Text");
27
28        string[] strings = { "bunny", "bunny (1)", "bunny (2)", "bunny (3)", "bunny (4)", "bunny (5)", "bunny (6)", "bunny (7)", "bunny (8)" };
29
30        for (int i = 0; i < strings.Length; i++)
31        {
32            string ObjectName = strings[i];
33            GameObject FocusObject = GameObject.Find(ObjectName);
34            SavePosition(FocusObject);
35            SaveQuaternionXYZ(FocusObject);
36        }
37
38        Debug.Log("FINISH Append Text");
39    }
40
41    void SavePosition(GameObject FocusObject)
42    {
43        Vector3 pos = FocusObject.transform.position;
44        string filePath = Application.dataPath + "/../Position.txt";
45        File.AppendAllText(filePath, Convert.ToString(pos.x) + "," + Convert.ToString(pos.y) + "," + Convert.ToString(pos.z) + "\n");
46    }
47    void SaveQuaternionXYZ(GameObject FocusObject)
48    {
49        float x = FocusObject.transform.localEulerAngles.x;
50        float y = FocusObject.transform.localEulerAngles.y;
51        float z = FocusObject.transform.localEulerAngles.z;
52        var rotX = Quaternion.AngleAxis(x, new Vector3(1, 0, 0));
53        var rotY = Quaternion.AngleAxis(y, new Vector3(0, 1, 0));
54        var rotZ = Quaternion.AngleAxis(z, new Vector3(0, 0, 1));
55        string filePathqua = Application.dataPath + "/../Quaternion.txt";
56        File.AppendAllText(filePathqua, Convert.ToString(rotX.x) + "," + Convert.ToString(rotX.y) + "," + Convert.ToString(rotX.z) + "," + Convert.ToString(rotX.w) + "\n");
57        File.AppendAllText(filePathqua, Convert.ToString(rotY.x) + "," + Convert.ToString(rotY.y) + "," + Convert.ToString(rotY.z) + "," + Convert.ToString(rotY.w) + "\n");
58        File.AppendAllText(filePathqua, Convert.ToString(rotZ.x) + "," + Convert.ToString(rotZ.y) + "," + Convert.ToString(rotZ.z) + "," + Convert.ToString(rotZ.w) + "\n");
59    }
60}

Python
1def get_trans_mat(R, t):
2    trans_mat = np.identity(4)
3    trans_mat[0:3, 0:3] = R
4    trans_mat[0:3, 3] = t
5    return trans_mat
6
7def Rotmat_xyz(x, y, z):
8    R_x = np.array([
9        [1, 0, 0],
10        [0, math.cos(math.radians(x)), math.sin(math.radians(x))],
11        [0, (-1)*math.sin(math.radians(x)), math.cos(math.radians(x))]
12    ])
13    R_y = np.array([
14        [math.cos(math.radians(y)), 0, (-1)*math.sin(math.radians(y))],
15        [0, 1, 0],
16        [math.sin(math.radians(y)), 0, math.cos(math.radians(y))]
17    ])
18    R_z = np.array([
19        [math.cos(math.radians(z)), math.sin(math.radians(z)), 0],
20        [(-1)*math.sin(math.radians(z)), math.cos(math.radians(z)), 0],
21        [0, 0, 1]
22    ])
23    return R_x, R_y, R_z
24
25def Rotmat_form_quaternion(q):
26    return np.array([
27        [q[3]*q[3]+q[0]*q[0]-q[1]*q[1]-q[2]*q[2], 2*(q[0]*q[1]-q[2]*q[3]), 2*(q[0]*q[2]+q[1]*q[3])],
28        [2*(q[0]*q[1]+q[2]*q[3]), q[3]*q[3]-q[0]*q[0]+q[1]*q[1]-q[2]*q[2], 2*(q[1]*q[2]-q[0]*q[3])],
29        [2*(q[0]*q[2]-q[1]*q[3]), 2*(q[1]*q[2]+q[0]*q[3]), q[3]*q[3]-q[0]*q[0]-q[1]*q[1]+q[2]*q[2]]
30    ])
31
32#Unityで取得した各軸の回転クォータニオンをテキストファイルから読み込む
33num = 2
34new_mat = np.zeros((3*num, 4))
35read_textfile(filepath_new, new_mat)
36quaX = new_mat[0]
37quaY = new_mat[1]
38quaZ = new_mat[2]
39
40#左手系のクォータニオンを右手系に変換
41quaX_right = np.array([-quaX[0], quaX[1], -quaX[2], quaX[3]])
42quaY_right = np.array([-quaY[0], quaY[1], -quaY[2], quaY[3]])
43quaZ_right = np.array([-quaZ[0], quaZ[1], -quaZ[2], quaZ[3]])
44#右手系のクォータニオンから右手系の回転行列を計算
45R_quaX = Rotmat_form_quaternion(quaX_right)
46R_quaY = Rotmat_form_quaternion(quaY_right)
47R_quaZ = Rotmat_form_quaternion(quaZ_right)
48
49mesh_coordinate = o3d.geometry.TriangleMesh.create_coordinate_frame(size=3.0)
50o3d.visualization.draw_geometries([mesh_coordinate, pcd_cad, pcd_scan])
51
52#取得点群をz軸に180度回転しモデルと向きを合わせた後にモデルの位置まで平行移動
53R_x180, R_y180, R_z180 = Rotmat_xyz(180, 180, 180)
54trans_mat_z180 = get_trans_mat(R_z180, np.zeros(3))
55pcd_scan.transform(trans_mat_z180)
56trans_mat = get_trans_mat(np.identity(3),  -(np.array([0, 0, -15]) + np.array([-3, -3, 4])))
57pcd_scan.transform(trans_mat)
58o3d.visualization.draw_geometries([mesh_coordinate, pcd_cad, pcd_scan])
59
60#モデル点群を回転（Open3dでの回転）
61trans_mat_quaX = get_trans_mat(R_quaX.T, np.zeros(3))
62trans_mat_quaY = get_trans_mat(R_quaY, np.zeros(3))
63trans_mat_quaZ = get_trans_mat(R_quaZ, np.zeros(3))
64pcd_cad.transform(trans_mat_quaY)
65pcd_cad.transform(trans_mat_quaX)
66pcd_cad.transform(trans_mat_quaZ)
67o3d.visualization.draw_geometries([pcd_scan, pcd_cad])

補足情報

pcd_cadは次のように作成しました．

Python
1mesh = o3d.io.read_triangle_mesh("stanford_bunny.stl")
2xyz = np.asarray(mesh.vertices)
3pcd_cad = o3d.geometry.PointCloud()
4pcd_cad.points = o3d.utility.Vector3dVector(xyz)
5pcd_cad.paint_uniform_color([0.5, 0.5, 0.5])