这是以前做的小项目里的一部分,由于时间久远,在这里整理以下,也方便自己以后查阅。
ZED-stereo是很好用功能强大的双目相机,可以调用自带的库直接读取点云数据,也可以很方便获得图像任意一点距离相机的深度。这篇博文主要记录如果使用ZED 相机配合SURF模板识别的方法,识别目标物体并且获得它距离相机的深度。
先上代码:
#调用ZED自带的库
import pyzed.camera as zcam
import pyzed.defines as sl
import pyzed.types as tp
import pyzed.core as core
import math
import numpy as np
import sys
import cv2
# Surf detect
#设置SURF的参数
MIN_MATCH_COUNT = 30
surf = cv2.xfeatures2d.SURF_create()
FLANN_INDEX_KDITREE = 0
flannParam = dict(algorithm=FLANN_INDEX_KDITREE,tree=5)
flann=cv2.FlannBasedMatcher(flannParam,{})
#读取模板图像
trainImg=cv2.imread("0.jpg",0)
trainKP,trainDesc=surf.detectAndCompute(trainImg,None)
font = cv2.FONT_HERSHEY_SIMPLEX
def main():
#ZED自带程序,照搬过来
print("Running...")
init = zcam.PyInitParameters()
zed = zcam.PyZEDCamera()
if not zed.is_opened():
print("Opening ZED Camera...")
status = zed.open(init)
if status != tp.PyERROR_CODE.PySUCCESS:
print(repr(status))
exit()
#获取深度,点云等数据
runtime = zcam.PyRuntimeParameters()
mat = core.PyMat()
depth = core.PyMat()
point_cloud = core.PyMat()
#ZED的参数设置
init_params = zcam.PyInitParameters()
init_params.depth_mode = sl.PyDEPTH_MODE.PyDEPTH_MODE_PERFORMANCE # Use PERFORMANCE depth mode
init_params.coordinate_units = sl.PyUNIT.PyUNIT_MILLIMETER # Use milliliter units (for depth measurements)
#改变相机的模式,VGA分辨率低但是速度会快很多
init_params.camera_resolution = sl.PyRESOLUTION.PyRESOLUTION_VGA
init_params.camera_fps = 100
key = ''
while key != 113: # for 'q' key
err = zed.grab(runtime)
if err == tp.PyERROR_CODE.PySUCCESS:
zed.retrieve_image(mat, sl.PyVIEW.PyVIEW_LEFT)
zed.retrieve_measure(depth, sl.PyMEASURE.PyMEASURE_DEPTH)
zed.retrieve_measure(point_cloud, sl.PyMEASURE.PyMEASURE_XYZRGBA)
frame = mat.get_data()
t1 = cv2.getTickCount()
#frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
#因为ZED是双目相机,所以这里识别部分只使用左镜头的图像
frame = cv2.resize(frame, (int(mat.get_width()/2),int(mat.get_height()/2)),interpolation=cv2.INTER_AREA)
# SURF detect
KP,Desc = surf.detectAndCompute(frame,None)
matches = flann.knnMatch(Desc,trainDesc,k=2)
goodMatch=[]
for m,n in matches:
if(m.distance<0.8*n.distance):
goodMatch.append(m)
if(len(goodMatch)>MIN_MATCH_COUNT):
yp=[]
qp=[]
for m in goodMatch:
yp.append(trainKP[m.trainIdx].pt)
qp.append(KP[m.queryIdx].pt)
yp,qp=np.float32((yp,qp))
H,status=cv2.findHomography(yp,qp,cv2.RANSAC,3.0)
h,w=trainImg.shape
trainBorder=np.float32([[[0,0],[0,h-1],[w-1,h-1],[w-1,0]]])
imgBorder=cv2.perspectiveTransform(trainBorder,H)
cv2.polylines(frame,[np.int32(imgBorder)],True, (0,255,0),3)#在imshow的图像上显示出识别的框
# get coordinate 获得目标物体bbox的坐标并计算出中心点坐标
c1 = imgBorder[0,0]
c2 = imgBorder[0,1]
c3 = imgBorder[0,2]
c4 = imgBorder[0,3]
xmin = min(c1[0],c2[0])
xmax = max(c3[0],c4[0])
ymin = min(c1[1],c4[1])
ymax = max(c2[1],c3[1])
#distance_point_cloud
x = round(xmin+xmax)
y = round(ymin+ymax)
#计算出的中心点坐标后,来获取点云数据
err, point_cloud_value = point_cloud.get_value(x, y)
#由点云数据计算出和相机的距离(左相机为原点)
distance = math.sqrt(point_cloud_value[0] * point_cloud_value[0] +
point_cloud_value[1] * point_cloud_value[1] +
point_cloud_value[2] * point_cloud_value[2])
#把距离打在屏幕里
| | |
|--|--|
| | |
if not np.isnan(distance) and not np.isinf(distance):
distance = round(distance)
print("Distance to Camera at ({0}, {1}): {2} mm\n".format(x, y, distance))
Z = "distance:{} mm".format(distance)
cv2.putText(frame,Z,(xmax,ymax), font, 0.7,(255,255,255),2,cv2.LINE_AA)
# Increment the loop
else:
print("Can't estimate distance at this position, move the camera\n")
else:
print("Not Eough match")
sys.stdout.flush()
t2 = cv2.getTickCount()
fps = cv2.getTickFrequency()/(t2-t1)
fps = "Camera FPS: {0}.".format(fps)
cv2.putText(frame,fps,(25,25),font,0.5,(255,255,255),2,cv2.LINE_AA)
cv2.imshow("ZED", frame)
key = cv2.waitKey(5)
else:
key = cv2.waitKey(5)
cv2.destroyAllWindows()
zed.close()
print("\nFINISH")
if __name__ == "__main__":
main()
运行之后的样子
B站上还有我运行的一小段视频,可以看出来真的很卡。
添加链接描述
程序的主体部分不变,读取模板的地方要修改一下,导入多个模板。
trainKP = []
trainDesc = []
train_h = []
train_w = []
for i in range(8):
trainImg = cv2.imread("%d.jpg"%(i))
height, width, c=trainImg.shape
train_h.append(height)
train_w.append(width)
tKP, tDesc = surf.detectAndCompute(trainImg,None)
trainKP.append(tKP)
trainDesc.append(tDesc)
我把模板的名称都改称1.jpg, 2.jpg … 这样读取方便,获取的特征点都存在list里
识别的部分也有些改变。
for k, d, h, w in zip(trainKP, trainDesc, train_h, train_w):
KP, Desc = surf.detectAndCompute(frame,None)
matches = flann.knnMatch(Desc, d, k=2) #trainDesc
goodMatch=[]
for m, n in matches:
if(m.distance<0.7*n.distance):
goodMatch.append(m)
if(len(goodMatch)>MIN_MATCH_COUNT):
yp=[]
qp=[]
for m in goodMatch:
yp.append(k[m.trainIdx].pt) #trainKP
qp.append(KP[m.queryIdx].pt)
yp,qp=np.float32((yp,qp))
H,status=cv2.findHomography(yp,qp,cv2.RANSAC,3.0)
trainBorder=np.float32([[[0,0],[0,h-1],[w-1,h-1],[w-1,0]]])
imgBorder=cv2.perspectiveTransform(trainBorder,H)
cv2.polylines(frame,[np.int32(imgBorder)],True, (0,255,0),3)
多模版识别是我曾经想用来识别一个立体物体时,围着它拍了8张照片当模板,然后发现运行起来实在实在太慢了,最后还是放弃了。
SURF的原理很多博文都讲的很好,这里不想多写了,何况我也不是很明白,哈哈。毕竟SURF的局限性还是很大,运行速度甚至还不如深度学习(当然在显卡可以加速的前提下)。
但是SURF优点就是对平面的,长的都一样的物体识别效果很好,在工业上应该用处还很大吧。