前言： 

        視覺測距作為機器視覺領域內基礎技術之一而受到廣泛的關注，其在機器人領域內佔有重要的地位，廣泛應用於機器視覺定位、目標跟蹤、視覺避障等。機器視覺測量主要分為：單目視覺測量、雙目視覺測量、結構光視覺測量等。結構光由於光源的限制，應用的場合比較固定；雙目視覺難點在於特徵點的匹配，影響了測量的精度和效率，其理論研究的重點集中於特徵的匹配上；而單目視覺結構簡單，運算速度快而具有廣闊的應用前景。

      今天看到一篇部落格提到一種簡單粗暴的單目視覺測距方法，文章地址如下文章地址。在這裡呢，自己嘗試把他復現了一下，過程不難，十分簡單，不過呢，精度也湊合。

一、測距原理  

      單目視覺測距是利用一個攝像機獲得的圖片得出深度資訊，按照測量的原理主要分為基於已知運動和已知物體的測量方法。

已知物體的測量方法是指在已知物體資訊的條件下利用攝像機獲得的目標圖片得到深度資訊。此類方法主要應用於單目視覺進行導航和定位，該類方法的缺點是利用單個特徵點進行測量，容易因特徵點提取的不準確性，產生誤差。

  我們採用攝像頭採集圖片，將三維場景投影到攝像機二維像平面上。對於測量地球座標系中的物體而言，小孔成像模型（也稱為線性攝像機模型）基本可以滿足測量的要求，即任意點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:[email protected]

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()

執行程式，結果如下：

對於影象一：

對於影象二：

對於影象三：