基於靶標的二維視覺定位補償

一、前言

省略了相機的內外參數標定，默認相機沒有畸變，對如下場景進行視覺定位。

場景設定如下：

1、相機鏡頭垂直水平面固定，也就是說本場景只能進行二維平面的定位補償，默認Z軸座標等於0；

2、相機完成標定，即像素座標能夠轉換爲世界座標；

3、靶標是如圖的一張白紙，白紙上有一個黑色圓形和一個黑色三角形；

定位補償的想法：

不論是二維平面定位還是三維空間定位，目的都是想要獲取目標的位置信息，而位置信息不僅包含了x,y,z=0，還包含旋轉角度。當然，在二維平面中旋轉角度只有一個，在三維空間中旋轉角度有三個。

1、設定模板：

也就是在之前我們設定的場景中，把靶標放在你想放的位置上，然後拍一張照。假設就是上圖所示。

因爲我們已經建立了像素座標系，以圖像的左上角爲原點。所以，進行圖像處理得到黑色圓的質點座標X_coordinate=[x0,y0]（名字無所謂），這個座標就是模板座標。當然爲了簡單，我們設定模板的時候，最好讓向量AB能夠和X軸平行且方向相同，因爲這樣的話AB向量和X軸的夾角就是0°，默認爲靶標沒有發生旋轉。

2、計算偏移量：

當模板座標確定後，如果靶標發生偏移，則再次識別圓，確定圓的質點座標[x1,y1]。則相對模板靶標的偏移量就是[x1-x0,y1-y0]。

3、計算旋轉角度：

知道了座標的偏移量還不行啊，因爲我們不知道靶標往那個方向偏移了，所以要確定靶標相對模板的旋轉角度。

在x軸上任意取一點[112,0],與原點[0,0]，可以構成一個向量X_Particle = np.array([112,0])。然後找到圓的質點和三角形的質點，求出AB向量。最後根據向量夾角的公式就能算出旋轉角度。

注意：向量的夾角範圍是0到180，所以要根據向量AB的方向來判斷。這裏我判斷如果向量AB的方向和Y軸方向一致，則夾角爲正；否則夾角爲否。

二、完整代碼

import cv2
import numpy as np


#定義線程函數——
def PositionParticle(frame):
    gauss_img = cv2.GaussianBlur(frame,(3,3),0)
    gray_img = cv2.cvtColor(gauss_img,cv2.COLOR_RGB2GRAY)
    _,er_img = cv2.threshold(gray_img,90,255,cv2.THRESH_BINARY_INV)
    cv2.imshow('er',er_img)
    #提取輪廓
    contours,_ = cv2.findContours(er_img,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
    #提取質點
    Particle = []
    for contour in contours:
        contour_ = np.squeeze(contour)
        x = np.int(np.sum(contour_,axis=0)[0] / len(contour_))
        y = np.int(np.sum(contour_,axis=0)[1] / len(contour_))
        Particle.append([x,y])
    #debug
    print(Particle)

    #霍夫圓檢測——找到圖像中的圓——魯棒性真差
    circles = cv2.HoughCircles(gray_img,cv2.HOUGH_GRADIENT,1,10,param1=100,param2=35)
    #debug
    print(circles)
    ######################求方向向量###魯棒性差#############
    Particle = np.array(Particle)
    #定義方向向量的起始點A和終止點B
    A,B = [],[]
    for circle in circles[0,:]:
        i = np.array(circle[:2])
        for j in Particle:
            #判斷已求質心中，那個是圓的——是圓的就賦值給A點
            if np.abs(np.sum(j-i)) < 5:
                B = j
            else:
                A = j
    Poisiton = np.array(B) - np.array(A)
    return Poisiton,B
#座標域判斷函數
def Coordinate(x_particle,mark_particle):
    if mark_particle[1] >= 0:
        angle = np.arccos(x_particle.dot(mark_particle) /
                          (np.sqrt(x_particle.dot(x_particle)) * np.sqrt(mark_particle.dot(mark_particle)))) * 180 / np.pi
    else:
        angle = np.arccos(x_particle.dot(mark_particle) /
                          (np.sqrt(x_particle.dot(x_particle)) * np.sqrt(mark_particle.dot(mark_particle)))) * (-180) / np.pi
    return angle



if __name__ == '__main__':
    #定義一個X軸的方向向量
    X_Particle = np.array([112,2])
    #定義模板座標
    X_coordinate = np.array([285,85])
    #debug
    print(X_Particle)
    video = cv2.VideoCapture(1)
    while video.isOpened():
        ok,frame = video.read()
        if not ok:
            print('video open error')
            break
        else:
            cv2.imshow('src',frame)
            key = cv2.waitKey(5)
            #定義鍵盤事件
            if key & 0XFF == 27:    #Esc事件
                break
            if key & 0XFF == 32:    #Space事件
                Mark_Particle,Circle = PositionParticle(frame)
                #debug
                print(Mark_Particle)
                #角度偏差
                angle = Coordinate(X_Particle,Mark_Particle)
                print(angle)
                #座標偏差
                lass = Circle - X_coordinate
                print(lass)


    cv2.destroyAllWindows()

三、代碼解釋

首先說明，代碼能夠運行，但是這就是一個練手的demo，不要期望精度有多高。另外，圓的識別也是一個問題，每次圓的質點座標都有1到2個像素的偏差。

在PositionParticle（frame）函數中，先使用輪廓提取圖像中的圓和三角形最外層輪廓；根據每一個輪廓的點，能求出輪廓對應的質點，但是不能確定那個是圓的質點，那個是三角形的質點。所以在進行霍夫圓檢測（如果識別不到圓，就把param2參數設小點），然後讓兩個質點分別和檢測出的圓心做距離比較（距離可以設置爲圓半徑的五分之一）,距離越小的應該就是圓的質點；最後就能求得AB向量。

函數Coordinate（x_particle,mark_particle），使用mark_particle[1]判斷就是AB向量的y值，當mark_particle[1]大於等於0時，AB向量和x軸夾角就是正的；當其小於0時，AB向量和x軸夾角就是否的。