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のピクセルポイントを取得して３次元座標に変換する
    }

}

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;
    }

}