使用C#与Halcon采集Realsense深度图并获取目标点的三维数据

        在机器视觉应用中，RealSense可以作为一款低成本3D传感器起到不错的效果，如精度要求不高的3D测量，有无检测等。而机器视觉行业常用的Halcon不能通过采集助手直接获取深度图，需要通过Realsense的SDK采集，之后再转化为HObject类型。

        RealSense采集到的深度图的像素值不具有实际物理意义，真正需要获取点的三维坐标必须通过RealSense的SDK才行，这里我们可以将深度图进行封装，将它与对应的DepthFrame数据一同保存，这样就可以在通过图像处理算法得到目标点像素坐标后，通过DepthFrame来获取该点的三维坐标。这里，可以定义如下结构

public struct DepthStruct
    {
        public HObject depthImg;
        public DepthFrame depthFrame;
        Intrinsics intrinsics;
        public DepthStruct(HObject depthImg, DepthFrame depthFrame)
        {
            this.depthImg = depthImg.Clone();
            this.depthFrame = depthFrame.Clone().As<DepthFrame>();
            intrinsics = depthFrame.Profile.As<VideoStreamProfile>().GetIntrinsics();

        }
        /// <summary>
        /// 根据深度图坐标，获取世界坐标
        /// </summary>
        /// <param name="r"></param>
        /// <param name="c"></param>
        /// <returns></returns>
        public double[] GetXYZ(int r, int c)
        {
            return Deproject(depthFrame, c, r, intrinsics);
        }

        /// <summary>
        /// 已知z的情况下，获取x,y的世界坐标
        /// </summary>
        /// <param name="r"></param>
        /// <param name="c"></param>
        /// <param name="z"></param>
        /// <returns></returns>
        public double[] GetXY(int r, int c, double z)
        {
            return Deproject(depthFrame, c, r, z, intrinsics);
        }

        /// <summary>
        /// https://github.com/IntelRealSense/librealsense/issues/4253
        /// 参考这个
        /// </summary>
        /// <param name="depth"></param>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="intrinsics"></param>
        /// <returns></returns>
        double[] Deproject(DepthFrame depth, int x, int y, Intrinsics intrinsics)
        {
            float z = depth.GetDistance(x, y);
            double[] r = new double[3];


            r[0] = z * (x - intrinsics.ppx) / intrinsics.fx;
            r[1] = z * (y - intrinsics.ppy) / intrinsics.fy;
            r[2] = z;

            return r;

        }

        double[] Deproject(DepthFrame depth, int x, int y, double z, Intrinsics intrinsics)
        {
            double[] r = new double[3];


            r[0] = z * (x - intrinsics.ppx) / intrinsics.fx;
            r[1] = z * (y - intrinsics.ppy) / intrinsics.fy;
            r[2] = z;

            return r;

        }
    }

    之后，我们可以如下定义Realsense类，来抓取深度图，在处理深度图结束后，调用我们自定义的DepthStruct.GetXYZ()来获取目标点的三维坐标。需要注意一点，如果你的目标点在深度图中处于空洞的位置，需要根据周围深度数据推测出你目标点的z值，之后调用GetXY（）来获取三维坐标。

public class RealsenseSR305
    {
        public static Context ctx = new Context();
        public bool isConnected = false;
        public Pipeline pipe = new Pipeline();
        string m_serialNum;

        public RealsenseSR305(string serialNum)
        {
            m_serialNum = serialNum;
            //设置参数
            //var cfg = new Config();
            //cfg.EnableDevice(serialNum);
            //cfg.EnableStream(Stream.Depth, 640, 240, Format.Z16, 10);
            //pipe.Start(cfg);
            
            isConnected = true;
        }


        public void Stop()
        {
            pipe.Stop();
        }

        public void Start()
        {
            var cfg = new Config();
            cfg.EnableDevice(m_serialNum);
            cfg.EnableStream(Stream.Depth, 640, 240, Format.Z16, 10);
            pipe.Start(cfg);
        }

        public RealsenseSR305()
        {

            isConnected = false;
        }

        public DepthStruct Grab()
        {

                HObject img;
                using (FrameSet frames = pipe.WaitForFrames(500))
                {
                    using (DepthFrame depth = frames.DepthFrame)
                    {
                        unsafe
                        {
                            HOperatorSet.GenImage1(out img, "uint2", 640, 240, (IntPtr)depth.Data.ToPointer());
                            DepthStruct dr = new DepthStruct(img, depth);
                            return dr;
                        }

                    }
                }











        }




        /// <summary>
        /// 列举设备号
        /// </summary>
        /// <returns></returns>
        static public List<string> QueryDevices()
        {
            
            var list = ctx.QueryDevices();
            if (list.Count == 0)
            {
                return new List<string>();
            }
            else
            {
                return list.Select(x => x.Info.GetInfo(CameraInfo.SerialNumber)).ToList();
            }
        }



        /// <summary>
        /// rs2_deproject_pixel_to_point没有没C#库包含进来，但很容易实现，如下
        /// </summary>
        /// <param name="depth"></param>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="intrinsics"></param>
        /// <returns></returns>


    }