一、测距原理
单目视觉测距是利用一个摄像机获得的图片得出深度信息，按照测量的原理主要分为基于已知运动和已知物体的测量方法。
已知物体的测量方法是指在已知物体信息的条件下利用摄像机获得的目标图片得到深度信息。此类方法主要应用于单目视觉进行导航和定位，该类方法的缺点是利用单个特征点进行测量，容易因特征点提取的不准确性，产生误差。
我们采用摄像头采集图片，将三维场景投影到摄像机二维像平面上。对于测量地球坐标系中的物体而言，小孔成像模型（也称为线性摄像机模型）基本可以满足测量的要求，即任意点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 -*-
# 定义编码，中文注释
# import the necessary packages
import numpy as np
import cv2
# 找到目标函数
def find_marker(image):
# convert the image to grayscale, blur it, and detect edges
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (5, 5), 0)
edged = cv2.Canny(gray, 35, 125)
# find the contours in the edged image and keep the largest one;
# we'll assume that this is our piece of paper in the image
(cnts, _) = cv2.findContours(edged.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
# 求最大面积
c = max(cnts, key=cv2.contourArea)
# compute the bounding box of the of the paper region and return it
# cv2.minAreaRect() c代表点集，返回rect[0]是最小外接矩形中心点坐标，
# rect[1][0]是width，rect[1][1]是height，rect[2]是角度
return cv2.minAreaRect(c)
# 距离计算函数
def distance_to_camera(knownWidth, focalLength, perWidth):
# compute and return the distance from the maker to the camera
return (knownWidth * focalLength) / perWidth
# 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
# A4纸的长和宽(单位:inches)
KNOWN_WIDTH = 11.69
KNOWN_HEIGHT = 8.27
# initialize the list of images that we'll be using
IMAGE_PATHS = ["Picture1.png", "Picture2.jpg", "Picture3.jpg"]
# load the furst image that contains an object that is KNOWN TO BE 2 feet
# from our camera, then find the paper marker in the image, and initialize
# the focal length
# 读入第一张图，通过已知距离计算相机焦距
image = cv2.imread(IMAGE_PATHS[0])
marker = find_marker(image)
focalLength = (marker[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH
# 通过摄像头标定获取的像素焦距
# focalLength = 811.82
print('focalLength = ', focalLength)
# 打开摄像头
camera = cv2.VideoCapture(0)
while camera.isOpened():
# get a frame
(grabbed, frame) = camera.read()
marker = find_marker(frame)
if marker == 0:
print(marker)
continue
inches = distance_to_camera(KNOWN_WIDTH, focalLength, marker[1][0])
# draw a bounding box around the image and display it
box = cv2.boxPoints(marker)
box = np.int0(box)
cv2.drawContours(frame, [box], -1, (0, 255, 0), 2)
# inches 转换为 cm
cv2.putText(frame, "%.2fcm" % (inches * 30.48 / 12),
(frame.shape[1] - 200, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX,
2.0, (0, 255, 0), 3)
# show a frame
cv2.imshow("capture", frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
camera.release()
cv2.destroyAllWindows()
