Python-OpenCV的单目视觉测距

    前言： 

        视觉测距作为机器视觉领域内基础技术之一而受到广泛的关注，其在机器人领域内占有重要的地位，广泛应用于机器视觉定位、目标跟踪、视觉避障等。机器视觉测量主要分为：单目视觉测量、双目视觉测量、结构光视觉测量等。结构光由于光源的限制，应用的场合比较固定；双目视觉难点在于特征点的匹配，影响了测量的精度和效率，其理论研究的重点集中于特征的匹配上；而单目视觉结构简单，运算速度快而具有广阔的应用前景。

      今天看到一篇博客提到一种简单粗暴的单目视觉测距方法，文章地址如下文章地址。在这里呢，自己尝试把他复现了一下，过程不难，十分简单，不过呢，精度也凑合。

一、测距原理  

      单目视觉测距是利用一个摄像机获得的图片得出深度信息，按照测量的原理主要分为基于已知运动和已知物体的测量方法。

已知物体的测量方法是指在已知物体信息的条件下利用摄像机获得的目标图片得到深度信息。此类方法主要应用于单目视觉进行导航和定位，该类方法的缺点是利用单个特征点进行测量，容易因特征点提取的不准确性，产生误差。   

    我们采用摄像头采集图片，将三维场景投影到摄像机二维像平面上。对于测量地球坐标系中的物体而言，小孔成像模型（也称为线性摄像机模型）基本可以满足测量的要求，即任意点p1 在图像中的投影位置p2为光心Oc与 p1点的连线与图像平面的交点，如下图所示：

现实中的物体的成像我们也可以表示为如下所示：

据此，我们将使用相似三角形来计算相机到一个已知的物体或者目标的距离。

      相似三角形就是这么一回事：假设我们有一个宽度为 W 的目标或者物体。然后我们将这个目标放在距离我们的相机为 D 的位置。我们用相机对物体进行拍照并且测量物体的像素宽度 P 。这样我们就得出了相机焦距的公式：

举个例子，假设我在离相机距离 D = 24 英寸的地方放一张标准的 8.5 x 11 英寸的 A4 纸（横着放；W = 11）并且拍下一张照片。我测量出照片中 A4 纸的像素宽度为 P = 249 像素。

因此我的焦距 F 是：

当我继续将我的相机移动靠近或者离远物体或者目标时，我可以用相似三角形来计算出物体离相机的距离：

为了更具体，我们再举个例子，假设我将相机移到距离目标 3 英尺（或者说 36 英寸）的地方并且拍下上述的 A4 纸。通过自动的图形处理我可以获得图片中 A4 纸的像素距离为 170 像素。将这个代入公式得：

或者约 36 英寸，合 3 英尺。

从以上的解释中，我们可以看到，要想得到距离，我们就要知道摄像头的焦距和目标物体的尺寸大小，这两个已知条件根据公式：　　

得出目标到摄像机的距离Ｄ，其中Ｐ是指像素距离，Ｗ是Ａ４纸的宽度，Ｆ是摄像机焦距。

　　接下来，是通过预先拍照，根据第一张照片算出摄像头的焦距，在根据已知的焦距算出接下来的照片中白纸到摄像机的距离。

二、演示代码

#!usr/bin/python
# -*- coding: utf-8 -*-
 
########利用三角形相似原理进行简单单目测距#########
# author：行歌
# email:1013007057@qq.com
 
import numpy as np
import cv2
 
# initialize the known distance from the camera to the object,
# which in this case is 24 inches
KNOWN_DISTANCE = 24.0
 
# initialize the known object width, which in this case,
# the piece of paper is 11 inches wide
KNOWN_WIDTH = 11.69
KNOWN_HEIGHT = 8.27
 
# initialize the list of images that we'll be using
IMAGE_PATHS = ["Picture1.jpg", "Picture2.jpg", "Picture3.jpg"]
 
 
def find_marker(image):
    gray_img = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    # 将彩色图转化为灰度图
 
    gray_img = cv2.GaussianBlur(gray_img, (5, 5), 0)
    # 高斯平滑去噪
 
    edged_img = cv2.Canny(gray_img, 35, 125)
    # Canny算子阈值化
    # cv2.imshow("edged_img",edged_img)
 
    img, countours, hierarchy = cv2.findContours(edged_img.copy(), cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
    # 注意，findcontours函数会“原地”修改输入的图像。opencv3会返回三个值,分别是img, countours, hierarchy
    # 第二个参数表示轮廓的检索模式,cv2.RETR_EXTERNAL表示只检测外轮廓；v2.RETR_LIST检测的轮廓不建立等级关系
    # cv2.RETR_CCOMP建立两个等级的轮廓；cv2.RETR_TREE建立一个等级树结构的轮廓。
    # 第三个参数method为轮廓的近似办法,cv2.CHAIN_APPROX_NONE存储所有的轮廓点，
    # 相邻的两个点的像素位置差不超过1，即max（abs（x1 - x2），abs（y2 - y1）） == 1
    # cv2.CHAIN_APPROX_SIMPLE压缩水平方向，垂直方向，对角线方向的元素，只保留该方向的终点坐标，
    # 例如一个矩形轮廓只需4个点来保存轮廓信息
 
    # cv2.drawContours(image,countours,-1,(0,0,255),2,8)
    # # 第三个参数指定绘制轮廓list中的哪条轮廓，如果是-1，则绘制其中的所有轮廓。
    #
    # cv2.imshow('image', image)
 
    # print(len(countours)),
    # 输出如下：15，即该图检测出15个轮廓
 
    c = max(countours, key = cv2.contourArea)
    # 提取最大面积矩形对应的点集
 
    rect = cv2.minAreaRect(c)
    # cv2.minAreaRect()函数返回矩形的中心点坐标，长宽，旋转角度[-90,0)，当矩形水平或竖直时均返回-90
    # c代表点集，返回rect[0]是最小外接矩形中心点坐标，
    # rect[1][0]是width，rect[1][1]是height，rect[2]是角度
 
 
    # box = cv2.boxPoints(rect)
    # # 但是要绘制这个矩形，我们需要矩形的4个顶点坐标box, 通过函数cv2.boxPoints()获得，
    # # 即得到box：[[x0, y0], [x1, y1], [x2, y2], [x3, y3]]
    # # print(box)，输出如下：
    # # [[508.09482  382.58597]
    # #  [101.76947  371.29916]
    # #  [109.783356  82.79956]
    # #  [516.1087    94.086365]]
    #
    # # 根据检测到的矩形的顶点坐标box，我们可以将这个矩形绘制出来，如下所示：
    # for i in range(len(box)):
    #     cv2.line(image, (box[i][0],box[i][1]),(box[(i+1)%4][0],box[(i+1)%4][1]),(0,0,255),2,8)
    # cv2.imshow('image', image)
 
    return rect
 
 
def distance_to_camera(knownWidth, focalLength, perWidth):
    return (knownWidth * focalLength) / perWidth
 
 
def calculate_focalDistance(img_path):
    first_image = cv2.imread(img_path)
    # cv2.imshow('first image',first_image)
 
    marker = find_marker(first_image)
    # 得到最小外接矩形的中心点坐标，长宽，旋转角度
    # 其中marker[1][0]是该矩形的宽度，单位为像素
 
    focalLength = (marker[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH
    # 获取摄像头的焦距
 
    print('焦距（focalLength ）= ',focalLength)
    # 将计算得到的焦距打印出来
 
    return focalLength
 
 
def calculate_Distance(image_path,focalLength_value):
    # 加载每一个图像的路径，读取照片，找到A4纸的轮廓
    # 然后计算A4纸到摄像头的距离
 
    image = cv2.imread(image_path)
    cv2.imshow("image", image)
    cv2.waitKey(300)
 
    marker = find_marker(image)
    distance_inches = distance_to_camera(KNOWN_WIDTH,focalLength_value, marker[1][0])
    # 计算得到目标物体到摄像头的距离，单位为英寸，
    # 注意，英寸与cm之间的单位换算为： 1英寸=2.54cm
 
    box = cv2.boxPoints(marker)
    # print( box )，输出类似如下：
    # [[508.09482  382.58597]
    #  [101.76947  371.29916]
    #  [109.783356 82.79956]
    #  [516.1087   94.086365]]
 
    box =np.int0( box)
    # 将box数组中的每个坐标值都从浮点型转换为整形
    # print( box )，输出类似如下：
    # [[508 382]
    #  [101 371]
    #  [109 82]
    #  [516 94]]
 
    cv2.drawContours(image, [box], -1, (0, 0, 255), 2)
    # 在原图上绘制出目标物体的轮廓
 
    cv2.putText(image, "%.2fcm" % (distance_inches * 2.54),
            (image.shape[1] - 300, image.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX,
            2.0, (0, 0, 255), 3)
    # cv2.putText()函数可以在照片上添加文字
    # cv2.putText(img, txt, (int(x),int(y)), fontFace, fontSize, fontColor, fontThickness)
    # 各参即为：照片/添加的文字/左上角坐标/字体/字体大小/颜色/字体粗细
 
    cv2.imshow("image", image)
 
 
 
 
 
if __name__ == "__main__":
    img_path = "Picture1.jpg"
    focalLength = calculate_focalDistance(img_path)
    # 获得摄像头焦
 
    for image_path in IMAGE_PATHS:
        calculate_Distance(image_path,focalLength)
        cv2.waitKey(1000)
    cv2.destroyAllWindows()
 

运行程序，结果如下：

对于图像一：

对于图像二：

对于图像三：