RealSense D415 捕捉乒乓球运动

刚好周末415到货了，今天试了试。

做了一个乒乓球捕捉的demo，左边是三维点云（把乒乓球以外的都变成了白色），右边上面是识别出来的球心坐标和球半径，右下是RGB挖出来的乒乓球图像（BGR格式）

---

程序框架借用了Intel给的Demo中的pointcloud工程，
先要做深度相机和RGB相机的align

frames = align_to_color.process(frames);

然后在RGB图像中用颜射识别找出乒乓球

// convert realsense data to opencv data
Mat image(Size(w, h), CV_8UC3, (void*)color.get_data(), Mat::AUTO_STEP);
// convert BGR data to HSV 
Mat imgHSV;
cvtColor(image, imgHSV, CV_BGR2HSV);
// track color with threshold in HSV
Mat imgOrange;
inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgOrange);
// morphology open to remove noise points
Mat element = getStructuringElement(MORPH_RECT, Size(11, 11));
morphologyEx(imgOrange, imgOrange, MORPH_OPEN, element);
// mask
image.setTo(255, ~imgOrange);
//image.copyTo(image, imgOrange);
imshow("color", image);

由此也就可以从深度相机得到乒乓球的点云，但是由于噪声和识别误差的存在，点云中有一些坏点需要剔除

bool mCompare(xyd_t a, xyd_t b)
{
	return a.d < b.d;
}

// given the x,y and depth infos, find the sphere center
std::vector<xyd_t> throwBadData(std::vector<xyd_t> xyds)
{
	// get rid of bad data
	std::sort(xyds.begin(), xyds.end(), mCompare);

	float median = xyds[xyds.size() / 2].d;

	auto iter = xyds.begin();
	while (iter != xyds.end())
	{
		if (((*iter).d > median + 0.05) || ((*iter).d < median - 0.05))
			iter = xyds.erase(iter);
		else
			iter++;
	}
	return xyds;
}

对于剩下的点要进行球面拟合以求出乒乓球的球心位置，但是此时的x，y坐标还是图像的像素值，需要转化为真实世界的坐标，好在intel的工程师对相机已经进行了标定，所有的参数都存在相机里面，通过下面语句调用

rs2_intrinsics intr = frames.get_profile().as<rs2::video_stream_profile>().get_intrinsics(); // Calibration data

有了相机参数，就可以通过相机的成像模型推导出像素对应的坐标，不过在sdk中已经很贴心的有可以直接用的函数了

#include <librealsense2/rsutil.h>
rs2_deproject_pixel_to_point(upoint, &intr, upixel, udist); // 用法可以参考measure那个demo

得到了所有点的真是坐标之后，就可以对球面进行拟合了，可以用最小二乘，也可以用RANSAC。（还有其他一些神奇的方法）
我这次用了参阅了大神@liyuanbhu的最小二乘算法：
https://blog.youkuaiyun.com/liyuanbhu/article/details/80201371

最后测试了一下，距离的精度在据相机50cm左右可以达到2~3mm, 1m距离估计在3-4mm。