971 lines
45 KiB
C#
971 lines
45 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using Intel.RealSense;
|
||
using Emgu.CV.Structure;
|
||
using Emgu.CV;
|
||
using System.IO;
|
||
using System.Drawing;
|
||
using System.Windows.Forms;
|
||
using Stream = Intel.RealSense.Stream;
|
||
using System.Threading;
|
||
using WindowsFormsApp1; // 根据你的项目命名空间进行修改
|
||
using ABB.Robotics.Controllers.IOSystemDomain;
|
||
using yolov5_onnx.Mod;
|
||
using static WindowsFormsApp1.ExtrinsicsExtensions;
|
||
using System.Diagnostics;
|
||
using System.Threading.Tasks;
|
||
using System.Runtime.InteropServices;
|
||
using NetTopologySuite.GeometriesGraph;
|
||
using System.Windows.Media.Media3D;
|
||
|
||
namespace RealSence_PathMove
|
||
{
|
||
public class IntelRealSense
|
||
{
|
||
private readonly Form1 form;
|
||
|
||
public IntelRealSense(Form1 form)
|
||
{
|
||
|
||
this.form = form;
|
||
|
||
}
|
||
/// <summary>
|
||
/// 相機連結狀態
|
||
/// </summary>
|
||
enum CameraState
|
||
{
|
||
Disconnected = 0,
|
||
Connected = 1,
|
||
Opened = 2
|
||
}
|
||
Colorizer colorizer = new Colorizer();
|
||
Align align = new Align(Intel.RealSense.Stream.Color); // 深度影像MAP彩色影像
|
||
CameraState CamState = CameraState.Disconnected; // 初始狀態設為斷線
|
||
public AdvancedDevice DeviceName;
|
||
private Pipeline pipeline = new Pipeline();
|
||
internal PictureBox pictureBox2;
|
||
internal PictureBox pictureBox3;
|
||
internal PictureBox pictureBox4;
|
||
internal PictureBox pictureBox5;
|
||
private CustomProcessingBlock block;
|
||
internal CancellationTokenSource cts;
|
||
private System.Threading.Tasks.Task dataProcessingTask = System.Threading.Tasks.Task.CompletedTask; // 初始化为已完成的任务。
|
||
VideoStreamProfile depthStream;
|
||
VideoStreamProfile colorStream;
|
||
public Intrinsics depthIntrinsics; // 定義成員變數，以便在不同方法中訪問
|
||
public Extrinsics depthExtrinsics;
|
||
public Intrinsics colorIntrinsics;
|
||
public Extrinsics colorExtrinsics;
|
||
public Bitmap bmpColor; // 請確保這裡有定義 bmpColor 成員
|
||
public Bitmap bmpDepth;
|
||
public List<System.Drawing.PointF> yoloFeaturePoints = new List<System.Drawing.PointF>();
|
||
// 假设您已经获得了特徵點在相機座標系中的坐标列表 cameraPointsList
|
||
List<MathNet.Numerics.LinearAlgebra.Vector<double>> cameraPointsList = new List<MathNet.Numerics.LinearAlgebra.Vector<double>>();
|
||
public int objectCounter = 1; // 初始化物體編號為1
|
||
StringBuilder coordTextBuilder = new StringBuilder();
|
||
private readonly object frameLock = new object();
|
||
public int deepestX, deepestY, shallowestX, shallowestY, Goldsamplepointcloudnumber;
|
||
public double X, Y, u, v, percentageInsideCrop;
|
||
public float Z, x, y, z, Rx, Ry, Rz, threshold, deepestZ, shallowestZ, GolddeepestZ, GoldshallowestZ;
|
||
public string folderPath, fileName, folderPath1, fileName1;
|
||
public ListViewItem Item { get; set; }
|
||
public bool camclose;
|
||
public Extrinsics SharedExtrinsics { get; private set; }
|
||
public ushort[] DepthData { get; private set; }
|
||
public Bitmap yoloimage { get; private set; }
|
||
public float MaxDepthValue { get; private set; }
|
||
public System.Numerics.Vector3 cameraPoint { get; private set; }
|
||
public float scale { get; private set; }
|
||
public Image<Rgb, byte> ColorImg { get; private set; }
|
||
public Image<Rgb, byte> DepthImg { get; private set; }
|
||
public Image<Rgb, byte> ROIImg;
|
||
public Bitmap bitmap { get; private set; }
|
||
public ushort[] croppedDepthData;
|
||
// 在類別中添加一個 List<Vector3> 來儲存三維座標
|
||
public List<System.Numerics.Vector3> storedCoordinates = new List<System.Numerics.Vector3>();
|
||
public Bitmap DepthBitmap { get; private set; }
|
||
public Rectangle CroppingRect { get; private set; }
|
||
public byte[] ColorData { get; private set; }
|
||
public Bitmap DepthImg1Bitmap { get; private set; }
|
||
public bool frameReceived = false;
|
||
|
||
public bool captureThread1bool = true;
|
||
public bool TakeImage;
|
||
public int i = 0;
|
||
public Thread uiThread; // UI 线程
|
||
public CancellationTokenSource tokenSource = new CancellationTokenSource();
|
||
public delegate void NewFrameEventHandler(VideoFrame colorFrame, DepthFrame depthFrame);
|
||
public event NewFrameEventHandler NewFrameArrived;
|
||
public event NewFrameEventHandler NewFrameArrived11;
|
||
|
||
/// <summary>
|
||
/// 初始化設定並連接RealSenseL515相機
|
||
/// </summary>
|
||
internal void Connect()
|
||
{
|
||
var ctx = new Context();
|
||
// 查找L515设备
|
||
var devices = ctx.QueryDevices();
|
||
var adev = devices.FirstOrDefault(d => d.Info[CameraInfo.Name] == "Intel RealSense L515");
|
||
|
||
if (adev == null)
|
||
{
|
||
throw new Exception("No RealSense L515 detected");
|
||
}
|
||
// 连接成功
|
||
Console.WriteLine("Connected to: " + adev.Info[CameraInfo.Name]);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 開RealSenseL515相機
|
||
/// </summary>
|
||
[Obsolete]
|
||
internal void OpenCamera() //開啟相機
|
||
{
|
||
Connect();
|
||
var cfg = new Config(); // 通過序列號啟用設備
|
||
cfg.EnableStream(Stream.Depth, 640, 480, Format.Z16, 30);
|
||
cfg.EnableStream(Stream.Color, 640, 480, Format.Rgb8, 30);
|
||
pipeline = new Pipeline(); // 建立管線
|
||
var pp = pipeline.Start(cfg);// 啟動管線
|
||
// 獲取深度流和彩色流的配置
|
||
depthStream = pp.GetStream<VideoStreamProfile>(Stream.Depth);
|
||
colorStream = pp.GetStream<VideoStreamProfile>(Stream.Color);
|
||
SharedExtrinsics = depthStream.GetExtrinsicsTo(colorStream);// 獲取深度流到彩色流的外部參數 // extrinsics.rotation 是列主要的旋轉矩陣 // extrinsics.translation 是以米為單位的 xyz 位移
|
||
//獲取並應用深度到彩色的外部參數
|
||
var p1 = SharedExtrinsics.Transform(new Intel.RealSense.Math.Vector());
|
||
Console.WriteLine($"將外部參數應用到原點:({p1.x}, {p1.y}, {p1.z})");
|
||
InternalAndExternalGinseng();
|
||
var sensor = pp.Device.QuerySensors().First(s => s.Is(Extension.DepthSensor)); // 取得深度感測器
|
||
var blocks = sensor.ProcessingBlocks.ToList(); // 取得處理區塊
|
||
Sensor sensor1 = pp.Device.Sensors[0];
|
||
scale = sensor1.DepthScale;
|
||
Console.WriteLine("取得深度單位" + scale + "m");
|
||
try
|
||
{
|
||
// 建立自定義處理區塊
|
||
block = new CustomProcessingBlock((f, src) =>
|
||
{
|
||
using (var releaser = new FramesReleaser())
|
||
{
|
||
// 對每個框架執行處理區塊
|
||
foreach (ProcessingBlock p in blocks)
|
||
{
|
||
f = p.Process(f).DisposeWith(releaser);
|
||
// 對齊
|
||
f = f.ApplyFilter(align).DisposeWith(releaser);
|
||
// 使用 colorizer 處理區塊上色
|
||
f = f.ApplyFilter(colorizer).DisposeWith(releaser);
|
||
// 取得影格
|
||
var frames = f.As<FrameSet>();
|
||
var colorFrame = frames[Stream.Color, Format.Rgb8].DisposeWith(releaser);
|
||
var depthFrame = frames[Stream.Depth, Format.Z16].DisposeWith(releaser);
|
||
var colorizedDepth = frames[Stream.Depth, Format.Rgb8].DisposeWith(releaser);
|
||
// 合成影格
|
||
var res = src.AllocateCompositeFrame(colorizedDepth, colorFrame, depthFrame);
|
||
src.FrameReady(res);
|
||
}
|
||
}
|
||
});
|
||
// 啟動自定義區塊
|
||
block.Start(f =>
|
||
{
|
||
// 取得影格
|
||
using (var frames = f.As<FrameSet>())
|
||
{
|
||
|
||
// 取得彩色與深度影格
|
||
var _colorFrame = frames.ColorFrame.DisposeWith(frames);
|
||
var _depthFrame = frames.DepthFrame.DisposeWith(frames); // 原始的 depthFrame
|
||
var colorizedDepth = frames.First<VideoFrame>(Stream.Depth, Format.Rgb8).DisposeWith(frames); // 取得彩色化的深度影格
|
||
|
||
Image<Rgb, byte> emguImage = ColorFrameToImage(_colorFrame); // 轉換成影像格式
|
||
bitmap = emguImage.ToBitmap();
|
||
|
||
var imageDepth = DepthFrameToImage(colorizedDepth); // 轉換成影像格式
|
||
DepthBitmap = imageDepth.ToBitmap();// 將 Emgu CV 影像轉換成 Bitmap
|
||
|
||
// 在 UI 線程上更新 PictureBox
|
||
form.Invoke((MethodInvoker)delegate
|
||
{
|
||
form.SetPictureBox4Image(bitmap);
|
||
form.SetPictureBox5Image(DepthBitmap);
|
||
});
|
||
// 觸發 NewFrameArrived 事件
|
||
OnNewFrameArrived(_colorFrame, _depthFrame);
|
||
OnNewFrameArrived1(_colorFrame, _depthFrame);
|
||
}
|
||
});
|
||
cts = new CancellationTokenSource();
|
||
// 创建并启动数据处理任务，覆盖现有的任务。
|
||
dataProcessingTask = System.Threading.Tasks.Task.Factory.StartNew(async () =>
|
||
{
|
||
while (!cts.Token.IsCancellationRequested)
|
||
{
|
||
using (var frames = pipeline.WaitForFrames())
|
||
{
|
||
if (frames != null)
|
||
{
|
||
block.Process(frames);
|
||
frameReceived = true;
|
||
}
|
||
else
|
||
{
|
||
frameReceived = false;
|
||
}
|
||
}
|
||
if (!frameReceived) // 如果没有接收到相机的帧数
|
||
{
|
||
if (form.cameraOpened) // 如果相机已经打开，则关闭它
|
||
{
|
||
form.cam = false;
|
||
form.cameraOpened = false;
|
||
await CloseCamera(); // 關閉相機
|
||
}
|
||
|
||
if (!form.cameraOpened) // 如果相机未打开，则打开它
|
||
{
|
||
form.cam = true;
|
||
OpenCamera(); // 打開像機
|
||
}
|
||
frameReceived = true;
|
||
}
|
||
}
|
||
}, cts.Token);
|
||
}
|
||
|
||
catch (Exception ex)
|
||
{
|
||
MessageBox.Show(ex.Message);
|
||
}
|
||
|
||
}
|
||
private void OnNewFrameArrived(VideoFrame colorFrame, DepthFrame depthFrame)
|
||
{
|
||
NewFrameEventHandler handler = NewFrameArrived;
|
||
handler?.Invoke(colorFrame, depthFrame);
|
||
}
|
||
private void OnNewFrameArrived1(VideoFrame colorFrame, DepthFrame depthFrame)
|
||
{
|
||
NewFrameEventHandler handler11 = NewFrameArrived11;
|
||
handler11?.Invoke(colorFrame, depthFrame);
|
||
}
|
||
|
||
// 將深度影格轉換為影像
|
||
Image<Rgb, byte> DepthFrameToImage(VideoFrame frame)
|
||
{
|
||
// 轉換並複製影格資料
|
||
var image = new Image<Rgb, byte>(frame.Width, frame.Height);
|
||
var bytes = new byte[frame.Stride * frame.Height];
|
||
System.Runtime.InteropServices.Marshal.Copy(frame.Data, bytes, 0, bytes.Length);
|
||
// 指定影像的位元組資料
|
||
image.Bytes = bytes;
|
||
return image;
|
||
}
|
||
// 將彩色影格轉換為影像
|
||
Image<Rgb, byte> ColorFrameToImage(VideoFrame frame)
|
||
{
|
||
// 轉換並複製影格資料
|
||
var image = new Image<Rgb, byte>(frame.Width, frame.Height);
|
||
var bytes = new byte[frame.Stride * frame.Height];
|
||
System.Runtime.InteropServices.Marshal.Copy(frame.Data, bytes, 0, bytes.Length);
|
||
// 指定影像的位元組資料
|
||
image.Bytes = bytes;
|
||
return image;
|
||
}
|
||
/// <summary>
|
||
/// 關RealSenseL515相機
|
||
/// </summary>
|
||
/// <returns></returns>
|
||
public async System.Threading.Tasks.Task CloseCamera()
|
||
{
|
||
if (pipeline != null)
|
||
{
|
||
CancellationTokenSource localCts = cts; // 避免并发访问
|
||
System.Threading.Tasks.Task localDataProcessingTask = dataProcessingTask;
|
||
|
||
try
|
||
{
|
||
if (localCts != null && !localCts.IsCancellationRequested)
|
||
{
|
||
localCts.Cancel(); // 取消任务
|
||
}
|
||
|
||
if (localDataProcessingTask != null)
|
||
{
|
||
// 等待数据处理任务结束
|
||
await System.Threading.Tasks.Task.WhenAll(
|
||
localDataProcessingTask,
|
||
System.Threading.Tasks.Task.Delay(Timeout.Infinite, localCts.Token)
|
||
);
|
||
}
|
||
}
|
||
catch (OperationCanceledException)
|
||
{
|
||
// 忽略任务取消异常
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Console.WriteLine("Error during camera close: " + ex.Message);
|
||
}
|
||
finally
|
||
{
|
||
if (pipeline != null)
|
||
{
|
||
pipeline.Stop(); // 停止管线
|
||
pipeline.Dispose(); // 释放资源
|
||
pipeline = null; // 确保不再引用已关闭的管线
|
||
}
|
||
}
|
||
}
|
||
}
|
||
/// <summary>
|
||
/// 提取金樣本
|
||
/// </summary>
|
||
[Obsolete]
|
||
public void CaptureImagesAndSave() //提取金樣本
|
||
{
|
||
|
||
DigitalSignal digitalSignalDo0 = (DigitalSignal)Form1.controller.IOSystem.GetSignal("Do0");
|
||
DigitalSignal digitalSignalDo1 = (DigitalSignal)Form1.controller.IOSystem.GetSignal("Do1");
|
||
digitalSignalDo0.Value = 1;
|
||
|
||
Form1.controller.Rapid.Start();
|
||
// 使用一個新的執行緒來運行 while 迴圈
|
||
Thread captureThread = new Thread(() =>
|
||
{
|
||
while (form.ImageProcessing)
|
||
{
|
||
if (digitalSignalDo1.Value == 1)
|
||
{
|
||
// 清除舊的資料
|
||
coordTextBuilder.Clear();
|
||
objectCounter = 1; // 重置物體編號1
|
||
Yolo3D();
|
||
digitalSignalDo1.Value = 0;
|
||
form.ImageProcessing = false;
|
||
break;
|
||
}
|
||
}
|
||
});
|
||
// 啟動執行緒
|
||
captureThread.Start();
|
||
}
|
||
/// <summary>
|
||
/// 提取比對樣本
|
||
/// </summary>
|
||
[Obsolete]
|
||
public void CompareSamples() //提取比對樣本
|
||
{
|
||
DigitalSignal digitalSignalDo0 = (DigitalSignal)Form1.controller.IOSystem.GetSignal("Do0");
|
||
DigitalSignal digitalSignalDo1 = (DigitalSignal)Form1.controller.IOSystem.GetSignal("Do1");
|
||
DigitalSignal digitalSignalDo3 = (DigitalSignal)Form1.controller.IOSystem.GetSignal("Do3");
|
||
digitalSignalDo1.Value = 0;
|
||
digitalSignalDo0.Value = 1;
|
||
Form1.controller.Rapid.Start();
|
||
ManualResetEvent pauseEvent = new ManualResetEvent(true);
|
||
// 使用一個新的執行緒來運行 while 迴圈
|
||
Thread captureThread1 = new Thread(() =>
|
||
{
|
||
while (form.ImageProcessing1)
|
||
{
|
||
Thread.Sleep(2); // 每 10 毫秒檢查一次
|
||
if (digitalSignalDo1.Value == 1)
|
||
{
|
||
Extract();
|
||
digitalSignalDo1.Value = 0;
|
||
digitalSignalDo3.Value = 0;
|
||
digitalSignalDo0.Value = 0;
|
||
form.ImageProcessing1 = false;
|
||
// 暫停執行緒
|
||
pauseEvent.Reset();
|
||
break;
|
||
}
|
||
}
|
||
});
|
||
// 啟動執行緒
|
||
captureThread1.Start();
|
||
// 恢復執行緒
|
||
pauseEvent.Set();
|
||
}
|
||
[Obsolete]
|
||
public void Extract()
|
||
{
|
||
// 訂閱 NewFrameArrived 事件
|
||
NewFrameArrived11 += Comparesamples_NewFrameArrived;
|
||
}
|
||
[Obsolete]
|
||
public async void Comparesamples_NewFrameArrived(VideoFrame colorFrame, DepthFrame depthFrame)
|
||
{
|
||
form.pictureBox6.Image = null;
|
||
ConvertDepthToPointCloud(depthFrame);
|
||
|
||
byte[] colorData1 = new byte[colorFrame.Width * colorFrame.Height * colorFrame.BitsPerPixel];
|
||
colorFrame.CopyTo(colorData1);
|
||
|
||
ushort[] depthData1 = new ushort[depthFrame.Width * depthFrame.Height];
|
||
depthFrame.CopyTo(depthData1);
|
||
|
||
using (VideoFrame depth1 = colorizer.Colorize(depthFrame))
|
||
{
|
||
Image<Rgb, byte> ColorImg1 = new Image<Rgb, byte>(colorFrame.Width, colorFrame.Height, colorFrame.Stride, colorFrame.Data);
|
||
Image<Rgb, byte> DepthImg1 = new Image<Rgb, byte>(depth1.Width, depth1.Height, depth1.Stride, depth1.Data);
|
||
|
||
//Image<Rgb, byte> DepthImg2 = DepthImg1;
|
||
//DepthImg1Bitmap = DepthImg2.ToBitmap();
|
||
|
||
string colorFileName = $"Compare samples_Color.bmp";
|
||
string depthFileName = $"Compare samples_Depth.bmp";
|
||
//string YOLO = $"YOLO.bmp";
|
||
string colorPath = Path.Combine(folderPath1, colorFileName);
|
||
string depthPath = Path.Combine(folderPath1, depthFileName);
|
||
ColorImg1.Save(colorPath);
|
||
form.pictureBox6.Image = ColorImg1.Bitmap;
|
||
DepthImg1.Save(depthPath);
|
||
}
|
||
// 取得當前日期和時間
|
||
DateTime now = DateTime.Now;
|
||
if (deepestZ != float.MinValue && deepestZ - shallowestZ > (GolddeepestZ - GoldshallowestZ) / 2.5 && i > percentageInsideCrop && deepestZ - shallowestZ < GolddeepestZ - GoldshallowestZ + 2)
|
||
{
|
||
NewFrameArrived11 -= Comparesamples_NewFrameArrived;
|
||
// 以日期時間為名稱建立資料夾
|
||
folderPath1 = $"C:\\c#\\cam\\比對樣本{now:yyyy_MM_dd_HH_mm_ss}";
|
||
form.Home();
|
||
await CalculateDisplacement();
|
||
}
|
||
else
|
||
{
|
||
// 以日期時間為名稱建立資料夾
|
||
folderPath1 = $"C:\\c#\\cam\\比對樣本超過範圍{now:yyyy_MM_dd_HH_mm_ss}";
|
||
if (form.checkBox1.Checked)
|
||
{
|
||
MessageBox.Show("超過離鏡頭允許範圍，請確認物件擺放位置\n 距離誤超過樣本位置3公分\n 確保物件在ROI範圍內");
|
||
NewFrameArrived11 -= Comparesamples_NewFrameArrived;
|
||
form.button30.Enabled = true;
|
||
form.ok1 = true;
|
||
}
|
||
}
|
||
// 检查文件夹是否存在，如果不存在则创建
|
||
if (!Directory.Exists(folderPath1))
|
||
{
|
||
Directory.CreateDirectory(folderPath1);
|
||
}
|
||
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// YOLO辨識
|
||
/// </summary>
|
||
public System.Drawing.Image ProcessImage(System.Drawing.Image colorBitmap) //YOLOV5-ONNX
|
||
{
|
||
yoloFeaturePoints.Clear(); // 清空上次的特征点
|
||
|
||
// 先複製原始圖片
|
||
var image = new Bitmap(colorBitmap);
|
||
var scorer = new YoloScorer<YoloCocoP5Model>("C:/c#/20231102/exp/weights/best.onnx");
|
||
List<YoloPrediction> predictions = scorer.Predict(image);
|
||
var graphics = Graphics.FromImage(image);
|
||
|
||
// 按照 centerX + centerY 的大小排序 predictions
|
||
predictions = predictions.OrderBy(prediction => prediction.Rectangle.Left + prediction.Rectangle.Width / 2 + prediction.Rectangle.Top + prediction.Rectangle.Height / 2).ToList();
|
||
|
||
foreach (var prediction in predictions)
|
||
{
|
||
double score = Math.Round(prediction.Score, 2);
|
||
graphics.DrawRectangles(new System.Drawing.Pen(prediction.Label.Color, prediction.Label.PenWidth), new[] { prediction.Rectangle });
|
||
var (x, y) = (prediction.Rectangle.X - 3, prediction.Rectangle.Y - 37);
|
||
// 計算方寬中心點和方寬寬高
|
||
float centerX = prediction.Rectangle.Left + prediction.Rectangle.Width / 2;
|
||
float centerY = prediction.Rectangle.Top + prediction.Rectangle.Height / 2;
|
||
float width = prediction.Rectangle.Width;
|
||
float height = prediction.Rectangle.Height;
|
||
// 將物體編號、物體名稱和信心值一起顯示在標籤中
|
||
var labelWithScore = $"{objectCounter}"; //{prediction.Label.Name}
|
||
graphics.DrawString(labelWithScore,
|
||
new Font("Times New Roman", 20, GraphicsUnit.Pixel), new SolidBrush(prediction.Label.Color),
|
||
new PointF(x, y));
|
||
Item = new ListViewItem(new[]
|
||
{
|
||
objectCounter.ToString(), // 編號
|
||
prediction.Label.Name.ToString(),// 物體名稱
|
||
score.ToString(), // 信心值
|
||
(centerX,centerY).ToString(),// 方寬中心點
|
||
(height,width).ToString() // 方寬寬高
|
||
});
|
||
|
||
form.listView4.Items.Add(Item);
|
||
objectCounter++; // 增加物體编號
|
||
if (prediction.Label.Name == "solder joint") //篩選特徵點
|
||
{
|
||
yoloFeaturePoints.Add(new System.Drawing.PointF(centerX, centerY));// 將YOLO預測的特徵點座標添加到列表
|
||
}
|
||
}
|
||
form.pictureBox1.Image = image;
|
||
|
||
return new Bitmap(image); // 返回新的 Bitmap
|
||
}
|
||
/// <summary>
|
||
/// 儲存彩色和深度影像和YOLO影像
|
||
/// </summary>
|
||
/// <param name="colorImage"></param>
|
||
/// <param name="depthImage"></param>
|
||
/// <param name="yoloImage"></param>
|
||
private void SaveImages()// 儲存金樣本彩色和深度影像和YOLO影像
|
||
{
|
||
folderPath = "C:\\c#\\cam\\金樣本";
|
||
|
||
// 检查文件夹是否存在，如果不存在则创建
|
||
if (!Directory.Exists(folderPath))
|
||
{
|
||
Directory.CreateDirectory(folderPath);
|
||
}
|
||
// 在這裡加入儲存影像的程式碼，可以使用 Bitmap.Save 方法或其他方式進行儲存
|
||
string yoloName = $"Gold Standard_YoLo.bmp";
|
||
string yoloPath = Path.Combine(folderPath, yoloName);
|
||
yoloimage.Save(yoloPath);
|
||
// 釋放 yoloImage
|
||
yoloimage.Dispose();
|
||
string colorFileName = $"Gold Standard_Color.bmp";
|
||
string depthFileName = $"Gold Standard_Depth.bmp";
|
||
string colorPath = Path.Combine(folderPath, colorFileName);
|
||
string depthPath = Path.Combine(folderPath, depthFileName);
|
||
ColorImg.Save(colorPath);
|
||
DepthImg.Save(depthPath);
|
||
Console.WriteLine("影像已儲存");
|
||
}
|
||
#region 坐標系轉換
|
||
/// <summary>
|
||
/// $"YOLO特徵點({u}, {v})的三維座標: X={X}, Y={Y}, Z={Z}"
|
||
/// </summary>
|
||
[Obsolete]
|
||
public void Yolo3D()
|
||
{
|
||
storedCoordinates.Clear();
|
||
ColorData = null;
|
||
DepthData = null;
|
||
// 初始化彩色圖像和深度圖像
|
||
ColorImg = null;
|
||
DepthImg = null;
|
||
// 訂閱 NewFrameArrived 事件
|
||
NewFrameArrived += Yolo3D_NewFrameArrived;
|
||
}
|
||
[Obsolete]
|
||
private void Yolo3D_NewFrameArrived(VideoFrame colorFrame, DepthFrame depthFrame)
|
||
{
|
||
// 有興趣範圍
|
||
CroppingRect = new Rectangle(form.left, form.top, form.right - form.left, form.bottom - form.top);
|
||
|
||
ColorData = new byte[colorFrame.Width * colorFrame.Height * colorFrame.BitsPerPixel];
|
||
colorFrame.CopyTo(ColorData);
|
||
|
||
DepthData = new ushort[depthFrame.Width * depthFrame.Height];
|
||
depthFrame.CopyTo(DepthData);
|
||
|
||
|
||
using (VideoFrame depth = colorizer.Colorize(depthFrame))
|
||
{
|
||
ColorImg = new Image<Rgb, byte>(colorFrame.Width, colorFrame.Height, colorFrame.Stride, colorFrame.Data);
|
||
DepthImg = new Image<Rgb, byte>(depth.Width, depth.Height, depth.Stride, depth.Data);
|
||
Image<Rgb, byte> DepthImg1 = DepthImg;
|
||
DepthImg1Bitmap = DepthImg1.ToBitmap();
|
||
}
|
||
// 確保彩色圖丟進 YOLO 出來的特徵點是跟深度影像點位一樣的
|
||
yoloimage = (Bitmap)ProcessImage(ColorImg.Bitmap); // 丟入 YOLO
|
||
Bitmap yolo = (Bitmap)yoloimage.Clone(); // 使用 Clone 方法進行複製
|
||
using (Graphics g = Graphics.FromImage(DepthImg1Bitmap))
|
||
{
|
||
|
||
// 假設 deph 是包含深度值的集合
|
||
List<float> deph2 = new List<float>();
|
||
// 使用 yoloFeaturePoints 進行處理
|
||
foreach (System.Drawing.PointF featurePoint in yoloFeaturePoints)
|
||
{
|
||
|
||
// 假设 maxDepth 是您深度图的最大深度值
|
||
//float maxDepth = 100.0f;
|
||
|
||
//featurePoint 為彩色YOLO過的特徵點座標
|
||
int u = (int)featurePoint.X;
|
||
int v = (int)featurePoint.Y;
|
||
// 在深度图上对应像素的坐标
|
||
int depthPixelX = (int)u;
|
||
int depthPixelY = (int)v;
|
||
|
||
|
||
Z = depthFrame.GetDistance(depthPixelX, depthPixelY);
|
||
// 將深度值加入到 Z 集合中
|
||
deph2.Add(Z);
|
||
|
||
//獲取影像相機座標系
|
||
cameraPoint = Compute3DCoordinate(depthPixelX, depthPixelY, Z);
|
||
|
||
MathNet.Numerics.LinearAlgebra.Vector<double> cameraPointVector = MathNet.Numerics.LinearAlgebra.Vector<double>.Build.Dense(new double[] { cameraPoint.X, cameraPoint.Y, cameraPoint.Z });
|
||
cameraPointsList.Add(cameraPointVector);
|
||
|
||
// 输出深度值
|
||
Console.WriteLine($"YOLO特徵點 ({depthPixelX}, {depthPixelY}): {Z * 100} cm");
|
||
coordTextBuilder.AppendLine($"YOLO特徵點({u}, {v}): 深度值: {Z * 100} cm");
|
||
form.label16.Text = coordTextBuilder.ToString();
|
||
Console.WriteLine($"相機座標系: ({cameraPoint.X * 1000}, {cameraPoint.Y * 1000}, {cameraPoint.Z * 1000}) mm");
|
||
System.Numerics.Vector3 currentCoordinate = new System.Numerics.Vector3(cameraPoint.X * 1000, cameraPoint.Y * 1000, cameraPoint.Z * 1000);
|
||
storedCoordinates.Add(currentCoordinate);
|
||
}
|
||
// 找出 Z 集合中的最大值
|
||
MaxDepthValue = deph2.Max();
|
||
Console.WriteLine($"maxDepthValue {MaxDepthValue} cm");
|
||
form.label21.Text = "待焊物離鏡頭距離不超過" + (MaxDepthValue * 100 + 3) + "cm";
|
||
|
||
}
|
||
using (Graphics g = Graphics.FromImage(DepthImg1Bitmap))
|
||
{
|
||
// 使用 yoloFeaturePoints 進行處理
|
||
foreach (System.Drawing.PointF featurePoint in yoloFeaturePoints)
|
||
{
|
||
// 假设 maxDepth 是您深度图的最大深度值
|
||
float maxDepth = 100.0f;
|
||
//featurePoint 為彩色YOLO過的特徵點座標
|
||
float u = featurePoint.X;
|
||
float v = featurePoint.Y;
|
||
// 在深度图上对应像素的坐标
|
||
int depthPixelX = (int)u;
|
||
int depthPixelY = (int)v;
|
||
// 在畫布上繪製特徵中心點
|
||
float depthPointRadius = 10.0f;
|
||
g.FillEllipse(System.Drawing.Brushes.Red, Convert.ToSingle(u - depthPointRadius), Convert.ToSingle(v - depthPointRadius), Convert.ToSingle(depthPointRadius * 2), Convert.ToSingle(depthPointRadius * 2));
|
||
|
||
float rectSize = 5.0f;
|
||
// 将深度值映射到颜色值（这里是简单的映射示例，具体映射关系可能需要根据实际情况调整）
|
||
byte mappedDepthColor = (byte)(Z / maxDepth * 255);
|
||
|
||
g.FillEllipse(new SolidBrush(System.Drawing.Color.FromArgb(mappedDepthColor, mappedDepthColor, mappedDepthColor)), depthPixelX - rectSize, depthPixelY - rectSize, 2 * rectSize, 2 * rectSize);
|
||
|
||
}
|
||
}
|
||
using (Graphics g = Graphics.FromImage(yolo))
|
||
{
|
||
// 使用 yoloFeaturePoints 進行處理
|
||
foreach (System.Drawing.PointF featurePoint in yoloFeaturePoints)
|
||
{
|
||
// 假设 maxDepth 是您深度图的最大深度值
|
||
float maxDepth = 100.0f;
|
||
//featurePoint 為彩色YOLO過的特徵點座標
|
||
float u = featurePoint.X;
|
||
float v = featurePoint.Y;
|
||
// 在深度图上对应像素的坐标
|
||
int depthPixelX = (int)u;
|
||
int depthPixelY = (int)v;
|
||
// 在畫布上繪製特徵中心點
|
||
float depthPointRadius = 10.0f;
|
||
g.FillEllipse(System.Drawing.Brushes.Red, Convert.ToSingle(u - depthPointRadius), Convert.ToSingle(v - depthPointRadius), Convert.ToSingle(depthPointRadius * 2), Convert.ToSingle(depthPointRadius * 2));
|
||
// 在pictureBox1上绘制矩形表示深度图上的颜色值
|
||
float rectSize = 5.0f;
|
||
// 将深度值映射到颜色值（这里是简单的映射示例，具体映射关系可能需要根据实际情况调整）
|
||
byte mappedDepthColor = (byte)(Z / maxDepth * 255);
|
||
// 在pictureBox1上绘制矩形表示深度图上的颜色值
|
||
g.FillEllipse(new SolidBrush(System.Drawing.Color.FromArgb(mappedDepthColor, mappedDepthColor, mappedDepthColor)), depthPixelX - rectSize, depthPixelY - rectSize, 2 * rectSize, 2 * rectSize);
|
||
|
||
}
|
||
}
|
||
form.groupBox2.Enabled = true;
|
||
form.pictureBox1.Image = yolo;
|
||
form.pictureBox2.Image = ColorImg.Bitmap;
|
||
form.pictureBox3.Image = DepthImg1Bitmap;
|
||
form.pictureBox3.Refresh();
|
||
form.pictureBox3.Refresh();
|
||
form.PlanningAOI();
|
||
// 檢查是否滿足條件，如果沒有，就等待直到滿足
|
||
while (!form.ROI)
|
||
{
|
||
Thread.Sleep(10); // 每 10 毫秒檢查一次
|
||
if (form.ImageProcessing == true)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
form.goldsample = true;
|
||
//建立點雲
|
||
ConvertDepthToPointCloud(depthFrame);
|
||
form.goldsample = false;
|
||
SaveImages();
|
||
TakeImage = true;
|
||
NewFrameArrived -= Yolo3D_NewFrameArrived;
|
||
}
|
||
//建立點雲
|
||
public void ConvertDepthToPointCloud(DepthFrame depthFrame)
|
||
{
|
||
i = 0;
|
||
threshold = MaxDepthValue * 1000 + 40;
|
||
|
||
// 初始化追蹤最深點的座標和深度值
|
||
deepestZ = float.MinValue;
|
||
deepestX = 0;
|
||
deepestY = 0;
|
||
// 初始化追蹤最淺點的座標和深度值
|
||
shallowestZ = float.MaxValue;
|
||
shallowestX = 0;
|
||
shallowestY = 0;
|
||
// 初始化追蹤 y 軸最小值和最大值
|
||
float yMin = float.MaxValue;
|
||
float yMax = float.MinValue;
|
||
// 建立一個StringBuilder來儲存.pcd檔的內容
|
||
StringBuilder pcdData = new StringBuilder();
|
||
|
||
// 初始化一個列表來儲存點雲數據
|
||
List<string> pointCloudData = new List<string>();
|
||
// 取得影像中間的y座標值
|
||
|
||
// 遍歷深度數據的每一個元素
|
||
for (int y = 0; y < depthFrame.Height; y++)
|
||
{
|
||
for (int x = 0; x < depthFrame.Width; x++)
|
||
{
|
||
// 檢查點是否在裁剪範圍內
|
||
if (x >= form.left && x <= form.right && y >= form.top && y <= form.bottom)
|
||
{
|
||
// 將深度數據轉換為點雲數據
|
||
float Z2 = (float)depthFrame.GetDistance(x, y);
|
||
Z2 *= 1000;
|
||
if (Z2 < threshold && Z2 != 0)
|
||
{
|
||
float xx = (float)((x - colorIntrinsics.ppx) * Z2 / colorIntrinsics.fx);
|
||
float yy = (depthFrame.Height - y - colorIntrinsics.ppy) * Z2 / colorIntrinsics.fy; // 翻轉y軸的座標
|
||
float z = Z2;
|
||
|
||
// 更新 y 軸的最小值和最大值
|
||
if (yy < yMin) yMin = yy;
|
||
if (yy > yMax) yMax = yy;
|
||
|
||
|
||
// 將點雲數據儲存到列表中
|
||
pointCloudData.Add($"{xx} {yy} {z}");
|
||
i++;
|
||
// 根據 y 軸的範圍來定義矩形框的大小
|
||
float boxSize = (yMax - yMin) / 5; // 以五行為主
|
||
float boxMin = yMin;
|
||
float boxMax = boxMin + boxSize;
|
||
|
||
// 在定義的矩形框中找到最淺的點和最深的點
|
||
if (yy >= boxMin && yy <= boxMax)
|
||
{
|
||
// 更新最深點的座標和深度值
|
||
if (Z2 > deepestZ)
|
||
{
|
||
deepestZ = Z2;
|
||
deepestX = (int)xx;
|
||
deepestY = (int)yy;
|
||
}
|
||
// 更新最淺點的座標和深度值
|
||
if (Z2 < shallowestZ)
|
||
{
|
||
shallowestZ = Z2;
|
||
shallowestX = (int)xx;
|
||
shallowestY = (int)yy;
|
||
}
|
||
}
|
||
|
||
}
|
||
}
|
||
}
|
||
}
|
||
// 最深點的座標和深度值
|
||
Console.WriteLine($"深度限制: {threshold}");
|
||
Console.WriteLine($"最深點: X={deepestX}, Y={deepestY}, Z={deepestZ}");
|
||
Console.WriteLine($"最淺點: X={shallowestX}, Y={shallowestY}, Z={shallowestZ}");
|
||
// 寫入.pcd檔的頭部資訊
|
||
pcdData.AppendLine("VERSION .7");
|
||
pcdData.AppendLine("FIELDS x y z");
|
||
pcdData.AppendLine("SIZE 4 4 4");
|
||
pcdData.AppendLine("TYPE F F F");
|
||
pcdData.AppendLine("COUNT 1 1 1");
|
||
pcdData.AppendLine("WIDTH " + i);
|
||
pcdData.AppendLine("HEIGHT " + 1);
|
||
pcdData.AppendLine("VIEWPOINT 0 0 0 1 0 0 0");
|
||
pcdData.AppendLine("POINTS " + i);
|
||
pcdData.AppendLine("DATA ascii");
|
||
// 寫入.pcd檔時，從列表中取出點雲數據並寫入.pcd檔
|
||
foreach (string point in pointCloudData)
|
||
{
|
||
pcdData.AppendLine(point);
|
||
}
|
||
if (form.goldsample == true)
|
||
{
|
||
Goldsamplepointcloudnumber = i;
|
||
// 計算在裁剪範圍內的點的百分比
|
||
percentageInsideCrop = Goldsamplepointcloudnumber * 0.7;
|
||
GolddeepestZ = deepestZ;
|
||
GoldshallowestZ = shallowestZ;
|
||
Console.WriteLine($"金樣本點雲數量 {Goldsamplepointcloudnumber}");
|
||
// 指定儲存.pcd檔的資料夾
|
||
folderPath = "C:\\c#\\cam\\金樣本";
|
||
// 指定.pcd檔的檔名
|
||
fileName = "Gold Standard.pcd";
|
||
// 檢查資料夾是否存在，如果不存在，則建立該資料夾
|
||
if (!Directory.Exists(folderPath))
|
||
{
|
||
Directory.CreateDirectory(folderPath);
|
||
}
|
||
// 檔案將被儲存到指定的資料夾
|
||
File.WriteAllText(Path.Combine(folderPath, fileName), pcdData.ToString());
|
||
}
|
||
else
|
||
{
|
||
Console.WriteLine($"比對樣本點雲數量 {i}");
|
||
// 取得當前日期和時間
|
||
DateTime now = DateTime.Now;
|
||
if (deepestZ != float.MinValue && deepestZ - shallowestZ > (GolddeepestZ - GoldshallowestZ) / 2.5 && i > percentageInsideCrop && deepestZ - shallowestZ < GolddeepestZ - GoldshallowestZ + 2)
|
||
{
|
||
// 以日期時間為名稱建立資料夾
|
||
folderPath1 = $"C:\\c#\\cam\\比對樣本{now:yyyy_MM_dd_HH_mm_ss}";
|
||
|
||
}
|
||
else
|
||
{
|
||
// 以日期時間為名稱建立資料夾
|
||
folderPath1 = $"C:\\c#\\cam\\比對樣本超過範圍{now:yyyy_MM_dd_HH_mm_ss}";
|
||
}
|
||
// 指定.pcd檔的檔名
|
||
fileName1 = "Compare samples.pcd";
|
||
// 檢查資料夾是否存在，如果不存在，則建立該資料夾
|
||
if (!Directory.Exists(folderPath1))
|
||
{
|
||
Directory.CreateDirectory(folderPath1);
|
||
}
|
||
File.WriteAllText(Path.Combine(folderPath1, fileName1), pcdData.ToString());
|
||
}
|
||
}
|
||
/// <summary>
|
||
/// 相機座標系中的像素座標轉換成相機座標系中的三維空間座標
|
||
/// </summary>
|
||
/// <param name="u"></param>
|
||
/// <param name="v"></param>
|
||
/// <param name="Z"></param>
|
||
/// <param name="dist">DepthFrame在座標(x,y)的深度值(單位：m)</param>
|
||
/// <returns></returns>
|
||
public System.Numerics.Vector3 Compute3DCoordinate(double x, double y, double Z)
|
||
{
|
||
X = (float)((x - colorIntrinsics.ppx) * Z / colorIntrinsics.fx);
|
||
Y = (float)((y - colorIntrinsics.ppy) * Z / colorIntrinsics.fy);
|
||
// 畸變模型
|
||
if (colorIntrinsics.model == Distortion.ModifiedBrownConrady)
|
||
{
|
||
float r2 = (float)(X * X + Y * Y);
|
||
float f = 1 + colorIntrinsics.coeffs[0] * r2 + colorIntrinsics.coeffs[1] * r2 * r2 + colorIntrinsics.coeffs[4] * r2 * r2 * r2;
|
||
float ux = (float)(X * f + 2 * colorIntrinsics.coeffs[2] * X * Y + colorIntrinsics.coeffs[3] * (r2 + 2 * X * X));
|
||
float uy = (float)(Y * f + 2 * colorIntrinsics.coeffs[3] * X * Y + colorIntrinsics.coeffs[2] * (r2 + 2 * Y * Y));
|
||
X = ux;
|
||
Y = uy;
|
||
}
|
||
Console.WriteLine($"Compute3DCoordinate: {X} m ,{Y} m ,{Z} m");
|
||
return new System.Numerics.Vector3((float)X, (float)Y, (float)Z);
|
||
}
|
||
/// <summary>
|
||
/// 相機內外參參數
|
||
/// </summary>
|
||
internal void InternalAndExternalGinseng()//內參出輸
|
||
{
|
||
// 打印深度流和彩色流的内参信息
|
||
depthIntrinsics = depthStream.GetIntrinsics();
|
||
depthExtrinsics = depthStream.GetExtrinsicsTo(colorStream);
|
||
colorIntrinsics = colorStream.GetIntrinsics();
|
||
colorExtrinsics = colorStream.GetExtrinsicsTo(depthStream);
|
||
Console.WriteLine("深度相機内参:");
|
||
Console.WriteLine("寬度: " + depthIntrinsics.width);// 深度相機寬度
|
||
Console.WriteLine("高度: " + depthIntrinsics.height);// 深度相機高度
|
||
Console.WriteLine("Fx: " + depthIntrinsics.fx);// 深度相機的水平焦距
|
||
Console.WriteLine("Fy: " + depthIntrinsics.fy);// 深度相機的垂直焦距
|
||
Console.WriteLine("Cx: " + depthIntrinsics.ppx);// 深度相機的主點在水平方向上的坐標
|
||
Console.WriteLine("Cy: " + depthIntrinsics.ppy);// 深度相機的主點在垂直方向上的坐標
|
||
Console.WriteLine("模型: " + depthIntrinsics.model);// 深度相機的内參模型
|
||
|
||
Console.WriteLine("彩色相機内参:");
|
||
Console.WriteLine("寬度: " + colorIntrinsics.width);
|
||
Console.WriteLine("高度: " + colorIntrinsics.height);
|
||
Console.WriteLine("Fx: " + colorIntrinsics.fx);
|
||
Console.WriteLine("Fy: " + colorIntrinsics.fy);
|
||
Console.WriteLine("Cx: " + colorIntrinsics.ppx);
|
||
Console.WriteLine("Cy: " + colorIntrinsics.ppy);
|
||
Console.WriteLine("模型: " + colorIntrinsics.model);
|
||
}
|
||
#endregion
|
||
/// <summary>
|
||
/// 計算位移量
|
||
/// </summary>
|
||
[Obsolete]
|
||
public async Task CalculateDisplacement()
|
||
{
|
||
await Task.Run(async () =>
|
||
{
|
||
using (Process process = new Process())
|
||
{
|
||
form.cam = false;
|
||
form.cameraOpened = false;
|
||
form.label17.Text = "相機狀態:關";
|
||
form.button27.Text = "開相機";
|
||
form.label17.BackColor = Color.Transparent;
|
||
await CloseCamera(); // 關閉相機
|
||
process.StartInfo.FileName = "C:\\Users\\asasa\\Desktop\\PCLdemo\\build\\Debug\\PCLdemo.exe"; // C++程式的路徑
|
||
process.StartInfo.UseShellExecute = false;
|
||
process.StartInfo.RedirectStandardOutput = true;
|
||
process.Start();
|
||
// 讀取C++程式的輸出
|
||
string output = process.StandardOutput.ReadToEnd();
|
||
process.WaitForExit();
|
||
|
||
// 顯示C++程式的完整輸出
|
||
Console.WriteLine("C++程式的輸出為:");
|
||
Console.WriteLine(output);
|
||
|
||
// 指定輸出文件的路徑
|
||
string outputFilePath = "C:\\c#\\cam\\output.txt";
|
||
|
||
// 讀取輸出文件的整個內容
|
||
string fileContent = File.ReadAllText(outputFilePath);
|
||
|
||
// 將文件內容按逗號分割成字符串數組
|
||
string[] values = fileContent.Split(',');
|
||
|
||
// 檢查值的數量是否正確
|
||
if (values.Length == 6)
|
||
{
|
||
// 提取並使用每個值
|
||
if (float.TryParse(values[0], out x) &&
|
||
float.TryParse(values[1], out y) &&
|
||
float.TryParse(values[2], out z) &&
|
||
float.TryParse(values[3], out Rx) &&
|
||
float.TryParse(values[4], out Ry) &&
|
||
float.TryParse(values[5], out Rz))
|
||
{
|
||
// 显示提取到的值
|
||
Console.WriteLine($"x: {x}, y: {y}, z: {z}, Rx: {Rx}, Ry: {Ry}, Rz: {Rz}");
|
||
if (x != 0)
|
||
{
|
||
// 嘗試關閉 C++ 進程
|
||
if (!process.HasExited)
|
||
{
|
||
process.Kill(); // 強制關閉
|
||
}
|
||
|
||
form.Pathchange();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Console.WriteLine("無法解析一個或多個值。");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Console.WriteLine("輸出格式無效。");
|
||
}
|
||
}
|
||
});
|
||
|
||
}
|
||
}
|
||
}
|
||
|