Tangoの仕組みを理解する（3DR前編）

前回に引き続き、Tango Unity SDKの[Examples](https://github.com/googlesamples/tango-examples-unity)を読み解いていきます。

MeshBuilderのコア部分にあたるTango3DRについて書こうと思ったのですが、長くなりそうだったので前後編に分けました。


![スクリーンショット 2017-01-06 20.42.10.png](https://qiita-image-store.s3.amazonaws.com/0/153460/1f1357d2-ace9-c83f-6859-ebc180b0a9ac.png)


## Tango3DR
Tangoからのデータを元に3Dメッシュを構築するライブラリです。Tango3DRのRは、"Reconstruction"の略で復元・再構成といった意味を持ちます。
Unity Tango SDKでは、[Tango 3D Reconstruction Library C API](https://developers.google.com/tango/apis/c/reconstruction/reference/)をTangoWrappers経由で使用しています。

大枠の処理の流れとして、以下のような感じになります。

1. 3D Reconstruction serverの作成
2. カメラパラメータの設定
3. GridIndexArrayの更新
4. メッシュの作成

前編では、1、2を中心にまとめています。


## ソースコードの解析
今回は、以下のファイルだけです。

- TangoSDK/Core/Scripts/TangoWrappers/Tango3DReconstruction.cs

### 3D Reconstruction serverの作成
Tango3DReconstructionクラスのコンストラクタで実行されています。[Tango3DR_create()](https://developers.google.com/tango/apis/c/reconstruction/reference/group/mesh-building-functions.html#group___mesh_building_functions_1ga8d001b4665243daf981d21a60edecb9e)が3D Reconstruction serverを作成する関数です。

```Tango3DReconstruction.cs
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()](https://developers.google.com/tango/apis/c/reconstruction/reference/group/config-params#group___config_params_1gad2e68adfa1706c497f1829b4c21693d1)で`config`を作成して、パラメータをセットしています。各パラメータの説明は[こちら](https://developers.google.com/tango/apis/c/reconstruction/reference/group/config-params)にあります。
Tangoの座標は右手座標系なので、２番目と３番目の要素を入れ替えて、左手座標系にしています。

設定を終えたら`Tango3DR_create()`に`config`を渡します。返り値のcontextは、Tango3DRの各処理で必要になるため、`m_context`にセットされます。
反映済みの`config`は、直後に`Tango3DR_Config_destroy()`で破棄しています。


### カメラパラメータの設定
`OnTangoServiceConnected()`に記述されており、Tangoの準備ができたら呼ばれます。

depthCameraとcolorCameraで似たような処理を行っているため、depthCameraに関するコードのみ抜き出します。

```Tango3DReconstruction.cs
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)の設定です。
カメラパラメータについては[公式にも説明](https://developers.google.com/tango/overview/intrinsics-extrinsics)があるのですが、今ひとつ理解できず、色々とググった結果、[OpenCVのドキュメント](http://docs.opencv.org/master/d9/d0c/group__calib3d.html#details)と[こちらのSlideShare](http://www.slideshare.net/nonane/cv-study)が役立ちました。


外部パラメータはカメラの位置・姿勢を示す行列で、[PoseProvider.GetPoseAtTime()](https://developers.google.com/tango/apis/unity/reference/class/tango/pose-provider#class_tango_1_1_pose_provider_1aadea0e50590f00b7247a21eceabbada2)でposeDataとして求められるようです。

ここで計算しているのは、[デバイス上の座標系変換](https://developers.google.com/tango/overview/coordinate-systems#on-device_coordinate_frames)のためのパラメータです。IMUを基点に計算しているのは、IMUがデバイスの位置や傾きを測定するセンサだからですかね。IMUについては[このはてブ](http://myenigma.hatenablog.com/entry/2015/11/09/183738)が詳しいです。

内部パラメータは、[VideoOverlayProvider.GetIntrinsics()](https://developers.google.com/tango/apis/unity/reference/class/tango/video-overlay-provider#class_tango_1_1_video_overlay_provider_1a8e1050de892f8af5310eb96eb0bffc71)から取得でき、[Tango3DR_setDepthCalibration()](https://developers.google.com/tango/apis/c/reconstruction/reference/group/mesh-building-functions#group___mesh_building_functions_1ga4708e919fe64d4b45d65d586b8c47e54)でそのまま渡しています。


設定したパラメータは、pointCloudの更新時に使われます。

```Tango3DReconstruction.cs
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を基点としたデバイスの移動・回転になります。いずれ消えちゃうかもしれませんが、[古いリファレンス](http://blog-imgs-78.fc2.com/r/a/m/ramenman3/20150411230302e80.html)の図がわかりやすいと思います。
AreaDescriptionについては、AreaLearningを扱う際に改めて調査することにします。

求めた姿勢にTango起動時に求めた行列でデバイス→デプス・カメラの座標変換をかけると、Unityのワールド座標系とTangoの座標系の変換ができる・・・といった感じかと思います。

`_UpdateDepth()`以降がpointCloudからメッシュを作成する処理になりますが、そちらは[後編](http://qiita.com/jyuko/items/ef35923806d7923f642c)とします。


## まとめ
前編は、Tango3DRでメッシュ作成する前のパラメータ設定についてまとめました。
カメラパラメータについては、まだ完全に理解し切れていない部分もあるので、間違った記述になっていたらすいません。

本文中でも引用しましたが、Tangoのドキュメントだけでなく、コンピュータビジョン分野の用語や理論を学ぶと、理解しやすくなると思います。