9
5

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

Tangoの仕組みを理解する(3DR前編)

Last updated at Posted at 2017-01-06

前回に引き続き、Tango Unity SDKのExamplesを読み解いていきます。

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

スクリーンショット 2017-01-06 20.42.10.png

Tango3DR

Tangoからのデータを元に3Dメッシュを構築するライブラリです。Tango3DRのRは、"Reconstruction"の略で復元・再構成といった意味を持ちます。
Unity Tango SDKでは、Tango 3D Reconstruction Library C APIをTangoWrappers経由で使用しています。

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

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

前編では、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の座標は右手座標系なので、2番目と3番目の要素を入れ替えて、左手座標系にしています。

設定を終えたら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のドキュメントだけでなく、コンピュータビジョン分野の用語や理論を学ぶと、理解しやすくなると思います。

9
5
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
9
5

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?