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

Tango Unity SDKにパッケージされている[Examplesのソースコード](https://github.com/googlesamples/tango-examples-unity)を読んで、サンプルアプリを作ります。

今回は、床（平面）を認識してマーカーを配置するExperimentalFloorFindingです。

![Screenshot_20170102-202251.png](https://qiita-image-store.s3.amazonaws.com/0/153460/903675ef-c3ba-a2f7-c264-6115b7c03a02.png)


## ソースコードの解析
以下の3ファイルを解析・改変します。
ソースコードのライセンスは、[Apache License 2.0]
(http://www.apache.org/licenses/LICENSE-2.0)です。

- TangoSDK/Examples/ExperimentalFloorFinding/Scripts/TangoFloorFindingUIController.cs
- TangoPrefabs/Scripts/TangoPointCloud.cs
- TangoPrefabs/Scripts/TangoPointCloudFloor.cs

### 床（平面）の高さを求める
GUIの"Find Floor"ボタンがトリガになります。ボタン押下時に、`m_pointCloud.FindFloor()`を呼び出しています。

```TangoFloorFindingUIController.cs
public void OnGUI()
{
    GUI.color = Color.white;

    if (!m_findingFloor)
    {
        if (GUI.Button(new Rect(Screen.width - 220, 20, 200, 80), "<size=30>Find Floor</size>"))
        {
            if (m_pointCloud == null)
            {
                Debug.LogError("TangoPointCloud required to find floor.");
                return;
            }

            m_findingFloor = true;
            m_marker.SetActive(false);
            m_tangoApplication.SetDepthCameraRate(TangoEnums.TangoDepthCameraRate.MAXIMUM);
            m_pointCloud.FindFloor();
        }
    }
    else
    {
            GUI.Label(new Rect(0, Screen.height - 50, Screen.width, 50), "<size=30>Searching for floor position. Make sure the floor is visible.</size>");
    }
}
```

`FindFloor()`では`m_findFloorWithDepth`にtrueをセットしています。

```TangoPointCloud.cs
public void FindFloor()
{
    m_floorFound = false;
    m_findFloorWithDepth = true;
    m_floorPlaneY = 0.0f;
}
```

pointCloudを取得するたびに`OnTangoPointCloudAvailable()`がcallされ、フラグを立てたことで`_FindFloorWithDepth()`が実行されます。
pointCloudについては、[前回の記事](http://qiita.com/jyuko/items/8db0f0160c550a3c3f01)を参照ください。

```TangoPointCloud.cs
public void OnTangoPointCloudAvailable(TangoPointCloudData pointCloud)
{
    ...
        // Try to find the floor using this set of depth points if requested.
        if (m_findFloorWithDepth)
        {
            _FindFloorWithDepth();
        }
    ...
}
```

`_FindFloorWithDepth()`がメインの処理になります。


```TangoPointCloud.cs
private const int RECOGNITION_THRESHOLD = 1000;
private const int NOISE_THRESHOLD = 500;
private const float SENSITIVITY = 0.02f;

private void _FindFloorWithDepth()
{
    m_numPointsAtY.Clear();
    m_nonNoiseBuckets.Clear();
        
    // Count each depth point into a bucket based on its world position y value.
    for (int i = 0; i < m_pointsCount; i++)
    {
        Vector3 point = m_points[i];
        if (!point.Equals(Vector3.zero))
        {
            // Group similar points into buckets based on sensitivity. 
            float roundedY = Mathf.Round(point.y / SENSITIVITY) * SENSITIVITY;
            if (!m_numPointsAtY.ContainsKey(roundedY))
            {
                m_numPointsAtY.Add(roundedY, 0);
            }
            
            m_numPointsAtY[roundedY]++;

            // Check if the y plane is a non-noise plane.
            if (m_numPointsAtY[roundedY] > NOISE_THRESHOLD && !m_nonNoiseBuckets.Contains(roundedY))
            {
                m_nonNoiseBuckets.Add(roundedY);
            }
        }
    }
    
    // Find a plane at the y value. The y value must be below the camera y position.
    m_nonNoiseBuckets.Sort();
    for (int i = 0; i < m_nonNoiseBuckets.Count; i++)
    {
        float yBucket = m_nonNoiseBuckets[i];
        int numPoints = m_numPointsAtY[yBucket];
        if (numPoints > RECOGNITION_THRESHOLD && yBucket < Camera.main.transform.position.y)
        {
            // Reject the plane if it is not the lowest.
            if (yBucket > m_nonNoiseBuckets[0])
            {
                return;
            }
            
            m_floorFound = true;
            m_findFloorWithDepth = false;
            m_floorPlaneY = yBucket;
            m_numPointsAtY.Clear();
            m_nonNoiseBuckets.Clear();
        }
    }
}
```

行っている処理はシンプルで、pointCloudから高さ(y座標)が同じpointの数を数えて、

- 一定数同じ高さのpointがある（平面である）
- カメラより位置が低い

の条件を満たす高さ(m_floorPlaneY)を求めています。


前半のループでは丸めた値を`m_numPointsAtY`でカウントしています。元の値はmm(ミリメートル)より細かい精度なので、`SENSITIVITY`で丸めています。`SENSITIVITY = 0.02f`は、0.02m = 2cmの精度です。
`m_nonNoiseBuckets`には、同じ高さのpointが`NOISE_THRESHOLD`を超えたときだけ、高さの値が格納されます。この配列が求める高さの候補になります。

後半の処理では、`m_nonNoiseBuckets`をソートした後、ループしているのですが、結局、一番小さい値以外は除外しているので、あまり意味はないような気もします。
if文の条件式で、point数が`RECOGNITION_THRESHOLD`より大きく、高さがカメラよりも低い場合、値を`m_floorPlaneY`にセットしています。
`RECOGNITION_THRESHOLD`と`NOISE_THRESHOLD`を別に定義している理由は、よくわかっていません。


### マーカーを配置する
`TangoPointCloudFloor`のy座標が`m_floorPlaneY`で更新されます。

```TangoPointCloudFloor.cs
public void Update()
{
    // If the point cloud has found the floor, adjust the position accordingly.
    if (m_pointCloud.m_floorFound)
    {
        m_floorFound = true;
        if (transform.position.y != m_pointCloud.m_floorPlaneY)
        {
            transform.position = new Vector3(0.0f, m_pointCloud.m_floorPlaneY, 0.0f);
            foreach (Transform t in transform)
            {
                t.gameObject.SetActive(true);
            }
        }
　　　　　　　　...
    }
    else
    {
        m_floorFound = false;
        m_depthTriggered = false;
    }
}
```

UIControllerに戻って、`TangoPointCloudFloor`が更新された後の処理を行います。
画面の中心方向にRayCastを伸ばして、Floorと衝突した点にマーカーを移動しています。

```TangoFloorFindingUIController.cs
public void Update()
{
    ...

    if (!m_findingFloor)
    {
        return;
    }

    // If the point cloud floor has found a new floor, place the marker at the found y position.
    if (m_pointCloudFloor.m_floorFound && m_pointCloud.m_floorFound)
    {
        m_findingFloor = false;

        // Place the marker at the center of the screen at the found floor height.
        m_marker.SetActive(true);
        Vector3 target;
        RaycastHit hitInfo;
        if (Physics.Raycast(Camera.main.ScreenPointToRay(new Vector3(Screen.width / 2.0f, Screen.height / 2.0f)), out hitInfo))
        {
            // Limit distance of the marker position from the camera to the camera's far clip plane. This makes sure that the marker
            // is visible on screen when the floor is found.
            Vector3 cameraBase = new Vector3(Camera.main.transform.position.x, hitInfo.point.y, Camera.main.transform.position.z);
            target = cameraBase + Vector3.ClampMagnitude(hitInfo.point - cameraBase, Camera.main.farClipPlane * 0.9f);
        }
        else
        {
            // If no raycast hit, place marker in the camera's forward direction.
            Vector3 dir = new Vector3(Camera.main.transform.forward.x, 0.0f, Camera.main.transform.forward.z);
            target = dir.normalized * (Camera.main.farClipPlane * 0.9f);
            target.y = m_pointCloudFloor.transform.position.y;
        }

        m_marker.transform.position = target;
        ...
    }
}
```




## サンプルアプリの作成

### マーカーの変更
GUIに紐付いているARMarkerを任意のGameObjectに変更するだけでOKです。
スクリーンショットの赤枠部分です。
![スクリーンショット 2017-01-02 22.23.19.png](https://qiita-image-store.s3.amazonaws.com/0/153460/0530a26f-b578-48dd-24c7-00e02b98ac3e.png)

### マーカーの向きを変更
マーカーの位置を更新する際に、`LookAt`でカメラの方向を向くようにしました。

```TangoFloorFindingUIController.cs
public void Update()
{
    ...

    if (m_pointCloudFloor.m_floorFound && m_pointCloud.m_floorFound)
    {　
        ...

        m_marker.transform.position = target;
        m_marker.transform.LookAt (Camera.main.transform.position); // 追加
        ...
    }
}
```

### テスト
![Screenshot_20170102-202251.png](https://qiita-image-store.s3.amazonaws.com/0/153460/903675ef-c3ba-a2f7-c264-6115b7c03a02.png)

![Screenshot_20170102-202903.png](https://qiita-image-store.s3.amazonaws.com/0/153460/dc0db676-e601-1557-0007-41698542ea98.png)


## まとめ
コメントが多めに書かれているので、[基礎を理解](http://qiita.com/jyuko/items/8db0f0160c550a3c3f01)して処理の流れを追えれば、読み解くのは難しくないと思います。

ARの静止画ではそれっぽく見えていますが、サンプルアプリを動かした感触では、実用レベルでの精度は改めて評価する必要があるかなと。
`_FindFloorWithDepth()`のロジックには改良の余地がありそうです。

【2017/1/12追記】
マーカーの配置に関しては、[AreaLearningを用いることで、マーカー座標のズレを補正](http://qiita.com/jyuko/items/8eb208104c1c132ff381)する方法があり、そちらの処理を採用した方が精度が上がります。

## おわりに
今回のサンプルでは、[クエリちゃん SD版モデル](http://query-chan.com/download/)を使用しました。
[![02_クエリちゃんライセンスロゴ-e1472646888241-300x256.png](https://qiita-image-store.s3.amazonaws.com/0/153460/0e428400-07c9-17ff-fe29-6be7b7e51062.png)](http://query-chan.com/download/)