public void estimate(EMarker m)
{
imagePoints = new AForge.Point[]
{
new AForge.Point( m.getContour()[3].X
, m.getContour()[3].Y ),
new AForge.Point( m.getContour()[2].X
, m.getContour()[2].Y ),
new AForge.Point( m.getContour()[1].X
,m.getContour()[1].Y ),
new AForge.Point( m.getContour()[0].X
, m.getContour()[0].Y )};
//change cordinate system
for (int i = 0; i < 4; i++) {
if (imagePoints[i].X < imageWidth / 2) imagePoints[i].X = -(imageWidth / 2) + imagePoints[i].X;
else imagePoints[i].X = imagePoints[i].X - (imageWidth / 2);
if (imagePoints[i].Y < imageHeight / 2) imagePoints[i].Y = (imageHeight / 2) - imagePoints[i].Y;
else imagePoints[i].Y = -imagePoints[i].Y + (imageHeight / 2);
}
positAlgorithm.EstimatePose(imagePoints,
out rotationMatrix,out translationVector);
rotationMatrix.ExtractYawPitchRoll(out estimatedYaw,
out estimatedPitch,
out estimatedRoll);
rotationMatrix = Matrix3x3.CreateFromYawPitchRoll(estimatedYaw,
estimatedPitch, estimatedRoll);
}
public void findMarkers(Image<Gray, Byte> imgGray)
{
contours.Clear();
contoursOk.Clear();
markers.Clear();
markerExists = false;
grayImage = imgGray;
binaryImage = threshold(imgGray, 127, 255);
findContours(binaryImage.Cols / 5);
findCandidates();
findRelationBetweenMarker();
findInternalFrameAndDetectMarker();
if (markerExists)
{
rot4MarkerAndCreateMarkerElem(0);
oldMarker = markers[0];
aproxCounter = 0 ;
}
else
{
if (aproxCounter < maxAproxCounter)
{
markers.Add(oldMarker);
markerExists = true;
aproxCounter++;
}
}
}