private async void PosController_PoseUpdated(ApriltagPoseEstimation pose)
{
await Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
lock (_frameLock)
{
var frame = pose.Frame;
var bgraData = new byte[frame.Data.Length];
var srcMat = new Mat(frame.Height, frame.Width, MatType.CV_8UC4, frame.Data);
var bgraMat = new Mat();
Cv2.CvtColor(srcMat, bgraMat, ColorConversionCodes.RGBA2BGRA);
Marshal.Copy(bgraMat.Data, bgraData, 0, frame.Data.Length);
if (LiveFrameSource == null || LiveFrameSource.PixelWidth != frame.Width || LiveFrameSource.PixelHeight != frame.Height)
{
LiveFrameSource = new WriteableBitmap(frame.Width, frame.Height);
LiveFeedImage.Source = LiveFrameSource;
}
bgraData.AsBuffer().CopyTo(LiveFrameSource.PixelBuffer);
LiveFrameSource.Invalidate();
//PoseText.Text = $"tag id: {pose.TagId} yaw: {pose.Yaw} pitch: {pose.Pitch} roll: {pose.Roll} tx: {pose.Tx} ty: {pose.Ty} tz: {pose.Tz}";
}
});
}
private async void Instance_PoseUpdated(ApriltagPoseEstimation pose)
{
await Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
lock (_frameLock)
{
//var frame = pose.Frame;
//var bgraData = new byte[frame.Data.Length];
//var srcMat = new Mat(frame.Height, frame.Width, MatType.CV_8UC4, frame.Data);
//var bgraMat = new Mat();
//Cv2.CvtColor(srcMat, bgraMat, ColorConversionCodes.RGBA2BGRA);
//Marshal.Copy(bgraMat.Data, bgraData, 0, frame.Data.Length);
//if (LiveFeedSource == null || LiveFeedSource.PixelWidth != frame.Width || LiveFeedSource.PixelHeight != frame.Height)
//{
// LiveFeedSource = new WriteableBitmap(frame.Width, frame.Height);
// LiveFeedImage.Source = LiveFeedSource;
//}
//bgraData.AsBuffer().CopyTo(LiveFeedSource.PixelBuffer);
//LiveFeedSource.Invalidate();
var tempAverrageQrIndex = 0;
var tempAverageQrCount = 0;
foreach (var result in pose.DetectResults)
{
//Debug.Print($"{ result.BarcodeText}\n");
var locReg = new Regex(@"^[A-Z]{2}[0-9]{1}[0-9]{2}[0-9][0-9]{2}$");
// only care about location tag
if (locReg.Match(result.BarcodeText).Success)
{
var LocationTag = result.BarcodeText;
var pos = Regex.Match(result.BarcodeText, @"(.{2})\s*$");;
var num = Int32.Parse(pos.Value);
tempAverrageQrIndex = tempAverrageQrIndex + num;
//AverrageQrIndex = AverrageQrIndex + num;
tempAverageQrCount += 1;
}
}
AverrageQrCount = tempAverageQrCount;
AverrageQrIndex = tempAverrageQrIndex;
//Debug.Print($"LocationTag: {ImageFrameCount} - { AverrageQrIndex}, {AverrageQrCount}, {AverrageQrIndex / AverrageQrCount} \n");
if (AverrageQrCount > 0)
{
FlightStacks.Instance._positionController.CurrentIndex = AverrageQrIndex / AverrageQrCount;
}
//PoseText.Text = $"tag id: {pose.TagId} yaw: {pose.Yaw} pitch: {pose.Pitch} roll: {pose.Roll} tx: {pose.Tx} ty: {pose.Ty} tz: {pose.Tz}";
}
});
}
