使用ZED-stereo相机配合SURF算法识别目标并测距

这是以前做的小项目里的一部分，由于时间久远，在这里整理以下，也方便自己以后查阅。

使用ZED-stereo相机配合SURF算法识别目标并测距

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优点就是对平面的，长的都一样的物体识别效果很好，在工业上应该用处还很大吧。