e.blog

主にUnity/UE周りのことについてまとめていきます

LiDARセンサーのデータを使ってUnityで点群を描画する

概要

今回はiPadiPhoneに搭載されているLiDARセンサーから得られる深度情報を使って、立体的に点群位置を計算する方法について書いていきます。

具体的な動作は以下の動画をご覧ください。画面中央に表示されているのが、センサーから得られた情報を元に点の位置を計算しそれを可視化している点群です。背景はカメラが映している実際のシーンです。この計算方法についてまとめます。

GitHub

また今回の実装はGitHubにもアップしてあるので、実際に動くものが見たい方はそちらをご覧ください。

github.com



実装解説

今回の実装にあたり、TokyoYoshidaさんこちらのリポジトリの実装と、KeijiroさんRcam2のリポジトリを参考にさせていただきました。

実装概要

まずは大まかな流れから見ていきましょう。

利用するデータ

LiDARセンサー付きiPad or iPhoneから以下のデータを取得します。

  • 深度データ
  • RGBデータ
  • カメラデータ

深度データ

深度データとは、以下のような深度値を格納したテクスチャのことです。これによって対象までの距離を知ることが出来ます。(以下の画像はUnityのカメラで撮影したものですが、基本的な概念は変わりません)

f:id:edo_m18:20220127152145p:plain

RGBデータ

RGBデータはそのままカラーの映像テクスチャです。言い換えればカメラ画像です。こちらは説明不要ですね。

カメラデータ

最後のカメラデータは、以下で説明するカメラ自体のデータのことです。具体的にはカメラの焦点距離解像度などです。

データから点群の座標を計算

細かい説明に入る前に位置を計算しているコードを概観してみましょう。

uint2 gridPoint = id.xy * _GridPointsScale;
float2 uv = float2(id.xy) / _DepthResolution;

float depth = _DepthMap.SampleLevel(_LinearClamp, uv, 0).x * 1000.0;
float xrw = (float(gridPoint.x) - _IntrinsicsVector.z) * depth / _IntrinsicsVector.x;
float yrw = (float(gridPoint.y) - _IntrinsicsVector.w) * depth / _IntrinsicsVector.y;
float3 prw = float3(xrw, yrw, depth);

float s = 0.001;
float4 pos = mul(float4x4(
    s, 0, 0, 0,
    0, s, 0, 0,
    0, 0, s, 0,
    0, 0, 0, 1
    ), float4(prw, 1.0));
pos = mul(_TransformMatrix, float4(pos.xyz, 1.0));

最後に求めた pos の値が点群ひとつの位置です。これを必要数分並べたのが冒頭の点群表示というわけです。位置の計算だけに関して言えば20行に満たないコードです。順を追って見ていきましょう。

点群のX, Y位置を求める

uint2 gridPoint = id.xy * _GridPointsScale;

id.xy は起動されたスレッドの X, Y 位置、言い換えると(今回の場合は)テクスチャの X, Y 位置と考えてもらって差し支えありません。その位置に対して _GridPointScale を掛けています。これがなにかと言うと、撮影されたカメラの解像度に比べて深度マップの解像度が異なるためです。そのためカメラ解像度と深度マップの比率を元に位置を合わせる必要があるわけです。(逆に言えば、解像度が合っていればこの計算は不要ということになります)

コンピュートシェーダに値を送っているところの計算を見てみると以下のようになります。

_pointCloudParticle.GridPointsScale = new Vector4(
    (float)metadata.cameraResolution.x / (float)metadata.depthResolution.x,
    (float)metadata.cameraResolution.y / (float)metadata.depthResolution.y,
    0, 0);

カメラ自体の解像度を深度マップの解像度で割った値を設定しているのが分かるかと思います。

なぜこれをしているかというと、サイズが異なるということは、言い換えれば X, Y のスケールと Z のスケールとが合わなくなってしまうということです。結果、点群の位置がおかしくなってしまうわけです。そのためにスケールを合わせているわけなんですね。

なぜX, Yを求めるのか? Zのスケールが合わないって?

ここで、なぜ X, Y を求める必要があるのかですが、これはピンホールカメラの仕組みを理解すると分かります。以下のスライドがとても分かりやすく解説してくれているので詳細はそちらをご覧ください。

www.slideshare.net

ここでは、中の画像を引用させてもらいつつなぜ計算が必要かについて説明します。

f:id:edo_m18:20211028123813p:plain

原点となっているのがピンホールカメラで言うところのホール部分です。そして右側の青い三角形がリアルな被写体までの関係を表し、左側の緑の三角形がカメラの画像センサー素子までの関係を表しています。

言い換えると、青い三角形のリアル被写体を、緑の三角形側の画像に転写していることを示しているわけです。そしてセンサー側の素子がカメラ解像度と一致します。

深度マップの場合、保存されているのは深度情報です。上の図で言うと Z の値ということですね。

そして前述の通り、カメラの解像度と深度マップのサイズが合っていないため、そのまま利用してしまうと上図での X, Y, Z のうち X, Y の値が(スケールが)異なってしまうわけですね。そのための補正が前述の話になります。言い換えると、Z の値に変化がないのに X, Y の値だけ変化してしまっているわけですね。

カメラデータの中身を紐解く

ARFoundationから得られるカメラデータをもう少し詳しく見てみましょう。まず最初に、得られるデータは以下になります。

// ARCameraManagerのインスタンスから情報を取得する
ARCameraManager.TryGetIntrinsics(out XRCameraIntrinsics intrinsics);

得られるデータ( XRCameraIntrinsics )は以下です。

[StructLayout(LayoutKind.Sequential)]
public struct XRCameraIntrinsics : IEquatable<XRCameraIntrinsics>
{
    mmary>
    /// The focal length in pixels.
    /// </summary>
    /// <value>
    /// The focal length in pixels.
    /// </value>
    /// <remarks>
    /// The focal length is the distance between the camera's pinhole and the image plane.
    /// In a pinhole camera, the x and y values would be the same, but these can vary for
    /// real cameras.
    /// </remarks>
    public Vector2 focalLength => m_FocalLength;

    /// <summary>
    /// The principal point from the top-left corner of the image, expressed in pixels.
    /// </summary>
    /// <value>
    /// The principal point from the top-left corner of the image, expressed in pixels.
    /// </value>
    /// <remarks>
    /// The principal point is the point of intersection between the image plane and a line perpendicular to the
    /// image plane passing through the camera's pinhole.
    /// </remarks>
    public Vector2 principalPoint => m_PrincipalPoint;

    /// <summary>
    /// The dimensions of the image in pixels.
    /// </summary>
    public Vector2Int resolution => m_Resolution;

    // 後略
}

主に注目すべきは focalLengthprincipalPoint の2点です。 focalLength は日本語に訳すと「焦点距離」、すなわち前述の焦点距離を意味しています。そして他方、 principalPoint はコードにも説明が載っているので引用すると、

The principal point is the point of intersection between the image plane and a line perpendicular to the image plane passing through the camera's pinhole.

principalPoint は、イメージ平面、つまり先ほどの原点を含む平面と、ピンホールを通過する、画像平面と垂直な線との交点ということになります。

前述のスライドから引用させていただくと、次に示す画像が分かりやすいでしょう。

f:id:edo_m18:20220126232719p:plain f:id:edo_m18:20220127140406p:plain

つまり、画像中心と光学中心のオフセット、というわけです。

深度マップに保存されているデータから点群位置を復元

さて、上記までで画像から点群位置を復元するための情報が集まりました。具体的に言うと、画像位置の X, Y および保存されている Z の値を用いて、ピクセルの、現実空間での位置を求めるための道具が揃ったということです。

実は冒頭で掲載した復元のための計算は、ここで求めた行列の「逆行列を掛けて」いることと考えることができます。もう一度、該当部分を抜き出してみましょう。

uint2 gridPoint = id.xy * _GridPointsScale;
float2 uv = float2(id.xy) / _DepthResolution;

float depth = _DepthMap.SampleLevel(_LinearClamp, uv, 0).x * 1000.0;
float xrw = (float(gridPoint.x) - _IntrinsicsVector.z) * depth / _IntrinsicsVector.x;
float yrw = (float(gridPoint.y) - _IntrinsicsVector.w) * depth / _IntrinsicsVector.y;

uv は前述の x, y の値を求めていると言い換えることができます。そしてその下の計算部分がまさに「逆行列を掛けている」と考えられる部分です。ただ、実際に行列を利用しているわけではありません。なぜなら、そもそも逆行列の意味は、対象行列に対して掛けることで単位行列にするもの、言い換えれば「行列の作用をなかったことにするもの」と言えます。つまり、「逆変換」をしてあげればいいわけですね。幸い、今回の行列はシンプルです。まず普通に行列を適用すると以下のようになります。( Z は画像での λ

Zx = f * X
x = f * X / Z

x は上記での uv.x です。ここで求めたいのは X の値ですね。なので上記式を変形すると、

X = x * Z / f

上のプログラムと形が似ていることが分かるかと思います。補足しておくと _IntrinsicsVector.xy には focalLength が、 _IntrinsicsVector.zw には principalPoint が格納されています。プログラムで float(gridPoint.x) - _IntrinsicsVector.z) となっているのは引用させていただいたスライドで書かれている「オフセット」を考慮したものです。細かいことを抜きに考えれば上の式と同じ形になっていることが分かるかと思います。

実は点群の位置復元のための計算は以上となります。仕組みが分かってしまえば計算自体はまったくむずかしいところはありません。あとは求めた位置をパーティクルに与え、対象のパーティクルの色をRGB映像からサンプリングしてやれば無事、冒頭の動画のように点群を復元することができるというわけです。

まとめ

各データがなにを意味し、それをどう取り扱えばいいかが分かれば計算自体は複雑ではないことが分かったかと思います。結局のところ、RGBのデータもX, Yのデータも、そして深度も、すべてテクスチャになっていることが理解できれば、あとはそこからデータを取り出し簡単な計算をするだけで済みます。あとはここから様々なエフェクトを考えることもできるでしょう。ただ、これを通信で送ろうとすると少しだけ問題が出てきます。精度の問題です。テクスチャに保存されているデータの精度を落とさずに通信相手に届ける必要があるため、そこは少し工夫が必要となります。とはいえ、通信相手側での計算ロジックは変わらないので、通信部分さえなんとかなれば遠隔で点群を復元することも可能でしょう。

再掲となりますが、実際に動作するコードはGitHubにアップしてあります。

github.com