コード例 #1
0
ファイル: Cv_K.cs プロジェクト: sanglin307/UnityOpenCV
        /// <summary>
        /// モデル状態を修正する. 修正された状態を kalman->state_post に保存し,これを出力として返す.
        /// </summary>
        /// <param name="kalman">更新されるカルマンフィルタ状態</param>
        /// <param name="measurement">観測ベクトルを含むCvMat</param>
        /// <returns>修正された状態を kalman->state_post に保存し,これを出力として返す.</returns>
#else
        /// <summary>
        /// Adjusts model state
        /// </summary>
        /// <param name="kalman">Kalman filter</param>
        /// <param name="measurement">CvMat containing the measurement vector. </param>
        /// <returns>The function stores adjusted state at kalman->state_post and returns it on output.</returns>
#endif
        public static CvMat KalmanCorrect(CvKalman kalman, CvMat measurement)
        {
            if (kalman == null)
                throw new ArgumentNullException("kalman");
            if (measurement == null)
                throw new ArgumentNullException("measurement");
            IntPtr result = CvInvoke.cvKalmanCorrect(kalman.CvPtr, measurement.CvPtr);
            return new CvMat(result, false);
        }
コード例 #2
0
ファイル: Cv_K.cs プロジェクト: 0sv/opencvsharp
        /// <summary>
        /// 次のモデル状態を推定する
        /// </summary>
        /// <param name="kalman">カルマンフィルタ状態</param>
        /// <param name="control">コントロールベクトル (uk).外部コントロールが存在しない場合(control_params=0)に限り,null である.</param>
        /// <returns></returns>
#else
        /// <summary>
        /// Estimates subsequent model state
        /// </summary>
        /// <param name="kalman">Kalman filter state. </param>
        /// <param name="control">Control vector (uk), should be null iff there is no external control (control_params=0).</param>
        /// <returns>The function returns the estimated state. </returns>
#endif
        public static CvMat KalmanPredict(CvKalman kalman, CvMat control)
        {
            if (kalman == null)
                throw new ArgumentNullException("kalman");
            IntPtr controlPtr = (control == null) ? IntPtr.Zero : control.CvPtr;
            IntPtr result = NativeMethods.cvKalmanPredict(kalman.CvPtr, controlPtr);
            if (result == IntPtr.Zero)
                return null;
            else
                return new CvMat(result, false);
        }
コード例 #3
0
        /// <summary>
        /// モデル状態を修正する. 修正された状態を kalman->state_post に保存し,これを出力として返す.
        /// </summary>
        /// <param name="kalman">更新されるカルマンフィルタ状態</param>
        /// <param name="measurement">観測ベクトルを含むCvMat</param>
        /// <returns>修正された状態を kalman->state_post に保存し,これを出力として返す.</returns>
#else
        /// <summary>
        /// Adjusts model state
        /// </summary>
        /// <param name="kalman">Kalman filter</param>
        /// <param name="measurement">CvMat containing the measurement vector. </param>
        /// <returns>The function stores adjusted state at kalman->state_post and returns it on output.</returns>
#endif
        public static CvMat KalmanCorrect(CvKalman kalman, CvMat measurement)
        {
            if (kalman == null)
            {
                throw new ArgumentNullException("kalman");
            }
            if (measurement == null)
            {
                throw new ArgumentNullException("measurement");
            }
            IntPtr result = NativeMethods.cvKalmanCorrect(kalman.CvPtr, measurement.CvPtr);

            return(new CvMat(result, false));
        }
コード例 #4
0
        /// <summary>
        /// 次のモデル状態を推定する
        /// </summary>
        /// <param name="kalman">カルマンフィルタ状態</param>
        /// <param name="control">コントロールベクトル (uk).外部コントロールが存在しない場合(control_params=0)に限り,null である.</param>
        /// <returns></returns>
#else
        /// <summary>
        /// Estimates subsequent model state
        /// </summary>
        /// <param name="kalman">Kalman filter state. </param>
        /// <param name="control">Control vector (uk), should be null iff there is no external control (control_params=0).</param>
        /// <returns>The function returns the estimated state. </returns>
#endif
        public static CvMat KalmanPredict(CvKalman kalman, CvMat control)
        {
            if (kalman == null)
            {
                throw new ArgumentNullException("kalman");
            }
            IntPtr controlPtr = (control == null) ? IntPtr.Zero : control.CvPtr;
            IntPtr result     = NativeMethods.cvKalmanPredict(kalman.CvPtr, controlPtr);

            if (result == IntPtr.Zero)
            {
                return(null);
            }
            else
            {
                return(new CvMat(result, false));
            }
        }
コード例 #5
0
ファイル: VideoCaptureScript.cs プロジェクト: Titoulion/Shoal
    // Set up the initial values for the Kalman filter
    void InitializeKalmanFilter()
    {
        // Create the Kalman Filter
        _kalman = Cv.CreateKalman(4, 2, 0);

        // Set the Kalman filter initial state
        _kalman.StatePre.Set2D(0, 0, _rectToTrack.X); // Initial X position
        _kalman.StatePre.Set2D(1, 0, _rectToTrack.Y); // Initial Y position
        _kalman.StatePre.Set2D(2, 0, 0);  // Initial X velocity
        _kalman.StatePre.Set2D(3, 0, 0);  // Initial Y velocity

        // Prediction Equations

        // X position is a function of previous X positions and previous X velocities
        _kalman.TransitionMatrix.Set2D(0, 0, 1);
        _kalman.TransitionMatrix.Set2D(1, 0, 0);
        _kalman.TransitionMatrix.Set2D(2, 0, 1);
        _kalman.TransitionMatrix.Set2D(3, 0, 0);

        // Y position is a function of previous Y positions and previous Y velocities
        _kalman.TransitionMatrix.Set2D(0, 1, 0);
        _kalman.TransitionMatrix.Set2D(1, 1, 1);
        _kalman.TransitionMatrix.Set2D(2, 1, 0);
        _kalman.TransitionMatrix.Set2D(3, 1, 1);

        // X velocity is a function of previous X velocities
        _kalman.TransitionMatrix.Set2D(0, 2, 0);
        _kalman.TransitionMatrix.Set2D(1, 2, 0);
        _kalman.TransitionMatrix.Set2D(2, 2, 1);
        _kalman.TransitionMatrix.Set2D(3, 2, 0);

        // Y velocity is a function of previous Y velocities
        _kalman.TransitionMatrix.Set2D(0, 3, 0);
        _kalman.TransitionMatrix.Set2D(1, 3, 0);
        _kalman.TransitionMatrix.Set2D(2, 3, 0);
        _kalman.TransitionMatrix.Set2D(3, 3, 1);

        // set Kalman Filter
        Cv.SetIdentity(_kalman.MeasurementMatrix, 1.0f);
        Cv.SetIdentity(_kalman.ProcessNoiseCov, 0.4f);  //0.4
        Cv.SetIdentity(_kalman.MeasurementNoiseCov, 3f); //3
        Cv.SetIdentity(_kalman.ErrorCovPost, 1.0f);
    }
コード例 #6
0
ファイル: Kalman.cs プロジェクト: neoxeo/opencvsharp
        public unsafe Kalman()
        {
            // cvKalmanPredict, cvKalmanCorrect
            // カルマンフィルタを用いて回転する点を追跡する

            // A matrix data
            float[] A = new float[] { 1, 1, 0, 1 };

            using (IplImage img = new IplImage(500, 500, BitDepth.U8, 3))
            using (CvKalman kalman = new CvKalman(2, 1, 0))
            using (CvWindow window = new CvWindow("Kalman", WindowMode.AutoSize))
            {
                // state is (phi, delta_phi) - angle and angle increment
                CvMat state = new CvMat(2, 1, MatrixType.F32C1);
                CvMat process_noise = new CvMat(2, 1, MatrixType.F32C1);
                // only phi (angle) is measured
                CvMat measurement = new CvMat(1, 1, MatrixType.F32C1);

                measurement.SetZero();
                CvRandState rng = new CvRandState(0, 1, -1, DistributionType.Uniform);
                int code = -1;

                for (; ; )
                {
                    Cv.RandSetRange(rng, 0, 0.1, 0);
                    rng.DistType = DistributionType.Normal;

                    Marshal.Copy(A, 0, kalman.TransitionMatrix.Data, A.Length);
                    kalman.MeasurementMatrix.SetIdentity(1);
                    kalman.ProcessNoiseCov.SetIdentity(1e-5);
                    kalman.MeasurementNoiseCov.SetIdentity(1e-1);
                    kalman.ErrorCovPost.SetIdentity(1);
                    // choose random initial state
                    Cv.Rand(rng, kalman.StatePost);
                    rng.DistType = DistributionType.Normal;

                    for (; ; )
                    {
                        float state_angle = state.DataSingle[0];
                        CvPoint state_pt = CalcPoint(img, state_angle);

                        // predict point position
                        CvMat prediction = kalman.Predict(null);
                        float predict_angle = prediction.DataSingle[0];
                        CvPoint predict_pt = CalcPoint(img, predict_angle);

                        Cv.RandSetRange(rng, 0, Math.Sqrt(kalman.MeasurementNoiseCov.DataSingle[0]), 0);
                        Cv.Rand(rng, measurement);

                        // generate measurement
                        Cv.MatMulAdd(kalman.MeasurementMatrix, state, measurement, measurement);

                        float measurement_angle = measurement.DataArraySingle[0];
                        CvPoint measurement_pt = CalcPoint(img, measurement_angle);

                        img.SetZero();
                        DrawCross(img, state_pt, CvColor.White, 3);
                        DrawCross(img, measurement_pt, CvColor.Red, 3);
                        DrawCross(img, predict_pt, CvColor.Green, 3);
                        img.Line(state_pt, measurement_pt, new CvColor(255, 0, 0), 3, LineType.AntiAlias, 0);
                        img.Line(state_pt, predict_pt, new CvColor(255, 255, 0), 3, LineType.AntiAlias, 0);

                        // adjust Kalman filter state
                        kalman.Correct(measurement);

                        Cv.RandSetRange(rng, 0, Math.Sqrt(kalman.ProcessNoiseCov.DataSingle[0]), 0);
                        Cv.Rand(rng, process_noise);
                        Cv.MatMulAdd(kalman.TransitionMatrix, state, process_noise, state);

                        window.ShowImage(img);
                        // break current simulation by pressing a key
                        code = CvWindow.WaitKey(100);
                        if (code > 0)
                        {
                            break;
                        }
                    }
                    // exit by ESCAPE
                    if (code == 27)
                    {
                        break;
                    }
                }
            }
        }
コード例 #7
0
ファイル: Cv_K.cs プロジェクト: sanglin307/UnityOpenCV
        /// <summary>
        /// 次のモデル状態を推定する.
        /// </summary>
        /// <param name="kalman">カルマンフィルタ状態</param>
        /// <returns></returns>
#else
        /// <summary>
        /// Estimates subsequent model state
        /// </summary>
        /// <param name="kalman">Kalman filter state. </param>
        /// <returns>The function returns the estimated state. </returns>
#endif
        public static CvMat KalmanPredict(CvKalman kalman)
        {
            return KalmanPredict(kalman, null);
        }
コード例 #8
0
ファイル: Cv_K.cs プロジェクト: sanglin307/UnityOpenCV
        /// <summary>
        /// モデル状態を修正する. 修正された状態を kalman->state_post に保存し,これを出力として返す.cvKalmanCorrectのエイリアス.
        /// </summary>
        /// <param name="kalman">更新されるカルマンフィルタ状態</param>
        /// <param name="measurement">観測ベクトルを含むCvMat</param>
        /// <returns>修正された状態を kalman->state_post に保存し,これを出力として返す.</returns>
#else
        /// <summary>
        /// Adjusts model state
        /// </summary>
        /// <param name="kalman">Kalman filter</param>
        /// <param name="measurement">CvMat containing the measurement vector. </param>
        /// <returns>The function stores adjusted state at kalman->state_post and returns it on output.</returns>
#endif
        public static CvMat KalmanUpdateByMeasurement(CvKalman kalman, CvMat measurement)
        {
            return KalmanCorrect(kalman, measurement);
        }
コード例 #9
0
ファイル: Cv_K.cs プロジェクト: sanglin307/UnityOpenCV
        /// <summary>
        /// 次のモデル状態を推定する. cvKalmanPredictのエイリアス.
        /// </summary>
        /// <param name="kalman">カルマンフィルタ状態</param>
        /// <param name="control">コントロールベクトル (uk).外部コントロールが存在しない場合(control_params=0)に限り,null である.</param>
        /// <returns></returns>
#else
        /// <summary>
        /// Estimates subsequent model state
        /// </summary>
        /// <param name="kalman">Kalman filter state. </param>
        /// <param name="control">Control vector (uk), should be null iff there is no external control (control_params=0).</param>
        /// <returns>The function returns the estimated state. </returns>
#endif
        public static CvMat KalmanUpdateByTime(CvKalman kalman, CvMat control)
        {
            return KalmanPredict(kalman, control);
        }
コード例 #10
0
ファイル: Cv_K.cs プロジェクト: sanglin307/UnityOpenCV
        /// <summary>
        /// 次のモデル状態を推定する. cvKalmanPredictのエイリアス.
        /// </summary>
        /// <param name="kalman">カルマンフィルタ状態</param>
        /// <returns></returns>
#else
        /// <summary>
        /// Estimates subsequent model state
        /// </summary>
        /// <param name="kalman">Kalman filter state. </param>
        /// <returns>The function returns the estimated state. </returns>
#endif
        public static CvMat KalmanUpdateByTime(CvKalman kalman)
        {
            return KalmanPredict(kalman);
        }
コード例 #11
0
ファイル: Cv_K.cs プロジェクト: shimat/opencvsharp_2410
        /// <summary>
        /// 次のモデル状態を推定する.
        /// </summary>
        /// <param name="kalman">カルマンフィルタ状態</param>
        /// <returns></returns>
#else
        /// <summary>
        /// Estimates subsequent model state
        /// </summary>
        /// <param name="kalman">Kalman filter state. </param>
        /// <returns>The function returns the estimated state. </returns>
#endif
        public static CvMat KalmanPredict(CvKalman kalman)
        {
            return(KalmanPredict(kalman, null));
        }
コード例 #12
0
ファイル: Cv_K.cs プロジェクト: shimat/opencvsharp_2410
        /// <summary>
        /// モデル状態を修正する. 修正された状態を kalman->state_post に保存し,これを出力として返す.cvKalmanCorrectのエイリアス.
        /// </summary>
        /// <param name="kalman">更新されるカルマンフィルタ状態</param>
        /// <param name="measurement">観測ベクトルを含むCvMat</param>
        /// <returns>修正された状態を kalman->state_post に保存し,これを出力として返す.</returns>
#else
        /// <summary>
        /// Adjusts model state
        /// </summary>
        /// <param name="kalman">Kalman filter</param>
        /// <param name="measurement">CvMat containing the measurement vector. </param>
        /// <returns>The function stores adjusted state at kalman->state_post and returns it on output.</returns>
#endif
        public static CvMat KalmanUpdateByMeasurement(CvKalman kalman, CvMat measurement)
        {
            return(KalmanCorrect(kalman, measurement));
        }
コード例 #13
0
ファイル: Cv_K.cs プロジェクト: shimat/opencvsharp_2410
        /// <summary>
        /// 次のモデル状態を推定する. cvKalmanPredictのエイリアス.
        /// </summary>
        /// <param name="kalman">カルマンフィルタ状態</param>
        /// <param name="control">コントロールベクトル (uk).外部コントロールが存在しない場合(control_params=0)に限り,null である.</param>
        /// <returns></returns>
#else
        /// <summary>
        /// Estimates subsequent model state
        /// </summary>
        /// <param name="kalman">Kalman filter state. </param>
        /// <param name="control">Control vector (uk), should be null iff there is no external control (control_params=0).</param>
        /// <returns>The function returns the estimated state. </returns>
#endif
        public static CvMat KalmanUpdateByTime(CvKalman kalman, CvMat control)
        {
            return(KalmanPredict(kalman, control));
        }
コード例 #14
0
ファイル: Cv_K.cs プロジェクト: shimat/opencvsharp_2410
        /// <summary>
        /// 次のモデル状態を推定する. cvKalmanPredictのエイリアス.
        /// </summary>
        /// <param name="kalman">カルマンフィルタ状態</param>
        /// <returns></returns>
#else
        /// <summary>
        /// Estimates subsequent model state
        /// </summary>
        /// <param name="kalman">Kalman filter state. </param>
        /// <returns>The function returns the estimated state. </returns>
#endif
        public static CvMat KalmanUpdateByTime(CvKalman kalman)
        {
            return(KalmanPredict(kalman));
        }