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

[前の記事](http://qiita.com/jyuko/items/1ca4ddff0accd6b483f6)の続きです。

MeshBuilderのコア部分にあたるTango3DRの後編になります。

![スクリーンショット 2017-01-06 20.42.10.png](https://qiita-image-store.s3.amazonaws.com/0/153460/68d065d0-092a-2179-7e09-733ee37c5c22.png)


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

の3、4についてまとめます。


## GridIndexArray
GridIndexArrayについては、MeshBuilderで作成されたメッシュを見るとわかりやすいです。

![スクリーンショット 2017-01-06 23.03.54.png](https://qiita-image-store.s3.amazonaws.com/0/153460/e264b606-123a-6933-d7eb-ec59a807b431.png)

Exportされたメッシュは、小さなメッシュの集合体になっており、各メッシュ・セグメントは"-1,1,2"といった3つの整数が名称になっています。
この分割単位がGrid Cellで、割り振られた3つの整数が[GridIndex](https://developers.google.com/tango/apis/c/reconstruction/reference/group/mesh-building-types#group___mesh_building_types_1ga824e52561f9bddd39622abad95a5c183)、GridIndexを配列にしたのが[GridIndexArray](https://developers.google.com/tango/apis/c/reconstruction/reference/group/mesh-building-types#group___mesh_building_types_1gabeef224040421aee20cf0d6d9d50dfea)です。

GridIndexArrayのリファレンスには、Grid Cellは16×16×16の[voxel](https://ja.wikipedia.org/wiki/%E3%83%9C%E3%82%AF%E3%82%BB%E3%83%AB)と書いてあります。
実際に、平面（床）に相当するGrid Cellのメッシュを見てみると、確かに16分割されていました。

![スクリーンショット 2017-01-06 23.02.38.png](https://qiita-image-store.s3.amazonaws.com/0/153460/e604fb7e-f41b-df8f-afd6-f2d771a13265.png)


## ソースコードの解析
前編と同じく、対象は以下のファイルだけです。

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

### GridIndexArrayの更新


pointCloud更新時に実行される`OnTangoPointCloudMultithreadedAvailable()`で`_UpdateDepth()`が実行されます。

```Tango3DReconstruction.cs
private void _UpdateDepth(TangoPointCloudIntPtr pointCloud, Matrix4x4 depthPose)
{
    ...

    APIPointCloud apiCloud;
    apiCloud.numPoints = pointCloud.m_numPoints;
    apiCloud.points = pointCloud.m_points;
    apiCloud.timestamp = pointCloud.m_timestamp;

    APIPose apiDepthPose = APIPose.FromMatrix4x4(ref depthPose);

    if (!m_sendColorToUpdate)
    {
         // No need to wait for a color image, update reconstruction immediately.
         IntPtr rawUpdatedIndices;
         Status result = (Status)API.Tango3DR_update(
             m_context, ref apiCloud, ref apiDepthPose, IntPtr.Zero, IntPtr.Zero, out rawUpdatedIndices);

        ...

        _AddUpdatedIndices(rawUpdatedIndices);
        API.Tango3DR_GridIndexArray_destroy(rawUpdatedIndices);
    }
    else
    {
        lock (m_lockObject)
        {
            // We need both a color image and a depth cloud to update reconstruction.  Cache the depth cloud
            // because there are much less depth points than pixels.
            m_mostRecentDepth = apiCloud;
            m_mostRecentDepth.points = IntPtr.Zero;
            Marshal.Copy(pointCloud.m_points, m_mostRecentDepthPoints, 0, pointCloud.m_numPoints * 4);

            m_mostRecentDepthPose = apiDepthPose;
            m_mostRecentDepthIsValid = true;
        }
    }
}
```

colorCameraの色情報を使うか否かで処理が分かれています。

利用しない場合は、ここで[Tango3DR_update()](https://developers.google.com/tango/apis/c/reconstruction/reference/group/mesh-building-functions.html#group___mesh_building_functions_1ga87077592f1cf7688fedac88186149b96)を行います。
3D Reconstruction serverの情報が更新され、`rawUpdatedIndices`に更新されたGrid CellのGridIndexArrayが返ってくるので、`_AddUpdatedIndices()`に渡します。

色情報を利用する場合は、`m_mostRecentDepth`、`m_mostRecentDepthPose`をセットした上で、colorCameraの情報を待ちます。
更新されると、`OnTangoImageMultithreadedAvailable()`→`_UpdateColor()`→`Tango3DR_update()`→`_AddUpdatedIndices()`という流れになります。


```Tango3DReconstruction.cs
private void _AddUpdatedIndices(IntPtr rawUpdatedIndices)
{
    int numUpdatedIndices = Marshal.ReadInt32(rawUpdatedIndices, 0);
    IntPtr rawIndices = Marshal.ReadIntPtr(rawUpdatedIndices, 4);
    lock (m_lockObject)
    {
        if (m_updatedIndices.Count == 0)
        {
            // Update in fast mode, no duplicates are possible.
            for (int it = 0; it < numUpdatedIndices; ++it)
            {
                GridIndex index;
                index.x = Marshal.ReadInt32(rawIndices, (0 + (it * 3)) * 4);
                index.y = Marshal.ReadInt32(rawIndices, (1 + (it * 3)) * 4);
                index.z = Marshal.ReadInt32(rawIndices, (2 + (it * 3)) * 4);
                m_updatedIndices.Add(index);
            }
        }
        else
        {
            // Duplicates are possible, need to check while adding.
            for (int it = 0; it < numUpdatedIndices; ++it)
            {
                GridIndex index;
                index.x = Marshal.ReadInt32(rawIndices, (0 + (it * 3)) * 4);
                index.y = Marshal.ReadInt32(rawIndices, (1 + (it * 3)) * 4);
                index.z = Marshal.ReadInt32(rawIndices, (2 + (it * 3)) * 4);
                if (!m_updatedIndices.Contains(index))
                {
                    m_updatedIndices.Add(index);
                }
            }
        }
    }
}
```

更新されたGrid CellのGridInexを`m_updatedIndices`に追加しています。
`m_updatedIndices`が空であれば存在チェックをせずに追加し、データがあれば重複させないように存在チェックが入ってます。


### メッシュの作成
UI側の処理を追っていないので、少し飛んでしまいますが、GridIndexを指定して`ExtractMeshSegment()`を実行すると、そのGrid Cellのメッシュを書き出します。

```Tango3DReconstruction.cs
internal Status ExtractMeshSegment(
    GridIndex gridIndex, Vector3[] vertices, Vector3[] normals, Color32[] colors, int[] triangles,
    out int numVertices, out int numTriangles)
{
    APIMeshGCHandles pinnedHandles = APIMeshGCHandles.Alloc(vertices, normals, colors, triangles);
    APIMesh mesh = APIMesh.FromArrays(vertices, normals, colors, triangles);

    int result = API.Tango3DR_extractPreallocatedMeshSegment(m_context, ref gridIndex, ref mesh);
    numVertices = (int)mesh.numVertices;
    numTriangles = (int)mesh.numFaces;
    pinnedHandles.Free();

    return (Status)result;
}
```

ここでは、[Tango3DR_extractPreallocatedMeshSegment()](https://developers.google.com/tango/apis/c/reconstruction/reference/group/mesh-building-functions#group___mesh_building_functions_1ga028b53a9dc743482a95a5b9a594bb902)を使っていますが、[GridIndexArrayを渡して複数のGrid Cellを1つのメッシュとして返す関数](https://developers.google.com/tango/apis/c/reconstruction/reference/group/mesh-building-functions#group___mesh_building_functions_1ga3f72eb9057d213e1fa91539f2a82c4f1)、[すべてのGrid Cellのメッシュを返す関数](https://developers.google.com/tango/apis/c/reconstruction/reference/group/mesh-building-functions#group___mesh_building_functions_1ga8853b816ad5e94b8aba0ae5c779c199e)もあります。


## まとめ
後編では、Tango3DRがGridIndexArrayを用いて、メッシュを作成する流れをまとめました。

元々、屋内向けのマップ構築や屋内ナビゲーションへの活用に興味があってTangoの学習を始めたので、仕組みの理解という点では、かなり視界がクリアになった気がします。