Qiita Teams that are logged in
You are not logged in to any team

Log in to Qiita Team
Community
OrganizationEventAdvent CalendarQiitadon (β)
Service
Qiita JobsQiita ZineQiita Blog
22
Help us understand the problem. What are the problem?

More than 1 year has passed since last update.

posted at

Realsenseの赤外線カメラを利用して簡易モーショントラッキングを行う

概要

この記事は 3D Sensor Advent Calendar 2019 の9日目の記事になります。
OptiTrack等のモーションキャプチャシステムが行っている赤外線カメラでマーカーをトラッキングして位置を追跡する手法をRealsenseの赤外線カメラと再帰性反射テープを使って作ってみたので紹介します。

出来る事

再帰性反射テープを張り付けた物体の位置をトラッキング出来ます。90FPSでトラッキング出来るので素早い動きもトラッキング出来ます。これらの処理を全てUnity上で行います。

以下のような事ができます。

必要機材

合計5~6万ほどで機材一式が揃います。

検証に利用したソフトウェア、ライブラリ

  • Unity
  • OpenCV for Unity
  • libRealsense

トラッキング原理

RealsenseにはIRカメラが2つ付いているので、立体視による3次元座標の計算が可能です。
この計算を、赤外線投光器から照射した赤外線を再帰性反射テープ反射した反射光に対して行います。
intel公式の以下のページにステレオ画像からデプス値を計算する方法が載っていますので、これを参考にしました。
https://github.com/IntelRealSense/librealsense/blob/master/doc/depth-from-stereo.md

機材構成の詳細

値段が安くて、トラッキングを実現するのに手間がかからないものを選びました。

  • RealsenseD435

    • 自分が調べた中で赤外線カメラとしてのコスパが最強です。最大90FPSまで速度でるし、2眼なのでRealsense単体で立体視できます。
    • 一応D435,D415双方とも使えますが、D435の方がカメラに余分なIRフィルターがついて無いので加工をする必要がないのと、グローバルシャッター方式なので高速に動いてるものでも問題なく撮影できるのでD435の方がお勧めです。
  • IR投光器(850nm)

    • 赤外線ダイオードを利用して自作するのが一番安いですが、超面倒なので在りものを購入しました。それでも一台10800円なのでコスパは高いと思います。
  • IRハイパスフィルタ(840nm)

    • ハイパスフィルタとは指定波長以上の光をカットするという意味です。これをRealsenseのIRカメラに張り付けて再帰性反射板から反射した赤外線のみを撮影します。IR投光器が850nmなのにフィルタが840nmなのは、両者の波長が一致して尚且つ値段が安いものが探しても無かったので、一番近いものを選んでいます。
  • 再帰性反射テープ

    • トラッキングするマーカーとして利用するものです。加工しやすく、どこにでも貼り付けられるテープ式のが便利です。

IR画像取得⇒3次元座標変換までの処理フロー

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.PNG

RsDeviceからTextureを受け取るのはRsStreamTextureRendererになります。ちなみにStreamIndexが1の場合がReanselseの右のIRカメラ、2が左のIRカメラとなります。
rsStreamRender.PNG

2. OpenCVでBlob detect

OpenCVforUnityのSimpleBlobExampleシーンを参考にしました。下記に、BlobDetectでBlobのピクセル座標を取得する所までのサンプルを載せます。詳細はコメントに書きました。

BlobDetectorsample.cs
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次元座標に変換する
    }

}

3. BlocDetectで取得したマーカーのピクセル座標を3次元座標に変換

下記にコードを書きました。処理の流れはコメントを参照してください。

Blob3DSample.cs
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]));

        //後は得られた3次元座標をよしなに利用する
    }


    /// <summary>
    /// 左右のIR画像の対応点から3次元の座標を計算
    /// </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値から3次元座標を計算
    /// </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;
    }

}

トラッキング位置ブレを軽減する方法

以上の方法で取得した座標データは調整を加えないとノイズが酷く使い物になりませんが以下の方法で対策が可能です。

  • 球形状のマーカーを利用する

    • これが一番無難。球以外の形状だとカメラとの位置関係によってカメラからみるマーカーの形状が変化してしまいますが、球なら全部円形状として見え、中心位置が正確に求められるので精度が増します。ボールに赤外線反射テープを巻きつけて自作するのが一番安いです。
  • ローパスフィルタ
    過去フレームの値を利用してローパスフィルタをかけ細かい位置ブレをフィルターします。
    以下のようなフィルター関数を作って利用しました。

LowPassFilter.cs
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の精度の限界について

  • 精度
    • 2眼カメラの3次元位置の計算精度はカメラ同士の距離が離れているほど高くなりますが、Realsensは5cmほどしか離れておらず、Realsenseとマーカーの距離がだいたい2mを超えるとブレ始めます。
    • なので、もっと精度を求めるのであれば単純に2眼の距離を物理的に離すのが一番簡単です。

まとめ

Realsenseでモーショントラッキングする仕組みをDIYしました。
明日12/10(火)はxbarusuiさんで「初心者が Unity で始める Azure Kinect」です。お楽しみに!

Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
22
Help us understand the problem. What are the problem?