概要
この記事は 3D Sensor Advent Calendar 2019 の9日目の記事になります。
OptiTrack等のモーションキャプチャシステムが行っている赤外線カメラでマーカーをトラッキングして位置を追跡する手法をRealsenseの赤外線カメラと再帰性反射テープを使って作ってみたので紹介します。
出来る事
再帰性反射テープを張り付けた物体の位置をトラッキング出来ます。90FPSでトラッキング出来るので素早い動きもトラッキング出来ます。これらの処理を全てUnity上で行います。
以下のような事ができます。
#RealSense
— unagi (@UnagiHuman) March 1, 2019
Realsense二台で再帰性反射材使ったモーショントラッキング。遅延も気にならないし、90FPSとれるので動きが早くてもトラッキング出来る。 pic.twitter.com/ZifZkVvRIW
必要機材
合計5~6万ほどで機材一式が揃います。
- RealsenseD435
- IR投光器(850nm)
- 再帰性反射テープ
- チャンバラキングネオ（反射テープを張る対象物）
- IRハイパスフィルタ(840nm)
- Realsense用スタンド
- Realsenseをスタンドに固定する為の雲台
- USB3.0延長ケーブル
検証に利用したソフトウェア、ライブラリ
- Unity
- OpenCV for Unity
- libRealsense
トラッキング原理
RealsenseにはIRカメラが2つ付いているので、立体視による３次元座標の計算が可能です。
この計算を、赤外線投光器から照射した赤外線を再帰性反射テープ反射した反射光に対して行います。
intel公式の以下のページにステレオ画像からデプス値を計算する方法が載っていますので、これを参考にしました。
https://github.com/IntelRealSense/librealsense/blob/master/doc/depth-from-stereo.md
機材構成の詳細
値段が安くて、トラッキングを実現するのに手間がかからないものを選びました。
-
RealsenseD435
- 自分が調べた中で赤外線カメラとしてのコスパが最強です。最大90FPSまで速度でるし、２眼なのでRealsense単体で立体視できます。
- 一応D435,D415双方とも使えますが、D435の方がカメラに余分なIRフィルターがついて無いので加工をする必要がないのと、グローバルシャッター方式なので高速に動いてるものでも問題なく撮影できるのでD435の方がお勧めです。
-
IR投光器(850nm)
- 赤外線ダイオードを利用して自作するのが一番安いですが、超面倒なので在りものを購入しました。それでも一台10800円なのでコスパは高いと思います。
-
IRハイパスフィルタ(840nm)
- ハイパスフィルタとは指定波長以上の光をカットするという意味です。これをRealsenseのIRカメラに張り付けて再帰性反射板から反射した赤外線のみを撮影します。IR投光器が850nmなのにフィルタが840nmなのは、両者の波長が一致して尚且つ値段が安いものが探しても無かったので、一番近いものを選んでいます。
-
再帰性反射テープ
- トラッキングするマーカーとして利用するものです。加工しやすく、どこにでも貼り付けられるテープ式のが便利です。
IR画像取得⇒３次元座標変換までの処理フロー
LibRealsenseのUnityWrapperで取得した左右のIRカメラからのIR画像からOpenCVでマーカーの座標を取得します。OpenCVはOpenCV for Unityを利用します。
1. IR画像取得
UnityWrapperのTexturesDepthAndInfraredシーンが参考になります。基本はRsDeviceのProfilesにInfraredの設定をして、RsStreamTextureRendererでIR画像のテクスチャーを取得します。ただし、必要なIR画像は左右のカメラ分ありますので、RsDeviceで左右のカメラからIR画像を取得する為の設定をします。
また、設定パラメータは取得FrameRateはMaxの90に設定してあり、後述のOpenCvForUnityのBlob検出の処理で処理落ちが発生しないように解像度は低めに設定しています。
RsDeviceからTextureを受け取るのはRsStreamTextureRendererになります。ちなみにStreamIndexが1の場合がReanselseの右のIRカメラ、2が左のIRカメラとなります。
2. OpenCVでBlob detect
OpenCVforUnityのSimpleBlobExampleシーンを参考にしました。下記に、BlobDetectでBlobのピクセル座標を取得する所までのサンプルを載せます。詳細はコメントに書きました。
using UnityEngine;
using OpenCVForUnity;
public class BlobDetectorSample : MonoBehaviour
{
private Mat _imgMat = null;
private FeatureDetector _blobDetector = null;
private MatOfKeyPoint _keypoints = null;
private string _blobparams_yml_filepath;
private Texture2D _infraredTexture;
void Start()
{
//OpenCV for UnityのUtilsクラス。StreamingAssetsを参照する
//blobparams.ymlはFeatureDetector.SIMPLEBLOBのパラメータ
_blobparams_yml_filepath = Utils.getFilePath("blobparams.yml");
}
/// <summary>
/// RsStreamTextureRendererにバインドする
/// </summary>
/// <param name="InfraredTexture"></param>
public void BindInfraredTexture(Texture2D InfraredTexture)
{
_infraredTexture = InfraredTexture;
}
private void Update()
{
if (_imgMat == null)
{
_keypoints = new MatOfKeyPoint();
//IRテクスチャ用のMat初期化
_imgMat = new Mat(_infraredTexture.height, _infraredTexture.width, CvType.CV_8UC1);
//OpenCV For UnityのFeatureDetector
_blobDetector = FeatureDetector.create(FeatureDetector.SIMPLEBLOB);
//_blobparams_yml_filepathで設定したパラメータでblobdetectする為の設定
_blobDetector.read(_blobparams_yml_filepath);
}
Utils.fastTexture2DToMat(_infraredTexture, _imgMat);
_blobDetector.detect(_imgMat, _keypoints);
var keys = _keypoints.toArray();
//後は取得したkeysからblobのピクセルポイントを取得して３次元座標に変換する
}
}
3. BlocDetectで取得したマーカーのピクセル座標を３次元座標に変換
-
左右のIR画像から得られたBlobからDepth計算
下記記事を参照にしてます。
https://github.com/IntelRealSense/librealsense/blob/master/doc/depth-from-stereo.md
baseLineの値やfxの値はlibrealsenceのStreamProfileから得られます。 -
左右の対応するBlobのpixel座標から３次元座標を計算
librealsenseのコードをほぼそのままC#に移植してます。（この部分はC# wrapperには無かった。。）
https://github.com/IntelRealSense/librealsense/blob/5e73f7bb906a3cbec8ae43e888f182cc56c18692/include/librealsense2/rsutil.h#L46
下記にコードを書きました。処理の流れはコメントを参照してください。
using UnityEngine;
using Intel.RealSense;
using UnityEngine.Assertions;
public class Blob3DSample : MonoBehaviour
{
/// <summary>
/// 右のIRカメラのStreamProfile
/// </summary>
public StreamProfile rightProfile;
/// <summary>
/// 左のIRカメラのStreamProfile
/// </summary>
public StreamProfile leftProfile;
/// <summary>
/// 右カメラのIR画像から算出したBlobの中心ピクセル位置。先ほどのBlobDetectSampleで計算した値をセット
/// </summary>
public Vector2 rightCameraBlobCenter;
/// <summary>
/// 左カメラのIR画像から算出したBlobの中心ピクセル位置。先ほどのBlobDetectSampleで計算した値をセット
/// </summary>
public Vector2 leftCameraBlobCenter;
/// <summary>
/// RsDevice参照
/// </summary>
[SerializeField] RsFrameProvider _source = null;
/// <summary>
/// realsenseのカメラ内部パラメタ
/// </summary>
private Intrinsics _intrinsics;
/// <summary>
/// realsenseのカメラ外部パラメタ(baseLine参照用)
/// </summary>
private Extrinsics _extrinsics;
// Start is called before the first frame update
void Start()
{
_extrinsics = this.rightProfile.GetExtrinsicsTo(leftProfile);
using (var profile = _source.ActiveProfile.GetStream<VideoStreamProfile>(Stream.Infrared, 1))
{
_intrinsics = profile.GetIntrinsics();
}
}
// Update is called once per frame
void Update()
{
var point = Vector3.positiveInfinity;
CalculatePosition(ref point, _intrinsics, rightCameraBlobCenter, leftCameraBlobCenter, Mathf.Abs(_extrinsics.translation[0]));
//後は得られた３次元座標をよしなに利用する
}
/// <summary>
/// 左右のIR画像の対応点から３次元の座標を計算
/// </summary>
/// <param name="point"></param>
/// <param name="intrin"></param>
/// <param name="pixelR"></param>
/// <param name="pixelL"></param>
/// <param name="baseline"></param>
void CalculatePosition(ref Vector3 point, in Intrinsics intrin, in Vector2 pixelR, in Vector2 pixelL, float baseline)
{
var depth = CalculateDepth(intrin.fx, pixelR.x, pixelL.x, baseline);
Rs2_Deproject_Pixel_to_Point(ref point, in intrin, in pixelR, depth * 0.001f);
}
/// <summary>
/// ステレオカメラからdepthを計算
/// </summary>
/// <param name="fx"></param>
/// <param name="rx"></param>
/// <param name="lx"></param>
/// <param name="baseLine"></param>
/// <param name="depthUnits"></param>
/// <returns></returns>
float CalculateDepth(float fx, float rx, float lx, float baseLine, float depthUnits = 0.001f)
{
return fx * baseLine / (depthUnits * Mathf.Abs(rx - lx));
}
/// <summary>
/// pixel座標とdepth値から３次元座標を計算
/// </summary>
/// <param name="point"></param>
/// <param name="intrin"></param>
/// <param name="pixel"></param>
/// <param name="depth"></param>
public void Rs2_Deproject_Pixel_to_Point(ref Vector3 point, in Intrinsics intrin, in Vector2 pixel, float depth)
{
Assert.IsTrue(intrin.model != Distortion.ModifiedBrownConrady);
Assert.IsTrue(intrin.model != Distortion.Ftheta);
float x = (pixel.x - intrin.ppx) / intrin.fx;
float y = (pixel.y - intrin.ppy) / intrin.fy;
if (intrin.model == Distortion.ModifiedBrownConrady)
{
float r2 = x * x + y * y;
float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
x = ux;
y = uy;
}
point.x = depth * x;
point.y = depth * y;
point.z = depth;
}
}
トラッキング位置ブレを軽減する方法
以上の方法で取得した座標データは調整を加えないとノイズが酷く使い物になりませんが以下の方法で対策が可能です。
-
球形状のマーカーを利用する
- これが一番無難。球以外の形状だとカメラとの位置関係によってカメラからみるマーカーの形状が変化してしまいますが、球なら全部円形状として見え、中心位置が正確に求められるので精度が増します。ボールに赤外線反射テープを巻きつけて自作するのが一番安いです。
-
ローパスフィルタ
過去フレームの値を利用してローパスフィルタをかけ細かい位置ブレをフィルターします。
以下のようなフィルター関数を作って利用しました。
public class LowPassFilter
{
public Vector3 filterdValue
{
get;
private set;
}
/// <summary>
/// ローパスフィルター係数
/// </summary>
private float lowpassK = 0.7f;
private Vector3 _prePosition;
public LowPassFilter(float lowpassK)
{
this.lowpassK = lowpassK;
this._prePosition = Vector3.zero;
}
public void FilterData(Vector3 position)
{
this.filterdValue = Filter(position, this._prePosition);
this._prePosition = position;
}
Vector3 Filter(Vector3 position, Vector3 prePosition)
{
return (1f - this.lowpassK) * prePosition + this.lowpassK * position;
}
}
Realsneseの精度の限界について
- 精度
- ２眼カメラの３次元位置の計算精度はカメラ同士の距離が離れているほど高くなりますが、Realsensは5cmほどしか離れておらず、Realsenseとマーカーの距離がだいたい2mを超えるとブレ始めます。
- なので、もっと精度を求めるのであれば単純に２眼の距離を物理的に離すのが一番簡単です。
- その場合は２眼の距離が離れている別製品のIRステレオカメラを購入するのがよいかも。
まとめ
Realsenseでモーショントラッキングする仕組みをDIYしました。
明日12/10(火)はxbarusuiさんで「初心者が Unity で始める Azure Kinect」です。お楽しみに！