public IplImage OpticalFlowLK(IplImage previous, IplImage current) { IplImage prev = this.GrayScale(previous); IplImage curr = this.GrayScale(current); optical = current; int rows = optical.Height; int cols = optical.Width; CvMat velx = Cv.CreateMat(rows, cols, MatrixType.F32C1); CvMat vely = Cv.CreateMat(rows, cols, MatrixType.F32C1); velx.SetZero(); vely.SetZero(); Cv.CalcOpticalFlowLK(prev, curr, new CvSize(15, 15), velx, vely); for (int i = 0; i < cols; i += 15) { for (int j = 0; j < rows; j += 15) { int dx = (int)Cv.GetReal2D(velx, j, i); int dy = (int)Cv.GetReal2D(vely, j, i); Cv.DrawCircle(optical, i, j, 1, CvColor.Red); if (Math.Abs(dx) < 30 && Math.Abs(dy) < 30) { if (Math.Abs(dx) < 10 && Math.Abs(dy) < 10) { continue; } Cv.DrawLine(optical, new CvPoint(i, j), new CvPoint(i + dx, j + dy), CvColor.Blue, 1, LineType.AntiAlias); Cv.DrawCircle(optical, new CvPoint(i + dx, j + dy), 3, CvColor.Blue, -1); } } } return(optical); }
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; } } } }
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; } } } }