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を作成する関数です。

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()でconfigを作成して、パラメータをセットしています。各パラメータの説明はこちらにあります。
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)の設定です。
カメラパラメータについては公式にも説明があるのですが、今ひとつ理解できず、色々とググった結果、OpenCVのドキュメントとこちらのSlideShareが役立ちました。

外部パラメータはカメラの位置・姿勢を示す行列で、PoseProvider.GetPoseAtTime()でposeDataとして求められるようです。

ここで計算しているのは、デバイス上の座標系変換のためのパラメータです。IMUを基点に計算しているのは、IMUがデバイスの位置や傾きを測定するセンサだからですかね。IMUについてはこのはてブが詳しいです。

内部パラメータは、VideoOverlayProvider.GetIntrinsics()から取得でき、Tango3DR_setDepthCalibration()でそのまま渡しています。

設定したパラメータは、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を基点としたデバイスの移動・回転になります。いずれ消えちゃうかもしれませんが、古いリファレンスの図がわかりやすいと思います。
AreaDescriptionについては、AreaLearningを扱う際に改めて調査することにします。

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

_UpdateDepth()以降がpointCloudからメッシュを作成する処理になりますが、そちらは後編とします。

まとめ

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

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