public MCvPoint3D32f Kalman4Joints(float x, float y, float z, Kalman kalman, SyntheticData syntheticData)
{
//Kalman headPC1
Emgu.CV.Matrix <float> MatrixGet = new Emgu.CV.Matrix <float>(6, 1);
MatrixGet[0, 0] = x;
MatrixGet[1, 0] = y;
MatrixGet[2, 0] = z;
kalman = new Kalman(6, 3, 0);
Emgu.CV.Matrix <float> state = MatrixGet;
kalman.CorrectedState = state;
kalman.TransitionMatrix = syntheticData.transitionMatrix;
kalman.MeasurementNoiseCovariance = syntheticData.measurementNoise;
kalman.ProcessNoiseCovariance = syntheticData.processNoise;
kalman.ErrorCovariancePost = syntheticData.errorCovariancePost;
kalman.MeasurementMatrix = syntheticData.measurementMatrix;
Matrix <float> prediction = new Matrix <float>(3, 1);
prediction = kalman.Predict();
MCvPoint3D32f predictPointheadPC1 = new MCvPoint3D32f(prediction[0, 0], prediction[1, 0], prediction[2, 0]);
MCvPoint3D32f measurePointheadPC1 = new MCvPoint3D32f(syntheticData.GetMeasurement()[0, 0],
syntheticData.GetMeasurement()[1, 0], syntheticData.GetMeasurement()[2, 0]);
Matrix <float> estimated = kalman.Correct(syntheticData.GetMeasurement());
MCvPoint3D32f estimatedPoint = new MCvPoint3D32f(estimated[0, 0], estimated[1, 0], estimated[2, 0]);
syntheticData.GoToNextState();
return(estimatedPoint);
}
//Filter data and store values
public void filterPoints(float[] pt)
{
//add tracking data as filter state
mk.state[0, 0] = pt[0];
mk.state[1, 0] = pt[1];
if (start)
{
//if it is first run of the filter add orientation value directly
mk.state[2, 0] = pt[2];
start = false;
}
else
{
//if it is not the first frame corect orientation to avoid high derivatives in rotation
//there is a factor of 5 to make rotations a similar order of magnitude as translation
mk.state[2, 0] = 5f * CorrectedOrientation(pt[2], estimated[2, 0] / 5f);
}
//run filter and estimate real position and orientation
kal.Predict();
estimated = kal.Correct(mk.GetMeasurement());
mk.GoToNextState();
//save estimated position
pars[0] = estimated[0, 0];
pars[1] = estimated[1, 0];
pars[2] = estimated[2, 0] / 5f;
}
private Point[] aplicarFiltroKalman(MCvBlob blob)
{
if (mPrimeiraExecucao)
{
kal.PredictedState = new Matrix <float>(new float[4, 1] {
{ blob.Center.X }, { blob.Center.Y }, { 0 }, { 0 }
});
}
Matrix <float> prediction = kal.Predict();
Point predictPt = new Point((int)prediction[0, 0], (int)prediction[1, 0]);
// Get blob point
medidaKalman[0, 0] = blob.Center.X;
medidaKalman[1, 0] = blob.Center.Y;
// The update phase
Matrix <float> estimado = kal.Correct(medidaKalman);
Point estadoPt = new Point((int)estimado[0, 0], (int)estimado[1, 0]);
Point medidoPt = new Point((int)medidaKalman[0, 0], (int)medidaKalman[1, 0]);
Point[] retorno = { estadoPt, medidoPt };
CvInvoke.WaitKey(10);
return(retorno);
}
public PointF[] filterPoints(PointF pt, int camID)
{
PointF[] results = new PointF[2];
if (camID == 0)
{
syntheticData.state[0, 0] = pt.X;
syntheticData.state[1, 0] = pt.Y;
Matrix <float> prediction = kal.Predict();
PointF predictPoint = new PointF(prediction[0, 0], prediction[1, 0]);
PointF measurePoint = new PointF(syntheticData.GetMeasurement()[0, 0],
syntheticData.GetMeasurement()[1, 0]);
Matrix <float> estimated = kal.Correct(syntheticData.GetMeasurement());
PointF estimatedPoint = new PointF(estimated[0, 0], estimated[1, 0]);
syntheticData.GoToNextState();
results[0] = predictPoint;
results[1] = estimatedPoint;
px = predictPoint.X;
py = predictPoint.Y;
cx = estimatedPoint.X;
cy = estimatedPoint.Y;
}
else
{
syntheticData2.state[0, 0] = pt.X;
syntheticData2.state[1, 0] = pt.Y;
Matrix <float> prediction = kal.Predict();
PointF predictPoint = new PointF(prediction[0, 0], prediction[1, 0]);
PointF measurePoint = new PointF(syntheticData2.GetMeasurement()[0, 0],
syntheticData.GetMeasurement()[1, 0]);
Matrix <float> estimated = kal.Correct(syntheticData2.GetMeasurement());
PointF estimatedPoint = new PointF(estimated[0, 0], estimated[1, 0]);
syntheticData.GoToNextState();
results[0] = predictPoint;
results[1] = estimatedPoint;
px2 = predictPoint.X;
py2 = predictPoint.Y;
cx2 = estimatedPoint.X;
cy2 = estimatedPoint.Y;
}
return(results);
}
public Point Update(Point pt)
{
state[0, 0] = (float)pt.X;
state[1, 0] = (float)pt.Y;
Matrix <float> prediction = kalman.Predict();
Point predictPoint = new Point(prediction[0, 0], prediction[1, 0]);
Point measurePoint = new Point(GetMeasurement()[0, 0], GetMeasurement()[1, 0]);
Matrix <float> estimated = kalman.Correct(GetMeasurement());
Point estimatedPoint = new Point(estimated[0, 0], estimated[1, 0]);
GoToNextState();
//Point[] results = new Point[2];
//results[0] = predictPoint; //预测位置
//results[1] = estimatedPoint; //估计位置
//px = predictPoint.X;
//py = predictPoint.Y;
//cx = estimatedPoint.X;
//cy = estimatedPoint.Y;
return(estimatedPoint);
}
public Matrix <float> FilterPoints(Kalman kalman, float x, float y, float z)
{
Matrix <float> prediction = kalman.Predict();
Matrix <float> estimated = kalman.Correct(new Matrix <float>(new[] { x, y, z }));
Matrix <float> results = new Matrix <float>(new[, ]
{
{ prediction[0, 0], prediction[1, 0], prediction[2, 0] },
{ estimated[0, 0], estimated[1, 0], estimated[2, 0] }
});
return(results);
}
public override void UpdateState(Vector3 point)
{
componentCount = Math.Max(1, componentCount);
if (Kalman != null &&
componentCount == ComponentCount &&
Mathf.Approximately(measurementNoise, mNoise) &&
processNoiseScale == procNoiseK &&
Mathf.Approximately(processNoiseExponent, procNoiseE))
{
Kalman.Correct(point);
}
else
{
ComponentCount = componentCount;
Dimension = VectorSize * ComponentCount;
mNoise = measurementNoise;
procNoiseK = processNoiseScale;
procNoiseE = processNoiseExponent;
var initialState = new Vector3[ComponentCount];
initialState[0] = lastPoint ?? point;
Kalman = new DiscreteKalmanFilter <Vector3[], Vector3>(
initialState,
GetProcessNoise(_ => processNoiseScale),
(int)VectorSize,
0,
ToArray,
ToVector3s,
ToArray)
{
ProcessNoise = GetProcessNoise(i =>
Mathf.Pow(processNoiseScale, processNoiseExponent * (i + 1))),
MeasurementMatrix = MakePositionMeasurementMatrix(),
TransitionMatrix = MakeTransitionMatrix(),
MeasurementNoise = Matrix.Diagonal((int)VectorSize, (double)measurementNoise)
};
if (lastPoint != null)
{
Kalman.Correct(point);
}
Kalman.Predict();
lastPoint = point;
}
}
//Filter data and store values
private float[] filterPoints(float[] pt)
{
//add tracking data as filter state
mk.state[0, 0] = pt[0];
mk.state[1, 0] = pt[1];
mk.state[2, 0] = pt[2];
//run filter and estimate real position and orientation
kal.Predict();
estimated = kal.Correct(mk.GetMeasurement());
mk.GoToNextState();
//save estimated position
pars[0] = estimated[0, 0];
pars[1] = estimated[1, 0];
pars[2] = estimated[2, 0] / 5f;
return(pars);
}
public void TestKalman()
{
Image <Bgr, Byte> img = new Image <Bgr, byte>(400, 400);
SyntheticData syntheticData = new SyntheticData();
// state is (phi, delta_phi) - angle and angle increment
Matrix <float> state = new Matrix <float>(new float[] { 0.0f, 0.0f }); //initial guess
#region initialize Kalman filter
Kalman tracker = new Kalman(2, 1, 0);
tracker.TransitionMatrix = syntheticData.TransitionMatrix;
tracker.MeasurementMatrix = syntheticData.MeasurementMatrix;
tracker.ProcessNoiseCovariance = syntheticData.ProcessNoise;
tracker.MeasurementNoiseCovariance = syntheticData.MeasurementNoise;
tracker.ErrorCovariancePost = syntheticData.ErrorCovariancePost;
tracker.CorrectedState = state;
#endregion
System.Converter <double, PointF> angleToPoint =
delegate(double radianAngle)
{
return(new PointF(
(float)(img.Width / 2 + img.Width / 3 * Math.Cos(radianAngle)),
(float)(img.Height / 2 - img.Width / 3 * Math.Sin(radianAngle))));
};
Action <PointF, Bgr> drawCross =
delegate(PointF point, Bgr color)
{
img.Draw(new Cross2DF(point, 15, 15), color, 1);
};
ImageViewer viewer = new ImageViewer();
System.Windows.Forms.Timer timer = new System.Windows.Forms.Timer();
timer.Interval = 200;
timer.Tick += new EventHandler(delegate(object sender, EventArgs e)
{
Matrix <float> measurement = syntheticData.GetMeasurement();
// adjust Kalman filter state
tracker.Correct(measurement);
tracker.Predict();
#region draw the state, prediction and the measurement
PointF statePoint = angleToPoint(tracker.CorrectedState[0, 0]);
PointF predictPoint = angleToPoint(tracker.PredictedState[0, 0]);
PointF measurementPoint = angleToPoint(measurement[0, 0]);
img.SetZero(); //clear the image
drawCross(statePoint, new Bgr(Color.White)); //draw current state in White
drawCross(measurementPoint, new Bgr(Color.Red)); //draw the measurement in Red
drawCross(predictPoint, new Bgr(Color.Green)); //draw the prediction (the next state) in green
img.Draw(new LineSegment2DF(statePoint, predictPoint), new Bgr(Color.Magenta), 1); //Draw a line between the current position and prediction of next position
//Trace.WriteLine(String.Format("Velocity: {0}", tracker.CorrectedState[1, 0]));
#endregion
syntheticData.GoToNextState();
viewer.Image = img;
});
timer.Start();
viewer.Disposed += delegate(Object sender, EventArgs e) { timer.Stop(); };
viewer.Text = "Actual State: White; Measurement: Red; Prediction: Green";
viewer.ShowDialog();
}