球體的雙目視覺定位（matlab，附程式碼）
阿新 • • 發佈：2019-07-02
球體的雙目視覺定位（matlab，附程式碼）
標籤（空格分隔）： 機器視覺
引言
雙目視覺定位是我們的一個課程設計，最近剛做完，拿出來與大家分享一下，實驗的目的是在拍攝的照片中識別球體，並求出該球體到相機的實際距離嗎，我們要求需要用matlab，但是matlab呼叫雙目攝像頭（一個USB口）卻老是隻能呼叫雙目攝像頭中的一個，但是利用Python的OpenCV庫卻可以同時呼叫兩個，因此我們選用了Python用於拍攝圖片。
1.基本流程
備註：因為得出的視差圖識別出圓球略困難，我們沒有采用視差圖深度圖去得出球體的深度，而是直接利用校正後的圖片畫素差，代入公式直接求出距離。
1.相機標定
首先我們要知道在攝像機模型中，一般要涉及四種座標系：世界座標系、攝像機座標系、影象座標系、畫素座標系。相機標定的作用可以理解為找到一種將世界座標系和影象座標系之間的變換關係。
- 1.畫素座標系（u,v）:
顧名思義，每一點都是一個畫素，原點通常設定在影象的左上角處，點的橫縱座標分別代表相應的畫素在影象中位於第幾行第幾列；- 2.影象座標系(x,y):
將原點設定在相機光軸與影象平面的交點處，橫軸、縱軸與畫素座標系的橫軸、縱軸平行； 影象平面的中心為座標原點，為了描述成像過程中物體從相機座標系到影象座標系的投影透射關係而引入，方便進一步得到畫素座標系下的座標。影象座標系是用物理單位（例如毫米）表示畫素在影象中的位置。- 3.相機座標系(x,y,z):
攝像機站在自己角度上衡量的物體的座標系。攝像機座標系的原點在攝像機的光心上，z軸與攝像機光軸平行。它是與拍攝物體發生聯絡的橋頭堡，世界座標系下的物體需先經歷剛體變化轉到攝像機座標系，然後在和影象座標系發生關係。它是影象座標與世界座標之間發生關係的紐帶，溝通了世界上最遠的距離。單位為長度單位如mm。- 4.世界座標系 (Xw,Yw,Zw):
物體在真實世界中的座標。
具體標定利用matlab的工具箱，步驟如下：
1.列印標定板：
2.拿著標定板拍照（拍照程式碼在文末），效果圖如圖：
3.將上面的照片按left，right分為兩組，放到兩個資料夾中，開啟matlab進行標定。在命令列視窗輸入stereoCameraCalibrator，出現如下介面：
4.然後將上面的“Skew”、“Tangential Distortion”以及“2 Coefficients”等選項選上，將“3 Coefficients”選項去掉，如下：
5.載入影象：
備註：標定板列印的標準一點，圖中的25是小方格邊長，方格！邊長！根據你的情況實際修改。
6.點選按鈕進行標定：
7.選中直方圖中誤差較大的一些組，（與此同時左側的圖片也會被選中），將這些刪除，然後他會自動重新。
8.覺得沒問題了，就把標定的資料匯出（那個綠色的對勾符號）。之後關掉這個工具箱，會彈出一個框，然後選擇yes儲存生成的.mat檔案。記住自己儲存的路徑，下次用的時候提前在matlab中開啟這個檔案。至此，標定完成
標定的引數包括很多，包括相機的內參，外參等，有旋轉矩陣，平移矩陣等。可以自己去查查。
2.影象校正
影象校正是根據標定獲取的畸變矩陣，來校正相機拍攝時造成的誤差，包括徑向畸變和切向畸變。兩者合起來是一個15的矩陣，不過matlab標定的時候卻只顯示兩個12的矩陣，查了知道缺失的一個引數可以設定為0，.如果你利用python雙目測距這篇文章做的雙目視覺定位，這句話肯定會給予你幫助。
3.識別圓球
這裡採用了霍夫變換去識別小球，霍夫變換找圓的基本思想是，對於一個圓 
我們可以把它放入以a,b,r為座標軸的三維座標系系中，而不是以x,y為座標軸的二維座標系中，又因為同一平面不在一條直線的三個點就可以確定一個圓，所以圖中對應的A點（a,b,r）就是一個圓心，但是如果這樣找的話，圖中可以找到很多這樣的三個點，所以也肯定能找到很多圓，因此需要進行‘投票’，即如果有很多點交於這一點，那麼這個點極有可能是圓心:
具體程式碼如下,(github上找的）：
function [Hough_space,Hough_circle_result,Para] = Hough_circle(BW,Step_r,Step_angle,r_min,r_max,p)
%---------------------------------------------------------------------------------------------------------------------------
% input：
% BW:二值影象;
% Step_r:檢測的圓半徑步長;
% Step_angle:角度步長，單位為弧度;
% r_min:最小圓半徑;
% r_max:最大圓半徑;
% p:以p*Hough_space的最大值為閾值，p取0，1之間的數.
% a = x-r*cos(angle); b = y-r*sin(angle);
%---------------------------------------------------------------------------------------------------------------------------
% output：
% Hough_space:引數空間，h(a,b,r)表示圓心在(a,b)半徑為r的圓上的點數;
% Hough_circle:二值影象，檢測到的圓;
% Para:檢測到的圓的圓心、半徑.
%---------------------------------------------------------------------------------------------------------------------------
circleParaXYR=[];
Para=[];
%得到二值影象大小
[m,n] = size(BW);
%計算檢測半徑和角度的步數、迴圈次數 並取整，四捨五入
size_r = round((r_max-r_min)/Step_r)+1;
size_angle = round(2*pi/Step_angle);
%建立引數空間
Hough_space = zeros(m,n,size_r);
%查詢非零元素的行列座標
[rows,cols] = find(BW);
%非零座標的個數
ecount = size(rows);
% Hough變換
% 將影象空間(x,y)對應到引數空間(a,b,r)
% a = x-r*cos(angle)
% b = y-r*sin(angle)
i = 1;
ecount = ecount(1);
for i=1:ecount
for r=1:size_r %半徑步長數按一定弧度把圓幾等分
for k=1:size_angle
a = round(rows(i)-(r_min+(r-1)*Step_r)*cos(k*Step_angle));
b = round(cols(i)-(r_min+(r-1)*Step_r)*sin(k*Step_angle));
if (a>0&&a<=m&&b>0&&b<=n)
Hough_space(a,b,r)=Hough_space(a,b,r)+1;%h(a,b,r)的座標，圓心和半徑
end
end
end
end
% 搜尋超過閾值的聚集點，對於多個圓的檢測，閾值要設的小一點！通過調此值，可以求出所有圓的圓心和半徑返回值就是這個矩陣的最大值
max_para = max(max(max(Hough_space)));
%一個矩陣中，想找到其中大於max_para*p數的位置
index = find(Hough_space>=max_para*p);
length = size(index);%符合閾值的個數
Hough_circle_result=zeros(m,n);
%通過位置求半徑和圓心。
length = length(1);
k = 1;
par = 1;
for i=1:ecount
for k=1:length
par3 = floor(index(k)/(m*n))+1;
par2 = floor((index(k)-(par3-1)*(m*n))/m)+1;
par1 = index(k)-(par3-1)*(m*n)-(par2-1)*m;
if((rows(i)-par1)^2+(cols(i)-par2)^2<(r_min+(par3-1)*Step_r)^2+5&&...
(rows(i)-par1)^2+(cols(i)-par2)^2>(r_min+(par3-1)*Step_r)^2-5)
Hough_circle_result(rows(i),cols(i)) = 1;%檢測的圓
end
end
end
% 從超過峰值閾值中得到
for k=1:length
par3 = floor(index(k)/(m*n))+1;%取整
par2 = floor((index(k)-(par3-1)*(m*n))/m)+1;
par1 = index(k)-(par3-1)*(m*n)-(par2-1)*m;
circleParaXYR = [circleParaXYR;par1,par2,par3];
Hough_circle_result(par1,par2)= 1; %這時得到好多圓心和半徑，不同的圓的圓心處聚集好多點，這是因為所給的圓不是標準的圓
end
%集中在各個圓的圓心處的點取平均，得到針對每個圓的精確圓心和半徑;
while size(circleParaXYR,1) >= 1
num=1;
XYR=[];
temp1=circleParaXYR(1,1);
temp2=circleParaXYR(1,2);
temp3=circleParaXYR(1,3);
c1=temp1;
c2=temp2;
c3=temp3;
temp3= r_min+(temp3-1)*Step_r;
if size(circleParaXYR,1)>1
for k=2:size(circleParaXYR,1)
if (circleParaXYR(k,1)-temp1)^2+(circleParaXYR(k,2)-temp2)^2 > temp3^2
XYR=[XYR;circleParaXYR(k,1),circleParaXYR(k,2),circleParaXYR(k,3)]; %儲存剩下圓的圓心和半徑位置
else
c1=c1+circleParaXYR(k,1);
c2=c2+circleParaXYR(k,2);
c3=c3+circleParaXYR(k,3);
num=num+1;
end
end
end
c1=round(c1/num);
c2=round(c2/num);
c3=round(c3/num);
c3=r_min+(c3-1)*Step_r;
Para=[Para;c1,c2,c3]; %儲存各個圓的圓心和半徑的值
circleParaXYR=XYR;
End4.計算距離
計算距離採用了一個公式：
，其中f為焦距，T為兩個相機之間的距離，分母為兩個圖片的畫素差,例如校正之後，球心在第一張圖片的畫素座標為（u,v1）,在第二章圖片的畫素座標為（u,v2）,則畫素差為|v1-v2|。
注意這個引數，rectifyStereoImages（）這個函式要用：
程式碼如下，讀入兩幅圖，但是隻在一張圖上進行了距離標註，另一張圖只負責提供圓心座標
注意如果不能準確識別出圓球，需要修改最小，最大圓半徑這兩個引數：
I1 = imread('E:\image\left_0.jpg');%讀取左右圖片
I2 = imread('E:\image\right_0.jpg');
%%%注意校正的第三個引數，他的名字由你得到的標定檔案得到，看上圖%%%%
[J1, J2] = rectifyStereoImages(I1,I2,calibrationSession.CameraParameters);
I1=rgb2gray(J1)
I=rgb2gray(J2)
%------------------------------------------
BW1=edge(I1,'sobel')
BW=edge(I2,'sobel')
%----------
Step_r = 0.5;
%角度步長0.1，單位為弧度
Step_angle = 0.1;
%最小圓半徑2
minr =5;
%最大圓半徑30
maxr = 8;
%以thresh*hough_space的最大值為閾值，thresh取0-1之間的數
thresh = 1;
%-----------這個只負責提供其中一張圖片的圓心座標，這個函式是上一段程式碼
[Hough_space,Hough_circle_result,Para] = Hough_circle(BW1,Step_r,Step_angle,minr,maxr,thresh);
%開始檢測另一個圓
[Hough_space,Hough_circle_result,Para1] = Hough_circle(BW,Step_r,Step_angle,minr,maxr,thresh);
%兩幅影象素差
sub=(Para(2)-Para1(2)) %*100/67%如果這裡有67%縮放比例
%焦距
jiaoju=3.6
%相機距離
jixian=12.2
depth=(jiaoju*jixian)/abs(sub)
%距離,轉成字串才能放上去
depth=num2str(depth)
axis equal
figure(1);
imshow(BW,[]),title('邊緣（圖一）');
axis equal
figure(2);
imshow(Hough_circle_result,[]),title('檢測結果（圖一）');
axis equal
figure(3),imshow(I,[]),title('檢測出圖中的圓（圖一）')
hold on;
%--------------------------------------------------------------
%以紅色線標記出的檢測圓心與圓
plot(circleParaXYR(:,2), circleParaXYR(:,1), 'r+');
%打上距離值
text(circleParaXYR(:,2)+circleParaXYR(3), circleParaXYR(:,1)-circleParaXYR(3),depth,'color','red')
for k = 1 : size(circleParaXYR, 1)
t=0:0.01*pi:2*pi;
x=cos(t).*circleParaXYR(k,3)+circleParaXYR(k,2);
y=sin(t).*circleParaXYR(k,3)+circleParaXYR(k,1);
plot(x,y,'r-');
End得到的效果圖：
隨便改改程式碼甚至還可以皮一下：
5.生成視差圖
視差圖還好說，深度圖卻相當詭異，所以我們把深度圖註釋掉了。
clear all
clc
I1 = imread('E:\image\left_0.jpg');%讀取左右圖片
I2 = imread('E:\image\right_0.jpg');
%I1=imcrop(I1,[0,118,320,320])%去除黑邊
%I2=imcrop(I2,[0,118,320,320])
figure
imshowpair(I1, I2, 'montage');
title('Original Images');
%---------------------------------------------------------------
load('E:\image\calibrationSession.mat');%載入你儲存的相機標定的mat
%校正圖片
[J1, J2] = rectifyStereoImages(I1,I2,calibrationSession.CameraParameters);
% figure
% imshow(J1)
figure
imshowpair(J1, J2, 'montage');
title('Undistorted Images');
%-----------------------------------------
figure; imshow(cat(3, J1(:,:,1), J2(:,:,2:3)), 'InitialMagnification', 100)
%----------------------------------------------------------------------------
%disparityRange = [-6 10];如此果想生成彩色視差圖，將此註釋解除
disparityMap1 = disparity(rgb2gray(J1),rgb2gray(J2),'DisparityRange',[0,16],'BlockSize',5,'ContrastThreshold',0.3,'UniquenessThreshold',5);
figure
imshow(disparityMap1)%%%,disparityRange);如此果想生成彩色視差圖，將此註釋解除
% title('Disparity Map');%生成彩色視差圖，將此註釋解除
% colormap(gca,jet) %生成彩色視差圖，將此註釋解除
% colorbar%生成彩色視差圖，將此註釋解除
%---深度圖註釋掉
%pointCloud3D = reconstructScene(disparityMap1, %calibrationSession.CameraParameters);%深度圖
%figure;
%imshow(pointCloud3D);效果圖：
6.拍照用的Python程式碼,參考python雙目測距：
import cv2
import time
AUTO = True # 自動拍照，或手動按s鍵拍照
INTERVAL = 2 # 自動拍照間隔
cv2.namedWindow("shuangmu")
cv2.moveWindow("shuangmu", 400, 0)
shuangmu_camera = cv2.VideoCapture(1)#開啟雙目攝像頭
counter = 0
utc = time.time()
###pattern = (12, 8) # 棋盤格尺寸
folder = "e:/keshe/" # 拍照檔案目錄
def shot(pos, frame):
global counter
path = folder + pos + "_" + str(counter) + ".jpg"
cv2.imwrite(path, frame)#儲存影象
print("snapshot saved into: " + path)
while True:
#ret是布林值，如果讀取幀是正確的則返回True，frame就是每一幀的影象
ret,double_frame = shuangmu_camera.read()
heigh=len(double_frame)
width=len(double_frame[0])
width2=int(width/2)
left1=double_frame[:heigh,0:width2]
right1=double_frame[:heigh,width2:width]
cv2.imshow("shuangmu", double_frame)
now = time.time()
if AUTO and now - utc >= INTERVAL:
shot("left", left1)
shot("right", right1)
counter += 1
utc = now
key = cv2.waitKey(1)#等待鍵盤輸入，1表示延時1ms切換到下一幀影象，對於視訊而言
if key == ord("q"):
break
elif key == ord("s"):
shot("right", right_frame)
counter += 1
cv2.destroyWindow("shuangmu")#釋放視窗