在机器视觉应用中,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>
}