一、前言
省略了相机的内外参数标定，默认相机没有畸变，对如下场景进行视觉定位。
场景设定如下：
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轴夹角就是否的。
