public void Update(Image <Bgr, byte> nowImage)
{
DebugImage = nowImage;
DerivePitchEdges(nowImage);
TopLeft = GetTopLeft();
TopRight = GetTopRight();
BottomLeft = GetBottomLeft();
BottomRight = GetBottomRight();
PointF[] sourcePoints = { TopLeft, TopRight, BottomLeft, BottomRight };
PointF[] destPoints =
{
new PointF(Instep, Border),
new PointF(PitchWidth - Instep, Border),
new PointF(Instep, PitchHeight + Border),
new PointF(PitchWidth - Instep, PitchHeight + Border)
};
m_WarpMat = CameraCalibration.GetPerspectiveTransform(sourcePoints, destPoints);
m_WarpMatInv = CameraCalibration.GetPerspectiveTransform(destPoints, sourcePoints);
PerspImage = nowImage.WarpPerspective(m_WarpMat, 1205, (int)(PitchHeight + Border * 2),
Emgu.CV.CvEnum.INTER.CV_INTER_CUBIC,
Emgu.CV.CvEnum.WARP.CV_WARP_FILL_OUTLIERS,
new Bgr(200, 200, 200)).Convert <Bgr, byte>();
ThresholdedPerspImage = ImageProcess.ThresholdHsv(PerspImage, 22, 89, 33, 240, 40, 250).
ThresholdBinaryInv(new Gray(100), new Gray(255));
//DerivePolePositions();
}
internal void FindMen(Image <Bgr, byte> perspImage)
{
Image <Hsv, byte> perspImageHsv = perspImage.Convert <Hsv, byte>();
int height = 660;
Image <Bgr, byte> stripBgr = new Image <Bgr, byte>(1, height);
Image <Hsv, byte> stripHsv = new Image <Hsv, byte>(1, height);
Image <Gray, byte> thresholded;
float gradient = (m_XStart - m_XEnd) / height;
string str = "";
for (int y = 0; y < height; y++)
{
int x = (int)((gradient * y) + m_XStart);
stripBgr.Data[y, 0, 0] = perspImage.Data[y, x, 0];
stripBgr.Data[y, 0, 1] = perspImage.Data[y, x, 1];
stripBgr.Data[y, 0, 2] = perspImage.Data[y, x, 2];
stripHsv.Data[y, 0, 0] = perspImageHsv.Data[y, x, 0];
stripHsv.Data[y, 0, 1] = perspImageHsv.Data[y, x, 1];
stripHsv.Data[y, 0, 2] = perspImageHsv.Data[y, x, 2];
str += stripBgr.Data[y, 0, 0] + "," + stripBgr.Data[y, 0, 1] + "," + stripBgr.Data[y, 0, 2] + "," +
stripHsv.Data[y, 0, 0] + "," + stripHsv.Data[y, 0, 1] + "," + stripHsv.Data[y, 0, 2] + "\r\n";
}
thresholded = ImageProcess.ThresholdHsv(stripBgr, 0, 23, 45, 98, 100, 256);
}
private void DerivePitchEdges(Image <Bgr, Byte> image)
{
ThresholdedImage = ImageProcess.ThresholdHsv(image, 22, 89, 33, 240, 40, 250);
MaskedImage = image.Copy(ThresholdedImage);
CannyImage = MaskedImage.Canny(200, 100);
var lines = CannyImage.HoughLinesBinary(1, Math.PI / 360, 100, 200, 50)[0];
foreach (var line in lines)
{
float dirY = line.Direction.Y;
if (line.Length > 500 && dirY < 0.3 && dirY > -0.3f)
{
if (line.P1.Y < image.Height / 2 && line.P2.Y < image.Height / 2)
{
m_TopBorderCandidateLines.Enqueue(line);
if (m_TopBorderCandidateLines.Count > 20)
{
m_TopBorderCandidateLines.Dequeue();
}
}
else if (line.P1.Y > image.Height / 2 && line.P2.Y > image.Height / 2)
{
m_BottomBorderCandidates.Enqueue(line);
if (m_BottomBorderCandidates.Count > 20)
{
m_BottomBorderCandidates.Dequeue();
}
}
}
}
}
internal void Update(Image <Bgr, byte> image)
{
Image <Gray, byte> threshImage;
bool pointObserved = false;
// Try looking in local window
if (Pos != Point.Empty)
{
int width = 120;
int height = 60;
int x = Math.Max(1, (int)Pos.X - (width / 2));
int y = Math.Max(1, (int)Pos.Y - (height / 2));
if (x + width > image.Width)
{
width = image.Width - x - 1;
}
if (y + height > image.Height)
{
height = image.Height - y - 1;
}
if (x < 0 || x + width > image.Width || y < 0 || y + height > image.Height ||
width < 1 || height < 1)
{
goto notGoodSearch;
}
Image <Bgr, byte> searchWindow = image.GetSubRect(new Rectangle(x, y, width, height));
threshImage = ImageProcess.ThresholdHsv(searchWindow, 14, 37, 150, 256, 98, 256);
PointF searchPos = ImageProcess.GetCentreOfMass(threshImage.Erode(1).Dilate(1).Erode(1), 4);
if (searchPos != Point.Empty)
{
pointObserved = true;
Pos = new PointF(x + searchPos.X, y + searchPos.Y);
}
}
notGoodSearch:
if (Pos == Point.Empty || pointObserved == false) // If window fails to find point, then search the whole image
{
threshImage = ImageProcess.ThresholdHsv(image, 17, 34, 150, 256, 98, 256);
PointF searchPos = ImageProcess.GetCentreOfMass(threshImage.Erode(2).Dilate(1).Erode(1), 6);
if (searchPos != Point.Empty)
{
pointObserved = true;
Pos = searchPos;
}
}
Matrix <float> prediction = m_KalmanFilter.Predict();
Matrix <float> measured = new Matrix <float>(new float[, ] {
{ Pos.X }, { Pos.Y }
});
if (pointObserved)
{
Matrix <float> estimated = m_KalmanFilter.Correct(measured);
Pos = new PointF(estimated[0, 0], estimated[1, 0]);
}
else
{
Pos = new PointF(prediction[0, 0], prediction[1, 0]);
}
if (Pos.X < 1)
{
Pos.X = 1;
}
if (Pos.Y < 1)
{
Pos.Y = 1;
}
if (Pos.X > 1279)
{
Pos.X = 1279;
}
if (Pos.Y > 719)
{
Pos.Y = 719;
}
}
