【RoboMaster】我是这样搞定第一次单目相机测距的
之前在做一个单目测距的小项目，大概需要就是用单目相机，对一个特定的目标进行识别并测算相机与该目标的距离。所以便去网上找了一堆教程，这里给大家总结一下，希望给小白们一个参考。
首先是基本需求了：
- opencv自然要会的，这咱就不多说了，会一点就行
- 需要一个摄像头，我用的是一个畸变很大的鱼眼免驱动摄像头，大家用电脑上的那个自带摄像头也可以的，就是不方便。
- 需要MATLAB进行相机标定
- 需要一个编程环境，我的是VS2017，至于VS怎么配置opencv，可以参看CSDN博文《VS2017配置Opencv4.10教程》：
https://blog.csdn.net/qq_43667130/article/details/104127798
其实上面都是废话，下面进入正题吧。
网上的方法大概有两种，这里主要介绍一个我身边的大哥们都称做PnP问题的一个方法，但会另外简单介绍两个比较简单粗暴的，原理可行但其实效果不佳的方法。
相机畸变矫正
在用相机进行单目测距时，需要用到一个叫相机内参的东西，而这需要靠相机标定来得到。这些大概要从相机模型说起了：
相机模型是每个学opencv的同学早晚的要接触到的吧！
我们高中都做过小孔成像的实验，小孔相机模型就是最简单通用的一种相机模型，这个模型我们就用下面一个图带过好了：
其中f为我们熟知的相机参数——焦距，而光轴与成像平面的交点称为主点，X表示箭头长度，Z是相机到箭头的距离。在上图这个简单且理想的小孔成像"相机"中，我们可以轻松的写出黄色箭头在现实世界坐标系与成像平面坐标系之间的转换关系：
但是在实际相机中，成像平面就是相机感光芯片，针孔就是透镜，然而主点却并不再在成像平面的中心了（也就是透镜光轴与感光芯片中心并不在一条线上了），因为在实际制作中我们是无法做到将相机里面的成像装置以微米级别的精度进行安装的，因此我们需要引入两个新的参数Cx和Cy，来对我们硬件的偏移进行矫正：
上式中我们引入了两个不同的焦距fx和fy，这是因为单个像素在低价成像装置上是矩形而不是正方形。其中，fx是透镜的物理焦距长度与成像装置的每个单元尺寸Sx的乘积。
通过上式我们可以知道相机内参的四个参数了，分别是fx,fy,Cx,Cy。但在计算中，我们常通过一些数学技巧来进行一定的变换，从而得到下式：
其中：
通过上面的式子，我们可以将空间中的点和图片中的点一一对应起来。式中的矩阵M就是我们常听说的相机内参矩阵了。
相机外参
而有相机内参，就有相机外参了，相机外参来源于相机自身的畸变，畸变可以分为径向畸变（有透镜的形状造成）和切向畸变（由整个相机自身的安装过程造成）。
镜像畸变是由凸透镜本身形状引起的，好的透镜，经过一些精密处理，畸变并不明显，但在普通网络相机上畸变显得特别突出。我们可以把畸变看作r=0附近的泰勒奇数展开的前几项来便是。一般为前两项 k1 , k2，对于鱼眼透镜 ，会用前三项 k3 。成像装置上某点的径向位置可以根据以下等式进行调整，这时我们便有了3个或2个的未知变量：
这里（x,y）是成像装置上畸变点的原始位置，（Xcorrected,Ycorrected）是矫正后的新位置。
切向畸变是由于制造上的缺陷使透镜不与成像平面平行而产生的。切向畸变可以用两个参数p1 和 p2 来表示：
至此，我们得到了共五个参数：K1 K2 K3 P1 P2 ，这五个参数是我们消除畸变所必须的，称为畸变向量，也叫相机外参。
相机标定
在上文，相机内参加上相机外参一共有至少8个参数，而我们要想消除相机的畸变，就要靠相机标定来求解这8个未知参数。
说完相机模型，又要说一下相机标定了，相机标定是为了求解上面这8个参数的，那求解出这8个参数可以干什么呢？可以进行软件消除畸变，也就是在得知上面8个参数后，利用上面罗列的数学计算式，将每个偏移的像素点归位。
标定需要用到一个叫标定板的东西，有很多种类，但常用的大概就是棋盘图了，棋盘要求精度需要很高，格子是正方形，买一张标定板很贵的，在csdn上下棋盘图也要画好多c币，所以大家可以用word画一张，很简单的，只要做一个5列7行的表格，拉大到全页，再设置每个格子的宽高来将它设为正方形再涂色就可以了。这张图里有符号，但打印出来就没有了，建议大家自己画一张就OK了。
标定过程是用MATLAB进行的，过程就不在这里说了，CSDN上的教程一抓一大把，在完成标定后MATLAB会返回相机的内参和外参。关于原理，《学习oepncv3》这本书已经说的很好了，除了照着书抄我说不出什么新意，但今天，原理不懂也没有关系。
有了相机内参外参后，我们就可以进行相机消畸变了：
#include 上面源码中我们在32行和39行有两个函数，就是opencv提供给我们进行消畸变的函数。
使用cv::initUndistortRecitifyMap（）函数计算矫正映射，函数原型如下：
initUndistortRectifyMap(
InputArray cameraMaxtrix, 3*3内参矩阵
InputArray distCoeffs, 畸变系数1*4向量
InputArray R, 可以使用或者设置为noArray()。是一个旋转矩阵，将在矫正前
预先使用，来补偿相机相对于相机所处的全局坐标系的旋转。
InputArray newCameraMatrix, 单目成像时一般不会使用它
Size size, 输出映射的尺寸，对应于用来矫正的图像的尺寸
int m1type, 最终的映射类型，可能只为CV_32FC1 32_16SC2，对应于map1的表示类型
OutputArray map1,
OutputArray map2
);
我们只需在程序开头使用该函数计算一次矫正映射，就可以使用cv::remap（）函数将该矫正应用到视频每一帧图像。
PnP方法测距
好了到此我们对相机的那点事儿有了一点点的了解了，那什么是PnP问题呢？在有些情况下我们已经知道了相机的内在参数，因此只需要计算正在观察的对象的位置，这种情况下与一般的相机标定明显不同，但有相通之处。这种操作就叫N点透视（Perspective N-Point）或PnP问题。
bool cv::solvePnP(
cv::InputArray objectPoints, //三维点坐标矩阵,至少四个（世界坐标系）
cv::InputArray imagePoints, //该四个点在图像中的像素坐标
cv::InputArray cameraMatrix, //相机内参矩阵（9*9）
cv::InputArray distCoeffs, //相机外参矩阵（1*4）或（1*5）
cv::OutputArray rvec, //输出旋转矩阵
cv::OutputArray tvec, //输出平移矩阵
bool useExtrinsicGuess = false,
int flags = cv::SOLVEPNP_ITERATIVE
);
首先来解释一下该函数的输出是什么吧，
旋转矩阵就是一个3*1的向量，该矩阵可以表示相机相对于世界坐标系XYZ轴的3个旋转角度。
平移矩阵也是一个3维向量，可以表示相机相对于物体的XYZ轴的偏移，而这个矩阵就是我们需要求的：我们知道了相机相对于物体的位置，也就得到了距离，从而实现了测距的目的。
那输入的参数都是什么呢？相机内参和相机外参就不用说了吧。
第一个参数，是物体任意四个点在世界坐标系的三位点坐标，为什么是四个其实很好理解，我们需要求解的是一个旋转矩阵和XYZ轴偏移量，一共四个未知量，需要至少列四个式子才可以求解。
更详细的解释大家可以看一下这篇CSDN：
https://blog.csdn.net/cocoaqin/article/details/77841261
第二个参数，我们在第一个参数中任意找的物体上的四个点在图像中的像素坐标。
现在就很清楚明白了吧？通过旋转向量和平移向量就可以得到相机坐标系相对于世界坐标系的旋转参数与平移情况。
不过我们还要解决一个问题，如何确保这四个点的位置呢？就是，例如物体是一个正方形板子，板子长为2L，我可以选板子中心作为世界坐标系的中心，那么我可以得到板子四个角上的坐标分别为(L,L),(L,-L),(-L,L),(-L,-L)。但如何确定图像上哪四个点是板子的四个角呢？你就需要把板子识别出来。但如果不是个板子是个人呢？你怎么把人分出来？这就需要更复杂的东西了，什么语义分割啊分类器啊啥的，这里就不多说了。
那我不取板子的四个角，利用角点检测任意取四个点也可以，这就解决了世界坐标系与像素坐标系之间的对应问题，但又有一个新问题，如何确保这四个角点是物体身上的而不是背景上的呢？还是要把正方形识别出来。。。
所以说这么多，我们便引入了二维码，我们可以直接识别二维码来测距，这儿就要用到一个叫ZBar库的东西了，它是一个可以识别二维码或条形码的函数库，具体的自行百度吧。那我们还需要学一个新库？opencv库都还没学明白呢，又要学一个识别二维码的？其实不需要，这个库的两个例程已经可以满足我们的需要了：
例程一：
#include 这个函数可以实现打开摄像头，并识别看到的二维码，进而打印二维码的类型和内容：
所以这个ZBar库需要怎么配置到我们的VS2017上并和opencv库一起使用呢？大家可以参看我的CSDN博文：
《Win10+VS2017+opencv410+ZBar库完美配置》
例程二：
#include 该例程可以在实现例程一的功能的基础上，还可以识别出二维码的位置。
代码实现
下面，如何实现测距代码编写呢？我们需要在上面例程二这个代码的基础上，加上相机畸变矫正的代码，还要加上一段PnP函数求解的代码：
vector<Point3f> obj = vector<Point3f>{
cv::Point3f(-HALF_LENGTH, -HALF_LENGTH, 0), //tl
cv::Point3f(HALF_LENGTH, -HALF_LENGTH, 0), //tr
cv::Point3f(HALF_LENGTH, HALF_LENGTH, 0), //br
cv::Point3f(-HALF_LENGTH, HALF_LENGTH, 0) //bl
}; //自定义二维码四个点坐标
cv::Mat rVec = cv::Mat::zeros(3, 1, CV_64FC1);//init rvec
cv::Mat tVec = cv::Mat::zeros(3, 1, CV_64FC1);//init tvec
solvePnP(obj, pnts, cameraMatrix, distCoeff, rVec, tVec, false, SOLVEPNP_ITERATIVE);
把上面三个部分融合在一起，就可以写出我们的单目测距代码啦：
#include "pch.h"
#include points_xy = decodedObjects[0].location; //假设图中就一个二维码对象，将二维码四角位置取出
imshow("二维码", im);
waitKey(30);
}
return EXIT_SUCCESS;
}
void img_init(void)
{
//初始化摄像头
cap1.open(0);
cap1.set(CAP_PROP_FOURCC, 'GPJM');
cap1.set(CAP_PROP_FRAME_WIDTH, imageWidth);
cap1.set(CAP_PROP_FRAME_HEIGHT, imageHeight);
}
// 找到并解码条形码和二维码
//输入为图像
//返回为找到的条形码对象
void decode(Mat &im, vector<decodedObject>&decodedObjects)
{
// 创建zbar扫描仪
zbar::ImageScanner scanner;
// 配置扫描仪
scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
// 转换图像为灰度图灰度
Mat imGray;
cvtColor(im, imGray, COLOR_BGR2GRAY);
// 将图像数据包 装在zbar图像中
//可以参考：https://blog.csdn.net/bbdxf/article/details/79356259
zbar::Image image(im.cols, im.rows, "Y800", (uchar *)imGray.data, im.cols * im.rows);
// Scan the image for barcodes and QRCodes
//扫描图像中的条形码和qr码
int n = scanner.scan(image);
// Print results
for (zbar::Image::SymbolIterator symbol = image.symbol_begin(); symbol != image.symbol_end(); ++symbol)
{
decodedObject obj;
obj.type = symbol->get_type_name();
obj.data = symbol->get_data();
// Print type and data
//打印
//cout << "Type : " << obj.type << endl;
//cout << "Data : " << obj.data << endl << endl;
// Obtain location
//获取位置
for (int i = 0; i < symbol->get_location_size(); i++)
{
obj.location.push_back(Point(symbol->get_location_x(i), symbol->get_location_y(i)));
}
decodedObjects.push_back(obj);
}
}
// 显示位置
void display(Mat &im, vector<decodedObject>&decodedObjects)
{
// Loop over all decoded objects
//循环所有解码对象
for (int i = 0; i < decodedObjects.size(); i++)
{
vector<Point> points = decodedObjects[i].location;
vector<Point> hull;
// If the points do not form a quad, find convex hull
//如果这些点没有形成一个四边形，找到凸包
if (points.size() > 4)
convexHull(points, hull);
else
hull = points;
vector<Point2f> pnts;
// Number of points in the convex hull
//凸包中的点数
int n = hull.size();
for (int j = 0; j < n; j++)
{
line(im, hull[j], hull[(j + 1) % n], Scalar(255, 0, 0), 3);
pnts.push_back(Point2f(hull[j].x, hull[j].y));
}
vector<Point3f> obj = vector<Point3f>{
cv::Point3f(-HALF_LENGTH, -HALF_LENGTH, 0), //tl
cv::Point3f(HALF_LENGTH, -HALF_LENGTH, 0), //tr
cv::Point3f(HALF_LENGTH, HALF_LENGTH, 0), //br
cv::Point3f(-HALF_LENGTH, HALF_LENGTH, 0) //bl
}; //自定义二维码四个点坐标
cv::Mat rVec = cv::Mat::zeros(3, 1, CV_64FC1);//init rvec
cv::Mat tVec = cv::Mat::zeros(3, 1, CV_64FC1);//init tvec
solvePnP(obj, pnts, cameraMatrix, distCoeff, rVec, tVec, false, SOLVEPNP_ITERATIVE);
cout << "tvec:\n " << tVec << endl;
}
}
下图是运行结果：
三个数分别是X,Y,Z的距离了，单位cm，精度可以达到0.1cm。
三角测距法
还记得文章开头的那个小孔相机模型吗？
三角测距法就是基于这个理想的，简单的模型，进行的，在知道物体大小，透镜焦距F，并测出图像中的物体长度后，就可以基于下面公式进行计算长度Z了。
像素块测距法
这个方法是玩openmv时知道的，openmv封装的单目测距算法，就是将目标对象先在固定的距离（10cm）拍一张照片，测出照片中该物体的像素面积。得到一个比例系数K，然后将物体挪到任意位置，就可以根据像素面积估算距离了。
不过这两种方法肯定鲁棒性都不咋样。
今天就到这里啦。
