Tangoの仕組みを理解する（AreaLearning編）

Tango Unity SDKの[Examples](https://github.com/googlesamples/tango-examples-unity)を読んで、サンプルアプリを作ります。

今回は、AreaLearningを取り上げます。主立った機能は試したので、この記事を書いたら、オリジナルアプリの開発に取り掛かる予定です。

![Screenshot_20170112-001709.png](https://qiita-image-store.s3.amazonaws.com/0/153460/b8c105fa-eebb-08cb-4589-fe60fff1741d.png)


## Area Description File（ADF） 
Area Learningは、Tangoで学習した領域をArea Description File（ADF）として保存することで、座標フレームを合わせる仕組みです。
ADFに物体のエッジなど特徴となる点や形状を記憶することで、Tangoが同じ地点を認識した際に、ADF作成時と座標系を合わせ、デバイスの位置・姿勢を推定します。


## ソースコードの解析
以下のファイルを解析します。予め、[C API Area Learning Tutorial](https://developers.google.com/tango/apis/c/c-area-learning
)に目を通しておくと、見通しがよくなります。

- TangoSDK/Examples/AreaLearning/Scripts/AreaLearningInGameController.cs
- TangoSDK/Examples/AreaLearning/Scripts/AreaDescriptionPicker.cs
- TangoSDK/Core/Scripts/TangoWrappers/AreaDescription.cs
- TangoSDK/Core/Scripts/TangoWrappers/TangoApplication.cs


以降のソースコードでは説明不要な処理を削って、見やすくしています。


### ADFの作成
`Save()`の処理を見ていきます。

```AreaLearningInGameController.cs
public void Save()
{
    StartCoroutine(_DoSaveCurrentAreaDescription());
}
```

```AreaLearningInGameController.cs
private IEnumerator _DoSaveCurrentAreaDescription()
{

    ...

    if (saveConfirmed)
    {
        // Disable interaction before saving.
        m_initialized = false;
        m_savingText.gameObject.SetActive(true);
        if (m_tangoApplication.m_areaDescriptionLearningMode)
        {
            m_saveThread = new Thread(delegate()
            {
                // Start saving process in another thread.
                m_curAreaDescription = AreaDescription.SaveCurrent();
                AreaDescription.Metadata metadata = m_curAreaDescription.GetMetadata();
                metadata.m_name = kb.text;
                m_curAreaDescription.SaveMetadata(metadata);
            });
            m_saveThread.Start();
        }
    }
}
```

ADFの作成は`SaveCurrent()`で行われます。処理に時間がかかるため、別スレッドを作成して実行しています。
`GetMetadata()`→`SetMetadata()`では、作成されたADFのメタ・データを取得して、nameを上書き保存しています。

`SaveCurrent()`では、[saveAreaDescription()](https://developers.google.com/tango/apis/c/reference/group/area-description#group___area_description_1ga07c6db409a2be634a38ae0456236e61a)を実行して、ADFを作成します。成功するとADFを一意に特定するUUIDが返ります。

```AreaDescription.cs
public static AreaDescription SaveCurrent()
{
    byte[] rawUUID = new byte[Common.UUID_LENGTH];
    if (AreaDescriptionAPI.TangoService_saveAreaDescription(rawUUID) != Common.ErrorType.TANGO_SUCCESS)
    {
        Debug.Log("Could not save area description.\n" + Environment.StackTrace);
        return null;
    }

     // Don't want to include the null terminator in the C# string.
     string uuid = Encoding.UTF8.GetString(rawUUID, 0, Common.UUID_LENGTH - 1);

     return AreaDescription.ForUUID(uuid);
}
```

`GetMetadata()`、`SaveMetadata()`についての詳しい説明は省きますが、いずれの場合も、まずは`_GetMetadataPtr()`を実行して[getAreaDescriptionMetadata()](https://developers.google.com/tango/apis/c/reference/group/area-description#group___area_description_1gad97ea9ac59c357617346b8ff715ea60a)からハンドラを取得します。

[TangoAreaDescriptionMetadata_get()](https://developers.google.com/tango/apis/c/reference/group/area-description#group___area_description_1ga736483938a6f01a62819cae0130d9ab2)でメタ・データを取得、[TangoAreaDescriptionMetadata_set()](https://developers.google.com/tango/apis/c/reference/group/area-description#group___area_description_1ga1f7308e1974cc2a962efe6e120482c5d)でメタ・データをセットして、[saveAreaDescriptionMetadata()](https://developers.google.com/tango/apis/c/reference/group/area-description.html#group___area_description_1ga3f5f7894fb3efb7b07e49e5477b35275)で保存しています。




### ADFの読み込み
`StartGame()`にて、ADFの読み込みが行われています。

```AreaDescriptionPicker.cs
public void StartGame(bool isNewAreaDescription)
{
    // The game has to be started with an Area Description.
    if (!isNewAreaDescription)
    {
        if (string.IsNullOrEmpty(m_curAreaDescriptionUUID))
        {
            AndroidHelper.ShowAndroidToastMessage("Please choose an Area Description.");
            return;
        }
    }
    else
    {
        m_curAreaDescriptionUUID = null;
    }

    // Dismiss Area Description list, footer and header UI panel.
    gameObject.SetActive(false);

    if (isNewAreaDescription)
    {
        // Completely new area description.
        m_guiController.m_curAreaDescription = null;
        m_tangoApplication.m_areaDescriptionLearningMode = true;
    }
    else
    {
        // Load up an existing Area Description.
        AreaDescription areaDescription = AreaDescription.ForUUID(m_curAreaDescriptionUUID);
        m_guiController.m_curAreaDescription = areaDescription;
        m_tangoApplication.m_areaDescriptionLearningMode = m_enableLearningToggle.isOn;
    }

    m_tangoApplication.Startup(m_guiController.m_curAreaDescription);

    ...
}
```

UIControllerの処理としては、ADFのUUIDをキーにAreaDescriptionを取得して、`TangoApplication.Startup()`に渡せばOKです。

Coresの処理まで見ていくと、AreaDescriptionは`_InitializeMotionTracking()`で使われています。

```TangoApplication.cs
public void Startup(AreaDescription  areaDescription)
{

    ...

    if (areaDescription != null)
    {
        _InitializeMotionTracking(areaDescription.m_uuid);
    }
    else
    {
        _InitializeMotionTracking(null);
    }

    ...

    _TangoConnect();
}
```

[Configuration Parameters](https://developers.google.com/tango/apis/c/reference/group/config-params#group___config_params)にUUIDをセットし、AreaDescriptionを基点にしたFramePairをPoseListenerに渡すことで、ADFの座標系への変換が行えるようになります。

```TangoApplication.cs
private void _InitializeMotionTracking(string uuid)
{

    System.Collections.Generic.List<TangoCoordinateFramePair> framePairs = new System.Collections.Generic.List<TangoCoordinateFramePair>();

    bool usedUUID = false;
    if (m_tangoConfig.SetBool(TangoConfig.Keys.ENABLE_MOTION_TRACKING_BOOL, m_enableMotionTracking) && m_enableMotionTracking)
    {
        TangoCoordinateFramePair motionTracking;
        motionTracking.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
        motionTracking.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
        framePairs.Add(motionTracking);

        if (m_enableAreaDescriptions)
        {
            if (!m_enableDriftCorrection)
            {
                m_tangoConfig.SetBool(TangoConfig.Keys.ENABLE_AREA_LEARNING_BOOL, m_areaDescriptionLearningMode);

                if (!string.IsNullOrEmpty(uuid))
                {
                    if (m_tangoConfig.SetString(TangoConfig.Keys.LOAD_AREA_DESCRIPTION_UUID_STRING, uuid))
                    {
                        usedUUID = true;
                    }
                }

                if (m_enableCloudADF)
                {
                    m_tangoConfig.SetString(TangoConfig.Keys.LOAD_AREA_DESCRIPTION_UUID_STRING, string.Empty);
                            m_tangoConfig.SetBool(TangoConfig.Keys.ENABLE_CLOUD_ADF_BOOL, true);
                    Debug.Log("Local AreaDescription cannot be loaded when cloud ADF is enabled, Tango is starting" +
                              "with cloud Area Description only." + Environment.StackTrace);
                }
            }
            else
            {
                m_tangoConfig.SetBool(TangoConfig.Keys.EXPERIMENTAL_ENABLE_DRIFT_CORRECTION_BOOL,
                                      m_enableDriftCorrection);
            }

            TangoCoordinateFramePair areaDescription;
            areaDescription.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
            areaDescription.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
                    
            TangoCoordinateFramePair startToADF;
            startToADF.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
            startToADF.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
                    
            framePairs.Add(areaDescription);
            framePairs.Add(startToADF);
        }
    }

    if (framePairs.Count > 0)
    {
        PoseListener.SetCallback(framePairs.ToArray());
    }

    // The C API does not default this to on, but it is locked down.
    m_tangoConfig.SetBool(TangoConfig.Keys.ENABLE_LOW_LATENCY_IMU_INTEGRATION, true);

    m_tangoConfig.SetBool(TangoConfig.Keys.ENABLE_MOTION_TRACKING_AUTO_RECOVERY_BOOL, m_motionTrackingAutoReset);

}
```

### ADFによるマーカー位置の補正
AreaLearningの効果がわかりやすいのは、マーカー位置の補正です。

Loop Closure Detectはコンピュータビジョンの用語で、デバイスが元の位置に戻った際に発生するセンサのズレを検出し、補正する処理のことを指すようです。
参考：http://cogrob.ensta-paristech.fr/loopclosure.html

```AreaLearningInGameController.cs
private void _UpdateMarkersForLoopClosures()
{
    // Adjust mark's position each time we have a loop closure detected.
    foreach (GameObject obj in m_markerList)
    {
        ARMarker tempMarker = obj.GetComponent<ARMarker>();
        if (tempMarker.m_timestamp != -1.0f)
        {
            TangoCoordinateFramePair pair;
            TangoPoseData relocalizedPose = new TangoPoseData();

            pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
            pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
            PoseProvider.GetPoseAtTime(relocalizedPose, tempMarker.m_timestamp, pair);

            Matrix4x4 uwTDevice = m_poseController.m_uwTss
                                  * relocalizedPose.ToMatrix4x4()
                                  * m_poseController.m_dTuc;

            Matrix4x4 uwTMarker = uwTDevice * tempMarker.m_deviceTMarker;

            obj.transform.position = uwTMarker.GetColumn(3);
            obj.transform.rotation = Quaternion.LookRotation(uwTMarker.GetColumn(2), uwTMarker.GetColumn(1));
        }
    }
}
```

ADFを用いてデバイスの姿勢を求め直し、マーカーの位置を補正しています。


## サンプルアプリの作成
### マーカーの変更
せっかくなので、今回もマーカーをクエリちゃんに変更します。

"UIController"の"Mark Prefabs"にマーカーを指定する"Element 0"〜"Element 2"があるので、プレハブ化されたクエリちゃんに変更します。
![スクリーンショット 2017-01-09 15.14.09.png](https://qiita-image-store.s3.amazonaws.com/0/153460/39e514ca-0722-8627-5fd6-2a0e495b7d15.png)


マーカーには、"ARMarker(Scripts)"を付与してあげる必要があります。
"Add Component">"Scripts">"ARMarker"でOKです。
![スクリーンショット 2017-01-09 15.15.28.png](https://qiita-image-store.s3.amazonaws.com/0/153460/f1ec007a-628c-ac74-f1f5-ad08af6fe95b.png)


### テスト
アプリを起動して周囲を撮影し、ADFを作成します。その際、マーカーを立てておきます。
![Screenshot_20170112-000707.png](https://qiita-image-store.s3.amazonaws.com/0/153460/7064cc44-f460-4582-57ad-4adf0ae8384a.png)

一旦アプリを終了して、作成したADFを選択すると、"WALK AROUND TO RELOCALIZE"のメッセージが表示されます。
![Screenshot_20170112-000249.png](https://qiita-image-store.s3.amazonaws.com/0/153460/dee2cfbc-8f88-e581-d030-0c213a9fac96.png)

この状態で、ADF作成時に撮影した方向にカメラを向けると、マーカーの位置を復元してくれます。

#### マーカー位置について

補正などを特に行っていないFloorFindingだと、視点を変えた際のズレが顕著に出ています。
![Screenshot_20170112-001059.png](https://qiita-image-store.s3.amazonaws.com/0/153460/5b405070-cd57-b4ab-fee4-fcd97f70b998.png)

![Screenshot_20170112-001123.png](https://qiita-image-store.s3.amazonaws.com/0/153460/6f21b458-f6c5-9059-8e70-0f4e1c61557f.png)


一方、**AreaDescriptionを使うと、位置が補正されて同じ地点に留まっているように見えます。**

![Screenshot_20170112-200111.png](https://qiita-image-store.s3.amazonaws.com/0/153460/7340ea2d-59c2-57b0-37d3-e4184e986bba.png)

![Screenshot_20170112-200119.png](https://qiita-image-store.s3.amazonaws.com/0/153460/de01ac9c-88d8-d366-49ba-3245829150a4.png)



## まとめ
ADFを使うと、デバイスの座標系を合わせることができるので、マーカーやメッシュの位置を正確に表すことができます。
また、AreaLearningを用いることで、MotionTrackingを行っている間の位置補正をしてくれるため、移動しても違和感のないARが実現されています。


## おわりに
今回のサンプルでは、クエリちゃん SD版モデルを使用しました。
![02_クエリちゃんライセンスロゴ-e1472646888241-300x256.png](https://qiita-image-store.s3.amazonaws.com/0/153460/148e3add-4811-9968-6aa2-b7facc5cc4e4.png)