如何簡要利用單鏡頭計算前方車輛距離
如果要計算前方物體的距離，會需要應用到世界座標轉換的方法，不過本文提供快速的計算方式，不考慮相機校正的部分，主要會解釋單應性矩陣的計算原理，但要精確還是要進行相機校正。如果如果想知道詳細相機校正的內容，可以參考此文章。
大致上的步驟，會利用實際世界的四個位置與其對應圖像上的四個位置去計算出單應性矩陣，也就是將像素座標轉世界座標，進而去推算前方車輛與我們車子的距離。
1.什麼是單應性矩陣？
單應性矩陣（Homography）是計算機視覺和影像處理中的一個重要概念，主要用來描述兩個圖像平面之間的變換關係。它可以應用在多種情境中，例如圖像拼接、立體視覺、相機校正以及車前距離評估等。可以用透視變換（Perspective Transform）表達大致概念。
圖一 透視示意圖
在日常生活中有一個常見的應用。在疫情情間遠距教學時，手邊不一定有複印機，這時就可以用透視變換的方法，掃描要上傳作業的作業。
圖二 日常應用示意圖
2.如何計算單應性矩陣?
計算的公式如式2-1。(u,v)是我們在實際是世界中的座標，(x,y)是圖像上的像素座標，h矩陣則是單應性矩陣。
從一般的情況分析，每一組(x,y)匹配到(u,v)，都有等式2-2成立。拆解式2-2，可以得到式2-3與式2-4。
式2-3與式2-4可以進一步變換為：
由於有八個單應性矩陣的參數要求解，而每組只能產生一對方程式，所以總共需要四組點，才夠求出單應性矩陣h的唯一解。最後可表示成式2-7。
可以用DLT(Direct Linear Transform)之類的方式求解，或者利用opencv，opencv有函式可以使用。
C++: Mat findHomography(InputArray srcPoints, InputArray dstPoints, int method=0, double ransacReprojThreshold=3, OutputArray mask=noArray() )
Python: cv2.findHomography(srcPoints, dstPoints[, method[, ransacReprojThreshold[, mask]]]) → retval, mask
3.實際的校正方式
在計算前方車輛距離時，因為是用地面上的距離去評估，所以我們使Z軸的值為一個定值。因此雖然真實世界是3D，但計算上只需要X軸與Y軸上的值。
我們以公尺(m)為單位，去測量現實世界的標定位置。如圖三所示，以圖片中間為原點，向左右延伸2公尺。前後距離以3公尺為起始位置開始標定，最後延伸到60公尺的位置，同時也是計算車前距離的最遠極限。
圖三 現實世界標定示意圖
程式
計算單應性矩陣程式：
#include <iostream>
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <fstream>
#include <string.h>
//width height
//像素座標
int image3[2][2] = {{212, 570},{1022, 633}};
int image5[2][2] = {{338, 499},{904, 539}};
int image10[2][2] = {{465, 432},{789, 453}};
int image20[2][2] = {{543, 388},{717, 399}};
int image30[2][2] = {{574, 373},{694, 381}};
int image40[2][2] = {{591, 365},{682, 371}};
int image50[2][2] = {{601, 361},{675, 367}};
//width height
//世界座標
int world3[2][2] = {{-200, 300},{200, 300}};
int world5[2][2] = {{-200, 500},{200, 500}};
int world10[2][2] = {{-200, 1000},{200, 1000}};
int world20[2][2] = {{-200, 2000},{200, 2000}};
int world30[2][2] = {{-200, 3000},{200, 3000}};
int world40[2][2] = {{-200, 4000},{200, 4000}};
int world50[2][2] = {{-200, 5000},{200, 5000}};
//單應性矩陣
cv::Mat H_3(3, 3, CV_64FC1);
cv::Mat H_inv_3(3, 3, CV_64FC1);
cv::Mat H_5(3, 3, CV_64FC1);
cv::Mat H_inv_5(3, 3, CV_64FC1);
cv::Mat H_10(3, 3, CV_64FC1);
cv::Mat H_inv_10(3, 3, CV_64FC1);
cv::Mat H_20(3, 3, CV_64FC1);
cv::Mat H_inv_20(3, 3, CV_64FC1);
cv::Mat H_30(3, 3, CV_64FC1);
cv::Mat H_inv_30(3, 3, CV_64FC1);
cv::Mat H_40(3, 3, CV_64FC1);
cv::Mat H_inv_40(3, 3, CV_64FC1);
char ctfct_3, ctfct_5, ctfct_10, ctfct_20, ctfct_30, ctfct_40, ctfct_inv_3, ctfct_inv_5, ctfct_inv_10, ctfct_inv_20, ctfct_inv_30, ctfct_inv_40;
//讀入二進制文件
int readbin(int num, bool inv) {
// 創建一個用來指向3X3 cv::Mat的指標
double* ptr;
std::string file_name;
if(!inv){
file_name = "Homography_" + std::to_string(num) + ".bin";
switch(num){
case 3:
ptr = H_3.ptr<double>();
break;
case 5:
ptr = H_5.ptr<double>();
break;
case 10:
ptr = H_10.ptr<double>();
break;
case 20:
ptr = H_20.ptr<double>();
break;
case 30:
ptr = H_30.ptr<double>();
break;
case 40:
ptr = H_40.ptr<double>();
break;
default:
break;
}
}
else{
file_name = "Homography_inv_" + std::to_string(num)+ ".bin";
switch(num){
case 3:
ptr = H_inv_3.ptr<double>();
break;
case 5:
ptr = H_inv_5.ptr<double>();
break;
case 10:
ptr = H_inv_10.ptr<double>();
break;
case 20:
ptr = H_inv_20.ptr<double>();
break;
case 30:
ptr = H_inv_30.ptr<double>();
break;
case 40:
ptr = H_inv_40.ptr<double>();
break;
default:
break;
}
}
//打開二進制文件
std::ifstream inFile(file_name, std::ios::in | std::ios::binary);
//檢查文件是否打開
if (!inFile.is_open()) {
std::cerr << "Failed to open the file." << std::endl;
return 1;
}
//獲取文件大小
inFile.seekg(0, std::ios::end);
int fileSize = inFile.tellg();
inFile.seekg(0, std::ios::beg);
//讀取數據
double* data = new double[fileSize / sizeof(double)];
inFile.read(reinterpret_cast<char*>(data), fileSize);
//關閉文件
inFile.close();
//將儲存的數據填充到cv::Mat變數中
std::memcpy(ptr, data, fileSize);
//釋放記憶體
delete[] data;
return 0;
}
//寫出二進制文件
int writeBin(std::vector<cv::Point2f> src_points,std::vector<cv::Point2f> dst_points ,std::string num ,bool inv) {
cv::Mat H = cv::findHomography(src_points, dst_points);
cv::Mat H_inv;
cv::invert(H, H_inv);
std::string file_name;
//將矩陣的數據寫入文件
const double* dataPtr ;
if(inv){
file_name = "Homography_inv_" + num + ".bin";
//打開二進制文件
std::ofstream outFile(file_name, std::ios::out | std::ios::binary);
//檢查文件是否打開
if (!outFile.is_open()) {
std::cerr << "Failed to open the file." << std::endl;
return 1;
}
dataPtr = reinterpret_cast<const double*>(H_inv.data);
outFile.write(reinterpret_cast<const char*>(dataPtr), H_inv.rows * H_inv.cols * sizeof(double));
//關閉文件
outFile.close();
}
else{
file_name = "Homography_" + num + ".bin";
//打開二進制文件
std::ofstream outFile(file_name, std::ios::out | std::ios::binary);
//檢查文件是否打開
if (!outFile.is_open()) {
std::cerr << "Failed to open the file." << std::endl;
return 1;
}
dataPtr = reinterpret_cast<const double*>(H.data);
outFile.write(reinterpret_cast<const char*>(dataPtr), H.rows * H.cols * sizeof(double));
//關閉文件
outFile.close();
}
std::cout << file_name << " has been written" << std::endl;
return 0;
}
//單應性矩陣讀入(false)與寫出(true)
void Homography(bool mode){
if(mode){
std::vector<cv::Point2f> imagePoint3 = {
cv::Point2f(image3[0][0], image3[0][1]),
cv::Point2f(image3[1][0], image3[1][1]),
cv::Point2f(image5[0][0], image5[0][1]),
cv::Point2f(image5[1][0], image5[1][1])
};
std::vector<cv::Point2f> imagePoint5 = {
cv::Point2f(image5[0][0], image5[0][1]),
cv::Point2f(image5[1][0], image5[1][1]),
cv::Point2f(image10[0][0], image10[0][1]),
cv::Point2f(image10[1][0], image10[1][1])
};
std::vector<cv::Point2f> imagePoint10 = {
cv::Point2f(image10[0][0], image10[0][1]),
cv::Point2f(image10[1][0], image10[1][1]),
cv::Point2f(image20[0][0], image20[0][1]),
cv::Point2f(image20[1][0], image20[1][1])
};
std::vector<cv::Point2f> imagePoint20 = {
cv::Point2f(image20[0][0], image20[0][1]),
cv::Point2f(image20[1][0], image20[1][1]),
cv::Point2f(image30[0][0], image30[0][1]),
cv::Point2f(image30[1][0], image30[1][1])
};
std::vector<cv::Point2f> imagePoint30 = {
cv::Point2f(image30[0][0], image30[0][1]),
cv::Point2f(image30[1][0], image30[1][1]),
cv::Point2f(image40[0][0], image40[0][1]),
cv::Point2f(image40[1][0], image40[1][1])
};
std::vector<cv::Point2f> imagePoint40 = {
cv::Point2f(image40[0][0], image40[0][1]),
cv::Point2f(image40[1][0], image40[1][1]),
cv::Point2f(image50[0][0], image50[0][1]),
cv::Point2f(image50[1][0], image50[1][1])
};
std::vector<cv::Point2f> worldPoint3 = {
cv::Point2f(world3[0][0], world3[0][1]),
cv::Point2f(world3[1][0], world3[1][1]),
cv::Point2f(world5[0][0], world5[0][1]),
cv::Point2f(world5[1][0], world5[1][1])
};
std::vector<cv::Point2f> worldPoint5 = {
cv::Point2f(world5[0][0], world5[0][1]),
cv::Point2f(world5[1][0], world5[1][1]),
cv::Point2f(world10[0][0], world10[0][1]),
cv::Point2f(world10[1][0], world10[1][1])
};
std::vector<cv::Point2f> worldPoint10 = {
cv::Point2f(world10[0][0], world10[0][1]),
cv::Point2f(world10[1][0], world10[1][1]),
cv::Point2f(world20[0][0], world20[0][1]),
cv::Point2f(world20[1][0], world20[1][1])
};
std::vector<cv::Point2f> worldPoint20 = {
cv::Point2f(world20[0][0], world20[0][1]),
cv::Point2f(world20[1][0], world20[1][1]),
cv::Point2f(world30[0][0], world30[0][1]),
cv::Point2f(world30[1][0], world30[1][1])
};
std::vector<cv::Point2f> worldPoint30 = {
cv::Point2f(world30[0][0], world30[0][1]),
cv::Point2f(world30[1][0], world30[1][1]),
cv::Point2f(world40[0][0], world40[0][1]),
cv::Point2f(world40[1][0], world40[1][1])
};
std::vector<cv::Point2f> worldPoint40 = {
cv::Point2f(world40[0][0], world40[0][1]),
cv::Point2f(world40[1][0], world40[1][1]),
cv::Point2f(world50[0][0], world50[0][1]),
cv::Point2f(world50[1][0], world50[1][1])
};
writeBin(imagePoint3,worldPoint3,"3",false);
writeBin(imagePoint3,worldPoint3,"3",true);
writeBin(imagePoint5,worldPoint5,"5",false);
writeBin(imagePoint5,worldPoint5,"5",true);
writeBin(imagePoint10,worldPoint10,"10",false);
writeBin(imagePoint10,worldPoint10,"10",true);
writeBin(imagePoint20,worldPoint20,"20",false);
writeBin(imagePoint20,worldPoint20,"20",true);
writeBin(imagePoint30,worldPoint30,"30",false);
writeBin(imagePoint30,worldPoint30,"30",true);
writeBin(imagePoint40,worldPoint40,"40",false);
writeBin(imagePoint40,worldPoint40,"40",true);
}
else{
readbin(3, false);
readbin(3, true);
readbin(5, false);
readbin(5, true);
readbin(10, false);
readbin(10, true);
readbin(20, false);
readbin(20, true);
readbin(30, false);
readbin(30, true);
readbin(40, false);
readbin(40, true);
}
}
//驗證單應性矩陣的正確性的公式
char Certification_Homography_coordinate_transform(double x,double y, double u_org, double v_org,cv::Mat H, std::string num){
cv::Mat A = (cv::Mat_<double>(3, 1) << x, y, 1.f);
cv::Mat tmp = H * A;
tmp = tmp/tmp.at<double>(2, 0);
std::string file_name = "Homography_" + num + ".bin";
std::cout << "Matrix from " << file_name << ":"<< std::endl;
std::cout << H << std::endl;
std::cout << std::endl;
if(std::fabs(u_org - tmp.at<double>(0, 0)) > 1 || std::fabs(v_org - tmp.at<double>(1, 0)) > 1 ){
printf("%lf %lf\n",tmp.at<double>(0, 0),tmp.at<double>(1, 0));
return -1;
}
else
return 0;
}
//驗證單應性矩陣的正確性
int Certification(){
ctfct_3 = Certification_Homography_coordinate_transform(image5[0][0], image5[0][1],world5[0][0], world5[0][1], H_3, "3");
ctfct_5 = Certification_Homography_coordinate_transform(image5[0][0], image5[0][1],world5[0][0], world5[0][1], H_5, "5");
ctfct_10 = Certification_Homography_coordinate_transform(image20[0][0], image20[0][1],world20[0][0], world20[0][1], H_10 ,"10");
ctfct_20 = Certification_Homography_coordinate_transform(image20[0][0], image20[0][1],world20[0][0], world20[0][1], H_20 ,"20");
ctfct_30 = Certification_Homography_coordinate_transform(image40[0][0], image40[0][1],world40[0][0], world40[0][1], H_30 ,"30");
ctfct_40 = Certification_Homography_coordinate_transform(image40[0][0], image40[0][1],world40[0][0], world40[0][1], H_40 ,"40");
ctfct_inv_3 = Certification_Homography_coordinate_transform(world5[0][0], world5[0][1],image5[0][0], image5[0][1], H_inv_3 ,"inv_3");
ctfct_inv_5 = Certification_Homography_coordinate_transform(world5[0][0], world5[0][1],image5[0][0], image5[0][1], H_inv_5 ,"inv_5");
ctfct_inv_10 = Certification_Homography_coordinate_transform(world20[0][0], world20[0][1],image20[0][0], image20[0][1], H_inv_10 ,"inv_10");
ctfct_inv_20 = Certification_Homography_coordinate_transform(world20[0][0], world20[0][1],image20[0][0], image20[0][1], H_inv_20 ,"inv_20");
ctfct_inv_30 = Certification_Homography_coordinate_transform(world40[0][0], world40[0][1],image40[0][0], image40[0][1], H_inv_30 ,"inv_30");
ctfct_inv_40 = Certification_Homography_coordinate_transform(world40[0][0], world40[0][1],image40[0][0], image40[0][1], H_inv_40 ,"inv_40");
//printf("%d\n",ctfct_3);
if(ctfct_3 != 0){
printf("H_3 wrong!\n");
return -1;
}
else if(ctfct_5 != 0){
printf("H_5 wrong!\n");
return -1;
}
else if(ctfct_10 !=0){
printf("H_10 wrong!\n");
return -1;
}
else if(ctfct_20 != 0){
printf("H_20 wrong!\n");
return -1;
}
else if(ctfct_30 != 0){
printf("H_30 wrong!\n");
return -1;
}
else if(ctfct_40 != 0){
printf("H_40 wrong!\n");
return -1;
}
else if(ctfct_inv_3 != 0){
printf("H_inv_3 wrong!\n");
return -1;
}
else if(ctfct_inv_5 != 0){
printf("H_inv_5 wrong!\n");
return -1;
}
else if(ctfct_inv_10 != 0){
printf("H_inv_10 wrong!\n");
return -1;
}
else if(ctfct_inv_20 != 0){
printf("H_inv_20 wrong!\n");
return -1;
}
else if(ctfct_inv_30 != 0){
printf("H_inv_30 wrong!\n");
return -1;
}
else if(ctfct_inv_40 != 0){
printf("H_inv_40 wrong!\n");
return -1;
}
else{
printf("Certification OK!!!\n");
}
return 0;
}
void Homography_coordinate_transform(double srcX,double srcY, double &dstX, double &dstY,cv::Mat H){
cv::Mat A = (cv::Mat_<double>(3, 1) << srcX, srcY, 1.f);
cv::Mat tmp = H * A;
tmp = tmp/tmp.at<double>(2, 0);
dstX = tmp.at<double>(0, 0);
dstY = tmp.at<double>(1, 0);
}
int main(int argc, char **argv) {
if(argc < 2){
printf("you need to selet mode, '-c' or '-C'\n");
return -1;
}
//計算單應性矩陣並寫出
if (std::string(argv[1]) == "-w"){
Homography(true);
return 0;
}
//驗證單應性矩陣的正確性
else if (std::string(argv[1]) == "-C"){
//讀取單應性矩陣數據
Homography(false);
//驗證單應性矩陣
Certification();
return 0;
}
else{
printf("'-d' or '-w' or '-C' \n");
return -1;
}
return 0;
}
總的來說，在現實世界標定四個點之後，比對圖像上的像素座標，計算出單應性矩陣，之後就可以用這組參數去換算距離。之後利用YOLO v8之類的物件檢測模型，去檢測前方物件，以物件框最下方中間位置為像素座標，最後轉換成距離。如圖四所示。
圖四 距離計算事例
後記
剛好有機會要教實驗室的學弟，相關的原理，一時興起就整理整理成一篇文章。有空應該會再補上，精確的求法。
參考資料
鐘竣耀（2022）。應用於汽車遠距離物件辨識之超輕量深度學習網路架構設計與實現。﹝碩士論文。國立雲林科技大學﹞臺灣博碩士論文知識加值系統。 https://hdl.handle.net/11296/ewk4cc。
分享至
成為作者繼續創作的動力吧！
作者的相關文章
黃賢的沙龍 的其他內容
你可能也想看
- 「天天秋嗨嗨」：vocus 秋季徵文，五大主題 & 獎品登場！
- 美國大選『終局之戰』（Endgame）即將展開， 美股將迎來新世紀？（上篇：模型推導寶座花落誰家？）
- 如果這個世界有神，那祂一定姓李名相赫｜矮袋鼠律師
- [概念]藥品中不純物要如何檢測?定量(Quantitative test)跟限度試驗(Limit test)有什麼差異?
- 台灣人該如何「減藥」？長期吃藥、吃保健食品的人生，是真正的健康嗎？
- 在年底前脫單！如何系統化地找對象？找尋理想情人的3要點，簡單的必勝關鍵心態，讓你輕鬆脫魯！（領取免費懶人包）
- 【如何有效準備一場簡報？知道題目後要先搞清楚的三件事】
- 極簡生活問答 # 12：沒自信、沒能力，看著存款減少，要如何堅持創業？
- 一再出包的簡訊實聯制要如何要求人民配合
- 胃藥可怕副作用？吃到胃癌、失智、胃壞掉的5個方法！減藥師告訴你如何成功減少胃藥。
- 《我要準時下班》論職場議題──如何減緩職場婦女育兒心理壓力？ 打造友善職場環境是重點
- “盆栽要剪，女人要扁”？當仇女成為顯學，作為正常人，我們該如何自處？
- 當武漢肺炎來襲，要如何應對審查大人對檢視中國曝險部位的要求
發表回應
成為會員 後即可發表留言
© 2024 vocus All rights reserved.