前回に引き続き、Tango Unity SDKのExamplesを読み解いていきます。
MeshBuilderのコア部分にあたるTango3DRについて書こうと思ったのですが、長くなりそうだったので前後編に分けました。
Tango3DR
Tangoからのデータを元に3Dメッシュを構築するライブラリです。Tango3DRのRは、"Reconstruction"の略で復元・再構成といった意味を持ちます。
Unity Tango SDKでは、Tango 3D Reconstruction Library C APIをTangoWrappers経由で使用しています。
大枠の処理の流れとして、以下のような感じになります。
- 3D Reconstruction serverの作成
- カメラパラメータの設定
- GridIndexArrayの更新
- メッシュの作成
前編では、1、2を中心にまとめています。
ソースコードの解析
今回は、以下のファイルだけです。
- TangoSDK/Core/Scripts/TangoWrappers/Tango3DReconstruction.cs
3D Reconstruction serverの作成
Tango3DReconstructionクラスのコンストラクタで実行されています。Tango3DR_create()が3D Reconstruction serverを作成する関数です。
internal Tango3DReconstruction(float resolution, bool generateColor, bool spaceClearing, int minNumVertices,
UpdateMethod updateMethod)
{
IntPtr config = API.Tango3DR_Config_create((int)APIConfigType.Context);
API.Tango3DR_Config_setDouble(config, "resolution", resolution);
API.Tango3DR_Config_setBool(config, "generate_color", generateColor);
API.Tango3DR_Config_setBool(config, "use_space_clearing", spaceClearing);
API.Tango3DR_Config_setInt32(config, "min_num_vertices", minNumVertices);
API.Tango3DR_Config_setInt32(config, "update_method", (int)updateMethod);
// The 3D Reconstruction library can not handle a left handed transformation during update. Instead,
// transform into the Unity world space via the external_T_tango config.
APIMatrix3x3 unityWorld_T_startService = new APIMatrix3x3();
unityWorld_T_startService.SetRow(0, new Vector3(1, 0, 0));
unityWorld_T_startService.SetRow(1, new Vector3(0, 0, 1));
unityWorld_T_startService.SetRow(2, new Vector3(0, 1, 0));
API.Tango3DR_Config_setMatrix3x3(config, "external_T_tango", ref unityWorld_T_startService);
API.Tango3DR_Config_setBool(config, "use_clockwise_winding_order", true);
m_context = API.Tango3DR_create(config);
API.Tango3DR_Config_destroy(config);
}
前処理として、Tango3DR_Config_create()でconfig
を作成して、パラメータをセットしています。各パラメータの説明はこちらにあります。
Tangoの座標は右手座標系なので、2番目と3番目の要素を入れ替えて、左手座標系にしています。
設定を終えたらTango3DR_create()
にconfig
を渡します。返り値のcontextは、Tango3DRの各処理で必要になるため、m_context
にセットされます。
反映済みのconfig
は、直後にTango3DR_Config_destroy()
で破棄しています。
カメラパラメータの設定
OnTangoServiceConnected()
に記述されており、Tangoの準備ができたら呼ばれます。
depthCameraとcolorCameraで似たような処理を行っているため、depthCameraに関するコードのみ抜き出します。
public void OnTangoServiceConnected()
{
// Calculate the camera extrinsics.
TangoCoordinateFramePair pair;
TangoPoseData imu_T_devicePose = new TangoPoseData();
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(imu_T_devicePose, 0, pair);
TangoPoseData imu_T_depthCameraPose = new TangoPoseData();
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_DEPTH;
PoseProvider.GetPoseAtTime(imu_T_depthCameraPose, 0, pair);
...
// Convert into matrix form to combine the poses.
Matrix4x4 device_T_imu = Matrix4x4.Inverse(imu_T_devicePose.ToMatrix4x4());
m_device_T_depthCamera = device_T_imu * imu_T_depthCameraPose.ToMatrix4x4();
...
// Update the camera intrinsics.
TangoCameraIntrinsics intrinsics = new TangoCameraIntrinsics();
Status status;
...
APICameraCalibration depthCameraCalibration;
VideoOverlayProvider.GetIntrinsics(TangoEnums.TangoCameraId.TANGO_CAMERA_DEPTH, intrinsics);
depthCameraCalibration.calibration_type = (int)intrinsics.calibration_type;
depthCameraCalibration.width = intrinsics.width;
depthCameraCalibration.height = intrinsics.height;
depthCameraCalibration.cx = intrinsics.cx;
depthCameraCalibration.cy = intrinsics.cy;
depthCameraCalibration.fx = intrinsics.fx;
depthCameraCalibration.fy = intrinsics.fy;
depthCameraCalibration.distortion0 = intrinsics.distortion0;
depthCameraCalibration.distortion1 = intrinsics.distortion1;
depthCameraCalibration.distortion2 = intrinsics.distortion2;
depthCameraCalibration.distortion3 = intrinsics.distortion3;
depthCameraCalibration.distortion4 = intrinsics.distortion4;
lock (m_lockObject)
{
status = (Status)API.Tango3DR_setDepthCalibration(m_context, ref depthCameraCalibration);
}
...
}
前半がカメラの外部パラメータ(camera extrinsics)の計算、後半がカメラの内部パラメータ(camera intrinsics)の設定です。
カメラパラメータについては公式にも説明があるのですが、今ひとつ理解できず、色々とググった結果、OpenCVのドキュメントとこちらのSlideShareが役立ちました。
外部パラメータはカメラの位置・姿勢を示す行列で、PoseProvider.GetPoseAtTime()でposeDataとして求められるようです。
ここで計算しているのは、デバイス上の座標系変換のためのパラメータです。IMUを基点に計算しているのは、IMUがデバイスの位置や傾きを測定するセンサだからですかね。IMUについてはこのはてブが詳しいです。
内部パラメータは、VideoOverlayProvider.GetIntrinsics()から取得でき、Tango3DR_setDepthCalibration()でそのまま渡しています。
設定したパラメータは、pointCloudの更新時に使われます。
public void OnTangoPointCloudMultithreadedAvailable(ref TangoPointCloudIntPtr pointCloud)
{
...
// Build World T depth camera
TangoPoseData world_T_devicePose = new TangoPoseData();
if (m_useAreaDescriptionPose)
{
TangoCoordinateFramePair pair;
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(world_T_devicePose, pointCloud.m_timestamp, pair);
}
else
{
TangoCoordinateFramePair pair;
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(world_T_devicePose, pointCloud.m_timestamp, pair);
}
...
// The 3D Reconstruction library can not handle a left handed transformation during update. Instead,
// transform into the Unity world space via the external_T_tango config.
Matrix4x4 world_T_depthCamera = world_T_devicePose.ToMatrix4x4() * m_device_T_depthCamera;
_UpdateDepth(pointCloud, world_T_depthCamera);
}
ここでは、pointCloud更新時のデバイスの姿勢を求めています。Tango接続時の姿勢もしくは領域学習したAreaDescriptionを基点としたデバイスの移動・回転になります。いずれ消えちゃうかもしれませんが、古いリファレンスの図がわかりやすいと思います。
AreaDescriptionについては、AreaLearningを扱う際に改めて調査することにします。
求めた姿勢にTango起動時に求めた行列でデバイス→デプス・カメラの座標変換をかけると、Unityのワールド座標系とTangoの座標系の変換ができる・・・といった感じかと思います。
_UpdateDepth()
以降がpointCloudからメッシュを作成する処理になりますが、そちらは後編とします。
まとめ
前編は、Tango3DRでメッシュ作成する前のパラメータ設定についてまとめました。
カメラパラメータについては、まだ完全に理解し切れていない部分もあるので、間違った記述になっていたらすいません。
本文中でも引用しましたが、Tangoのドキュメントだけでなく、コンピュータビジョン分野の用語や理論を学ぶと、理解しやすくなると思います。