球體的雙目視覺定位（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;  
End

4.計算距離

計算距離采用了一個公式：，其中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")#釋放窗口