实践篇 | 机器人单目相机测距的实验
众志成城 抗击肺炎
之前在做一个单目测距的小项目，大概需要就是用单目相机，对一个特定的目标进行识别并测算相机与该目标的距离。所以便去网上找了一堆教程，这里给大家总结一下，希望给小白们一个参考。首先是基本需求：
-
opencv自然要会的，这咱就不多说了，会一点就行 -
需要一个摄像头，我用的是一个畸变很大的鱼眼免驱动摄像头，大家用电脑上的那个自带摄像头也可以的，就是不方便。 -
需要MATLAB进行相机标定
-
需要一个编程环境，VS2017，至于VS怎么配置opencv，可以参看一些相关的CSDN博文。
网上的方法大概有两种，这里主要介绍一个我身边的大哥们都称做PnP问题的一个方法，但会另外简单介绍两个比较简单粗暴的，原理可行但其实效果不佳的方法。
在用相机进行单目测距时，需要用到一个叫相机内参的东西，而这需要靠相机标定来得到。这些大概要从相机模型说起，相机模型是每个学opencv的同学早晚的要接触到的吧！
我们高中都做过小孔成像的实验，小孔相机模型就是最简单通用的一种相机模型，这个模型我们就用下面一个图带过好了：
上式中我们引入了两个不同的焦距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 <opencv2/opencv.hpp>
#include <opencv2\highgui\highgui.hpp>
#include <iostream>
#include <stdio.h>
using namespace std;
using namespace cv;
const int imageWidth = 640; //定义图片大小，即摄像头的分辨率
const int imageHeight = 480;
Size imageSize = Size(imageWidth, imageHeight);
Mat mapx, mapy;
// 相机内参
Mat cameraMatrix = (Mat_<double>(3, 3) << 273.4985, 0, 321.2298,
0, 273.3338, 239.7912,
0, 0, 1);
// 相机外参
Mat distCoeff = (Mat_<double>(1, 4) << -0.3551, 0.1386,0, 0);
Mat R = Mat::eye(3, 3, CV_32F);
VideoCapture cap1; //打开摄像头
void img_init(void) //初始化摄像头
{
cap1.set(CAP_PROP_FOURCC, 'GPJM');
cap1.set(CAP_PROP_FRAME_WIDTH, imageWidth);
cap1.set(CAP_PROP_FRAME_HEIGHT, imageHeight);
}
int main()
{
initUndistortRectifyMap(cameraMatrix, distCoeff, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);
Mat frame;
img_init();
while (1)
{
cap1>>frame;
imshow("原鱼眼摄像头图像",frame);
remap(frame,frame,mapx,mapy, INTER_LINEAR);
imshow("消畸变后",frame);
waitKey(30);
}
return 0;
}
上面源码中我们在32行和39行有两个函数，就是opencv提供给我们进行消畸变的函数。
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轴偏移量，一共四个未知量，需要至少列四个式子才可以求解。
第二个参数，我们在第一个参数中任意找的物体上的四个点在图像中的像素坐标。
现在就很清楚明白了吧？通过旋转向量和平移向量就可以得到相机坐标系相对于世界坐标系的旋转参数与平移情况。
不过我们还要解决一个问题，如何确保这四个点的位置呢？就是，例如物体是一个正方形板子，板子长为2L，我可以选板子中心作为世界坐标系的中心，那么我可以得到板子四个角上的坐标分别为(L,L),(L,-L),(-L,L),(-L,-L)。但如何确定图像上哪四个点是板子的四个角呢？你就需要把板子识别出来。但如果不是个板子是个人呢？你怎么把人分出来？这就需要更复杂的东西了，什么语义分割啊分类器啊啥的，这里就不多说了。
那我不取板子的四个角，利用角点检测任意取四个点也可以，这就解决了世界坐标系与像素坐标系之间的对应问题，但又有一个新问题，如何确保这四个角点是物体身上的而不是背景上的呢？
所以说这么多，我们便引入了二维码，我们可以直接识别二维码来测距，这儿就要用到一个叫ZBar库的东西了，它是一个可以识别二维码或条形码的函数库，具体的自行百度吧。那我们还需要学一个新库？opencv库都还没学明白呢，又要学一个识别二维码的？其实不需要，这个库的两个例程已经可以满足我们的需要了：
#include <zbar.h>
#include <opencv2\opencv.hpp>
#include <iostream>
int main(int argc, char*argv[])
{
zbar::ImageScanner scanner;
scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
cv::VideoCapture capture;
capture.open(0); //打开摄像头
cv::Mat image;
cv::Mat imageGray;
std::vector<cv::Point2f> obj_location;
bool flag = true;
if (!capture.isOpened())
{
std::cout << "cannot open cam!" << std::endl;
}
else
{
while (flag)
{
capture >> image;
cv::cvtColor(image, imageGray, CV_RGB2GRAY);
int width = imageGray.cols;
int height = imageGray.rows;
uchar *raw = (uchar *)imageGray.data;
zbar::Image imageZbar(width, height, "Y800", raw, width * height);
scanner.scan(imageZbar); //扫描条码
zbar::Image::SymbolIterator symbol = imageZbar.symbol_begin();
if (imageZbar.symbol_begin() != imageZbar.symbol_end()) //如果扫描到二维码
{
flag = false;
//解析二维码
for (int i = 0; i < symbol->get_location_size(); i++)
{
obj_location.push_back(cv::Point(symbol->get_location_x(i), symbol->get_location_y(i)));
}
for (int i = 0; i < obj_location.size(); i++)
{
cv::line(image, obj_location[i], obj_location[(i + 1) % obj_location.size()], cv::Scalar(255, 0, 0), 3);//定位条码
}
for (; symbol != imageZbar.symbol_end(); ++symbol)
{
std::cout << "Code Type: " << std::endl << symbol->get_type_name() << std::endl; //获取条码类型
std::cout << "Decode Result: " << std::endl << symbol->get_data() << std::endl; //解码
}
imageZbar.set_data(NULL, 0);
}
cv::imshow("Result", image);
cv::waitKey(50);
}
cv::waitKey();
}
return 0;
}
这个函数可以实现打开摄像头，并识别看到的二维码，进而打印二维码的类型和内容：
#include <opencv2/opencv.hpp>
#include <zbar.h>
using namespace cv;
using namespace std;
using namespace zbar;
typedef struct
{
string type;
string data;
vector <Point> location;
} decodedObject;
// Find and decode barcodes and QR codes
void decode(Mat &im, vector<decodedObject>&decodedObjects)
{
// Create zbar scanner
ImageScanner scanner;
// Configure scanner
scanner.set_config(ZBAR_NONE, ZBAR_CFG_ENABLE, 1);
// Convert image to grayscale
Mat imGray;
cvtColor(im, imGray,COLOR_BGR2GRAY);
// Wrap image data in a zbar image
Image image(im.cols, im.rows, "Y800", (uchar *)imGray.data, im.cols * im.rows);
// Scan the image for barcodes and QRCodes
int n = scanner.scan(image);
// Print results
for(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);
}
}
// Display barcode and QR code location
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;
// 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);
}
}
// Display results
imshow("Results", im);
waitKey(0);
}
int main(int argc, char* argv[])
{
// Read image
Mat im = imread("zbar-test.jpg");
// Variable for decoded objects
vector<decodedObject> decodedObjects;
// Find and decode barcodes and QR codes
decode(im, decodedObjects);
// Display location
display(im, decodedObjects);
return EXIT_SUCCESS;
}
下面，如何实现测距代码编写呢？我们需要在上面例程二这个代码的基础上，加上相机畸变矫正的代码，还要加上一段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 <iostream>
#include <opencv2/opencv.hpp>
#include <zbar.h>
using namespace cv;
using namespace std;
#define HALF_LENGTH 15 //二维码宽度的二分之一
const int imageWidth = 640; //设置图片大小，即摄像头的分辨率
const int imageHeight = 480;
Size imageSize = Size(imageWidth, imageHeight);
Mat mapx, mapy;
// 相机内参
Mat cameraMatrix = (Mat_<double>(3, 3) << 273.4985, 0, 321.2298,
0, 273.3338, 239.7912,
0, 0, 1);
// 相机外参
Mat distCoeff = (Mat_<double>(1, 4) << -0.3551, 0.1386, 0, 0);
Mat R = Mat::eye(3, 3, CV_32F);
VideoCapture cap1;
typedef struct //定义一个二维码对象的结构体
{
string type;
string data;
vector <Point> location;
} decodedObject;
void img_init(void);
void decode(Mat &im, vector<decodedObject>&decodedObjects);
void display(Mat &im, vector<decodedObject>&decodedObjects);
int main(int argc, char* argv[])
{
initUndistortRectifyMap(cameraMatrix, distCoeff, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);
img_init();
namedWindow("yuantu", WINDOW_AUTOSIZE);
Mat im;
while (waitKey(1) != 'q') {
cap1 >> im;
if (im.empty()) break;
remap(im, im, mapx, mapy, INTER_LINEAR);//畸变矫正
imshow("yuantu", im);
// 已解码对象的变量
vector<decodedObject> decodedObjects;
// 找到并解码条形码和二维码
decode(im, decodedObjects);
// 显示位置
display(im, decodedObjects);
//vector<Point> 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;
}
}
三角测距法就是基于这个理想的，简单的模型，进行的，在知道物体大小，透镜焦距F，并测出图像中的物体长度后，就可以基于下面公式进行计算长度Z了。
这个方法是玩openmv时知道的，openmv封装的单目测距算法，就是将目标对象先在固定的距离（10cm）拍一张照片，测出照片中该物体的像素面积。得到一个比例系数K，然后将物体挪到任意位置，就可以根据像素面积估算距离了。
相关内容
微信扫码咨询专知VIP会员