單目相機測距
單目測距的小項目，大概需要就是用單目相機，對一個特定的目標進行識別並測算相機與該目標的距離。所以便去網上找了一堆教程，這里給大家總結一下，希望給小白們一個參考。
首先是基本需求了
- opencv自然要會的，這咱就不多說了，會一點就行
- 需要一個攝像頭，我用的是一個畸變很大的魚眼免驅動攝像頭，大家用電腦上的那個自帶攝像頭也可以的，就是不方便。
- 需要MATLAB進行相機標定
其實上面都是廢話，下面進入正題吧。
網上的方法大概有兩種，這里主要介紹一個我身邊的大哥們都稱做PnP問題的一個方法，但會另外簡單介紹兩個比較簡單粗暴的，原理可行但其實效果不佳的方法。
相機畸變矯正
在用相機進行單目測距時，需要用到一個叫相機內參的東西，而這需要靠相機標定來得到。這些大概要從相機模型說起了：
相機模型是每個學opencv的同學早晚的要接觸到的吧！
做過小孔成像的實驗，小孔相機模型就是最簡單通用的一種相機模型，這個模型我們就用下面一個圖帶過好了：
其中f為我們熟知的相機參數——焦距，而光軸與成像平面的交點稱為主點，X表示箭頭長度，Z是相機到箭頭的距離。在上圖這個簡單且理想的小孔成像"相機"中，我們可以輕松的寫出黃色箭頭在現實世界坐標系與成像平面坐標系之間的轉換關系：
但是在實際相機中，成像平面就是相機感光芯片，針孔就是透鏡，然而主點卻並不再在成像平面的中心了（也就是透鏡光軸與感光芯片中心並不在一條線上了），因為在實際制作中我們是無法做到將相機里面的成像裝置以微米級別的精度進行安裝的，因此我們需要引入兩個新的參數Cx和Cy，來對我們硬件的偏移進行矯正：
上式中我們引入了兩個不同的焦距fx和fy，這是因為單個像素在低價成像裝置上是矩形而不是正方形。其中，fx是透鏡的物理焦距長度與成像裝置的每個單元尺寸Sx的乘積。
通過上式我們可以知道相機內參的四個參數了，分別是fx,fy,Cx,Cy。但在計算中，我們常通過一些數學技巧來進行一定的變換，從而得到下式：
其中：
通過上面的式子，我們可以將空間中的點和圖片中的點一一對應起來。式中的矩陣M就是我們常聽說的相機內參矩陣了。
相機外參
而有相機內參，就有相機外參了，相機外參來源於相機自身的畸變，畸變可以分為徑向畸變（有透鏡的形狀造成）和切向畸變（由整個相機自身的安裝過程造成）。
鏡像畸變是由凸透鏡本身形狀引起的，好的透鏡，經過一些精密處理，畸變並不明顯，但在普通網絡相機上畸變顯得特別突出。我們可以把畸變看作r=0附近的泰勒奇數展開的前幾項來便是。一般為前兩項 k1 , k2，對於魚眼透鏡 ，會用前三項 k3 。成像裝置上某點的徑向位置可以根據以下等式進行調整，這時我們便有了3個或2個的未知變量：
至此，我們得到了共五個參數：K1 K2 K3 P1 P2 ，這五個參數是我們消除畸變所必須的，稱為畸變向量，也叫相機外參。
切向畸變是由於制造上的缺陷使透鏡不與成像平面平行而產生的。切向畸變可以用兩個參數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提供給我們進行消畸變的函數。
使用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);
#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提供給我們進行消畸變的函數。
使用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.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;}
這個函數可以實現打開攝像頭，並識別看到的二維碼，進而打印二維碼的類型和內容：
所以這個ZBar庫需要怎么配置到我們的VS2017上並和opencv庫一起使用呢？大家可以參看我的CSDN博文《Win10+VS2017+opencv410+ZBar庫完美配置》：
https://blog.csdn.net/qq_43667130/article/details/104128684
例程二：
#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 codesvoid 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 imageImage image(im.cols, im.rows, "Y800", (uchar *)imGray.data, im.cols * im.rows);
// Scan the image for barcodes and QRCodesint n = scanner.scan(image);
// Print resultsfor(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 datacout << "Type : " << obj.type << endl;cout << "Data : " << obj.data << endl << endl;
// Obtain locationfor(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 objectsfor(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 hullif(points.size() > 4) convexHull(points, hull);else hull = points;
// Number of points in the convex hullint 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 resultsfor (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，然后將物體挪到任意位置，就可以根據像素面積估算距離了。
不過這兩種方法肯定魯棒性都不咋樣。