public override MarkerFrame Process(IplImage input)
{
var hInput = input.DangerousGetHandle();
var hCamMatrix = cameraMatrix != null?cameraMatrix.DangerousGetHandle() : IntPtr.Zero;
var hDistortion = distortion != null?distortion.DangerousGetHandle() : IntPtr.Zero;
detector.ThresholdType = ThresholdType;
detector.Param1 = Param1;
detector.Param2 = Param2;
detector.CornerRefinement = CornerRefinement;
var detectedMarkers = detector.Detect(hInput, hCamMatrix, hDistortion, MarkerSize);
return(new MarkerFrame(parameters, detectedMarkers));
}
private void CameraLoop()
{
CancellationToken cancellationToken = cancellationTokenSource.Token;
var parameters = new CameraParameters();
Size size;
var cameraMatrix = new Mat(3, 3, Depth.F32, 1);
var distortion = new Mat(1, 4, Depth.F32, 1);
parameters.CopyParameters(cameraMatrix, distortion, out size);
using (var detector = new MarkerDetector())
{
detector.ThresholdMethod = ThresholdMethod.AdaptiveThreshold;
detector.Param1 = 7.0;
detector.Param2 = 7.0;
detector.MinSize = 0.04f;
detector.MaxSize = 0.5f;
detector.CornerRefinement = CornerRefinementMethod.Lines;
var markerSize = 10;
using (var capture = Capture.CreateCameraCapture(0))
{
while (!cancellationToken.IsCancellationRequested)
{
IplImage image = capture.QueryFrame();
var detectedMarkers = detector.Detect(image, cameraMatrix, distortion, markerSize);
foreach (var marker in detectedMarkers)
{
if (marker.Id == _MarkerIndex)
{
double sideOffset = Map(marker.Center.X, 46, 590, -8, 8);
double hightOffset = Map(marker.Center.Y, 46, 435, 6, -6);
double height = Map(marker.Area, 6280, 10150, 18.5, 14);
armDistance += hightOffset > 1.0 ? 0.4 : (hightOffset < -1.0 ? -0.4 : 0);
armAngle += sideOffset > 1.0 ? 0.7 : (sideOffset < -1.0 ? -0.7 : 0);
//armDistance += hightOffset;
_Arm?.MoveToRelativeAsync(armAngle, armDistance, armHeight);
marker.Draw(image, Scalar.Rgb(1, 0, 0));
}
else
{
marker.Draw(image, Scalar.Rgb(0, 1, 0));
}
}
WindwoDisplay(image);
}
}
}
}
public override IObservable <MarkerFrame> Process(IObservable <IplImage> source)
{
return(Observable.Defer(() =>
{
Mat cameraMatrix = null;
Mat distortion = null;
CameraParameters parameters = null;
var detector = new MarkerDetector();
Size size;
cameraMatrix = new Mat(3, 3, Depth.F32, 1);
distortion = new Mat(1, 4, Depth.F32, 1);
var parametersFileName = CameraParameters;
if (!string.IsNullOrEmpty(parametersFileName))
{
if (!File.Exists(parametersFileName))
{
throw new InvalidOperationException("Failed to open the camera parameters at the specified path.");
}
parameters = new CameraParameters();
parameters.ReadFromXmlFile(parametersFileName);
parameters.CopyParameters(cameraMatrix, distortion, out size);
}
return source.Select(input =>
{
detector.ThresholdMethod = ThresholdMethod;
detector.Param1 = Param1;
detector.Param2 = Param2;
detector.MinSize = MinSize;
detector.MaxSize = MaxSize;
detector.CornerRefinement = CornerRefinement;
var detectedMarkers = detector.Detect(input, cameraMatrix, distortion, MarkerSize);
return new MarkerFrame(parameters, detectedMarkers);
});
}));
}
