/// <summary>
///
/// </summary>
/// <param name="ptr"></param>
#else
/// <summary>
/// Initializes from pointer
/// </summary>
/// <param name="ptr"></param>
#endif
public CvRandState(IntPtr ptr)
{
if (ptr == null)
{
throw new ArgumentNullException("ptr");
}
CvRandState s = (CvRandState)Marshal.PtrToStructure(ptr, typeof(CvRandState));
this._disttype = s._disttype;
this._param = s._param;
this._state = s._state;
}
/// <summary>
///
/// </summary>
/// <param name="value"></param>
internal void Set(CvRandState value)
{
this._disttype = value._disttype;
this._param = value._param;
this._state = value._state;
}
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;
}
}
}
}
/// <summary>
///
/// </summary>
/// <param name="value"></param>
internal void Set(CvRandState value)
{
this._disttype = value._disttype;
this._param = value._param;
this._state = value._state;
}
