この記事は株式会社ソニックムーブ Advent Calendar 2017の2日目です。
ソニックムーブからリリースされている measuAR というアプリがありまして、ARKitを使って物の長さを測ることができます。このアプリの以前のバージョンには任意の方向の平面を検出して測定をサポートする機能がありました（現在は、より測りやすくなっているのでその機能はなくなっています）。
ARKitの機能としてできる平面検出は水平面だけですが、それだけだとできることが限られてしまいます。そこで任意の方向の平面検出を「自分はこうやってみた」というのを書きますが、もっといいやり方があったら教えてください。
なお「ARKitで」というタイトルですが、考え方はARKitに限らず適用できると思います。あと、Unity ARKit Pluginでやってます。
サンプルプロジェクトはgithubの次の場所にあります。
実行するとこんな感じになります。
ARKitで任意の方向の平面を検出する https://t.co/tbNOYvOLQf pic.twitter.com/tDdk2hQAuE
— レッツ・ラクサン☆ショータイム (@rakusan) 2017年12月1日
#三行で
- 点群を取得する
- RANSACを使ってそれらしい平面を見つける
- 最小二乗法で点群にもっともフィットする平面を計算する
#点群を取得する
これは特に難しいことはないですね。Unity ARKit Pluginのサンプルで点群を描画するやつがあるので、それを参考にすればすぐできます。今回のコードは次のようになっています。
private Vector3[] pointCloudData;
private bool frameUpdated;
void Start () {
UnityARSessionNativeInterface.ARFrameUpdatedEvent += ARFrameUpdated;
frameUpdated = false;
}
private void ARFrameUpdated(UnityARCamera camera) {
pointCloudData = camera.pointCloudData;
frameUpdated = true;
}
void Update () {
if (frameUpdated) {
Ransac ();
frameUpdated = false;
}
}
ARFrameUpdated が呼ばれたら pointCloudData に点群を格納しておいて、Update のタイミングで後述する処理をしています。
#RANSACを使ってそれらしい平面を見つける
RANSACという手法を使って点群からそれらしい平面を見つけます。RANSACはrandom sample consensusの略で、次のようなものです。
- 仮のモデル（今回の場合は平面の式）を決定するのに必要な数のデータをデータ群の中からランダムにサンプリングする
- 何らかの基準を設け、仮のモデルに合うデータがデータ群の中にいくつあるかを数える
- ここまでを繰り返す
- 2.で数えた数が一番多かった仮のモデルを採用する
今回は平面の式が必要で、平面の式は3点の座標があれば決定します。ですので、1.でサンプリングするデータの数は3になります。実装は次のようになります。
private void Ransac() {
if (pointCloudData.Length < 10) {
return;
}
int kmax = 200;
float threshold = 0.02f;
int inlierMax = 0;
Vector3 tmpPlanePoint = Vector3.zero;
Vector3 tmpPlaneNormal = Vector3.zero;
for (int k = 0; k < kmax; ++k) {
int i0, i1, i2;
Next3Randoms (pointCloudData.Length, out i0, out i1, out i2);
Vector3 p0 = pointCloudData [i0];
Vector3 p1 = pointCloudData [i1];
Vector3 p2 = pointCloudData [i2];
Vector3 v1 = p1 - p0;
Vector3 v2 = p2 - p0;
Vector3 normal = Vector3.Cross (v1, v2).normalized;
// 平面の式
// Dot(normal, p-p0) = 0, ここでpは平面上の任意の点
//
// 平面と点qの距離（normalの長さは1とする）
// distance = | Dot(normal, q-p0) |
// = | Dot(normal, q) - Dot(normal, p0) |
float dot_normal_p0 = Vector3.Dot(normal, p0);
int inlierCount = pointCloudData.Count (q => Mathf.Abs (Vector3.Dot (normal, q) - dot_normal_p0) < threshold);
if (inlierCount > inlierMax) {
inlierMax = inlierCount;
tmpPlanePoint = p0;
tmpPlaneNormal = normal;
}
}
if (inlierMax >= pointCloudData.Length / 4) {
// 採用した仮の平面にうまく合っている点だけ取り出す
float dot_normal_p0 = Vector3.Dot(tmpPlaneNormal, tmpPlanePoint);
IEnumerable<Vector3> inliers = pointCloudData.Where (q => Mathf.Abs (Vector3.Dot (tmpPlaneNormal, q) - dot_normal_p0) < threshold);
LeastSquaresMethod (tmpPlanePoint, tmpPlaneNormal, inliers);
}
}
private void Next3Randoms(int maxValue, out int i1, out int i2, out int i3) {
i1 = random.Next (maxValue);
do {
i2 = random.Next (maxValue);
} while (i2 == i1);
do {
i3 = random.Next (maxValue);
} while (i3 == i1 || i3 == i2);
}
Ransac関数の冒頭で、点群のデータが一定数以上無い場合は処理を中止しています。この数は適当に調整してください。
ループの先頭で点群の中からランダムに3つの点 p0, p1, p2 を選んでいますが、同じ点を重複して選んでしまわないようにしています。そして、選んだ3点で2つのベクトル v1, v2 を作って外積を計算します。これが仮の平面の法線です。このときの平面の式は次のようになります。
Dot(normal, p-p0) = 0
次のCountのLINQ式では、点群のすべての点に対して今求めた平面への距離を計算しています。平面と点の距離は次の式で計算できます。
distance = | Dot(normal, q-p0) |
この距離が threshold 未満の場合は仮の平面にうまく合っている点とみなして inlierCount にカウントします。threshold は適宜決めてください。ここでは0.02(2cm)としています。
そして inlierCount が前回までの最大値 inlierMax よりも大きければ inlierMax を更新し、採用する仮の平面の現在の候補として tmpPlanePoint, tmpPlaneNormal に記録しておきます。
ループを抜けた後、inlierMax が点群のデータ数の1/4以上あれば仮の平面を採用します。この1/4という値は適宜調整してください。仮の平面を採用したら、その平面にうまく合っている点だけ取り出して、最小二乗法を行う関数 LeastSquaresMethod に渡しています。
最小二乗法で点群にもっともフィットする平面を得る
RANSACでそれらしい平面は見つかりますが、さらに最小二乗法を使って点群にもっともフィットする平面を計算することができます。
##平面の最小二乗法
計算を簡単にするため、求めたい平面（以下、平面P）がxy平面に ほぼ 水平である場合を考えます。そして、平面Pと点群の各点の z方向の 距離の二乗和が最小になるように平面Pを決定します。
まず、平面Pの式を次のように表します。これは、xy平面上(x, y)の位置での平面Pのz座標、という形になっています。
z = ax + by + c\tag{1}
ここで、点群の中の点 $(x_i, y_i, z_i)$ と平面Pのz方向の距離は
d_i = (ax_i + by_i + c) - z_i
その二乗和は
\sum_{i=1}^nd_i^2 = \sum_{i=1}^n(ax_i + by_i + c - z_i)^2
です。これが最小になる a, b, c を求めれば平面Pが得られます。この式が最小となるとき a, b, c それぞれに対する偏微分はゼロになります。
\sum2x_i(ax_i + by_i + c - z_i) = 0 \\
\sum2y_i(ax_i + by_i + c - z_i) = 0 \\
\sum2(ax_i + by_i + c - z_i) = 0\tag{2}
これを次のように行列を使って表します。
\begin{pmatrix}
\sum{x_i^2} & \sum{x_iy_i} & \sum{x_i} & -\sum{x_iz_i} \\
\sum{x_iy_i} & \sum{y_i^2} & \sum{y_i} & -\sum{y_iz_i} \\
\sum{x_i} & \sum{y_i} & n & -\sum{z_i} \\
0 & 0 & 0 & 1
\end{pmatrix}
\begin{pmatrix}
a \\
b \\
c \\
1
\end{pmatrix}
=
\begin{pmatrix}
0 \\
0 \\
0 \\
1
\end{pmatrix}
左の行列をMと置き、左側からMの逆行列をかけると、
\begin{pmatrix}
a \\
b \\
c \\
1
\end{pmatrix}
=M^{-1}
\begin{pmatrix}
0 \\
0 \\
0 \\
1
\end{pmatrix}
したがって、Mの逆行列の一番右の列を取り出すと a, b, c が得られます。
##点群の分布する範囲に平面を制限する
最小二乗法で求めた平面Pは、それだけでは無限に広がる平面です。点群の分布する範囲に平面を制限するには、平面P上の適切な位置と広さを決める必要があります。
(2)の3つ目の式に注目し、これを次のように書き換えます。
a\sum{x_i}+b\sum{y_i}+cn-\sum{z_i}=0
さらに次のように書き換えます。
\frac{\sum{z_i}}{n}=a\frac{\sum{x_i}}{n}+b\frac{\sum{y_i}}{n}+c
これは、(1)の式に点群の重心を代入したものとなり、最小二乗法で求めた平面Pは点群の重心を通ることがわかります。点群の重心は点群の分布を表すのに都合がよいので、点群の重心を中心に平面の範囲を制限することにします。
広さの決定は用途によって適切な方法が異なると思いますが、サンプルプロジェクトでは点群の重心から各点への距離の二乗和の平方根（重心からの距離を偏差としたときの標準偏差）によって広さを決めています。
##座標変換
平面の最小二乗法の説明において「計算を簡単にするため、求めたい平面（以下、平面P）がxy平面にほぼ水平である場合を考えます」と書きました。そのため、点群の分布がこの条件に当てはまるように座標系を変換する必要があります。これにはRANSACで得られた仮の平面の法線を使います。クォータニオンを用いれば次のように簡単に座標変換できます。
Quaternion rotation = Quaternion.FromToRotation (tmpPlaneNormal, Vector3.forward);
foreach (Vector3 q in pointCloudData) {
Vector3 q2 = rotation * q;
}
この座標変換をした点群で平面の最小二乗法を行い、得られた結果を逆変換して元の座標系に戻します。
##最小二乗法の実装
private void LeastSquaresMethod(Vector3 tmpPlanePoint, Vector3 tmpPlaneNormal, IEnumerable<Vector3> inliers) {
// 座標変換に使うクォータニオン
Quaternion rotation = Quaternion.FromToRotation (tmpPlaneNormal, Vector3.forward);
// 最小二乗法を解く行列の要素
float A = 0, B = 0, C = 0, D = 0, E = 0, F = 0, G = 0, H = 0;
foreach (Vector3 q in inliers) {
Vector3 q2 = rotation * q;
A += q2.x * q2.x; // Σ(xi^2)
B += q2.x * q2.y; // Σ(xi*yi)
C += q2.x; // Σ(xi)
D += q2.x * q2.z; // Σ(xi*zi)
E += q2.y * q2.y; // Σ(yi^2)
F += q2.y; // Σ(yi)
G += q2.y * q2.z; // Σ(yi*zi)
H += q2.z;
}
// 行列と逆行列を作り、逆行列の一番右の列を取り出す
int n = inliers.Count ();
Matrix4x4 matrix = new Matrix4x4 (
new Vector4 ( A, B, C, 0),
new Vector4 ( B, E, F, 0),
new Vector4 ( C, F, n, 0),
new Vector4 (-D, -G, -H, 1));
Vector3 solution = matrix.inverse.GetColumn(3);
float a = solution.x;
float b = solution.y;
float c = solution.z;
// 重心と平面の法線を作り、元の座標系に変換する
Quaternion inverseRotation = Quaternion.Inverse (rotation);
Vector3 planePoint = inverseRotation * new Vector3 (C/n, F/n, H/n);
Vector3 planeNormal = inverseRotation * new Vector3(a, b, -1);
// 標準偏差で平面の広さを決める
float variance = inliers.Average (q => Vector3.SqrMagnitude (q - planePoint));
float sd = Mathf.Sqrt (variance);
// 平面のプレハブをインスタンス化
GameObject plane = Instantiate (planePrefab, planePoint, Quaternion.LookRotation (planeNormal));
plane.transform.localScale = new Vector3 (sd, sd, 1);
Destroy (plane, 0.25f);
}
#終わりに
RANSACを使いそれらしい平面を見つけた上でさらに最小二乗法を使い、点群にもっともフィットする平面を計算しました。現在のフレーム内だけからざっくり平面らしきものがわかればよい用途にはこれでも実用になると思います。しかし、ARKitの水平面検出のようにカメラの動きにともなって認識済みの平面を徐々に拡張していくようなことまではしていません。そこまでできるとさらに活用の幅が広がるので、今後試してみたいと思います。
#参考文献
http://www.sol-j.co.jp/mailmag/d-0001.html
http://oz-log.blogspot.jp/2010/10/blog-post.html