單目攝像機測距(python+opencv)
我的論文方向目前是使用單目攝像頭實現機器人對人的跟隨,首先單目攝像頭與kinect等深度攝像頭最大的區別是無法有效獲取深度資訊,那就首先從這方面入手,嘗試通過影象獲取攝像頭與人的距離。
在網上看了幾天關於攝像頭標定和攝像頭焦距等原理的文章,然後通過這篇文章真正啟發了我:用python和opencv來測量目標到相機的距離 主要的測距的原理是利用相似三角形計算物體到相機的距離。
在這裡我的環境為: Ubuntu14.04 + Opencv2.4.9
一 用相似三角形計算物體或者目標到相機的距離
我們將使用相似三角形來計算相機到一個已知的物體或者目標的距離。
相似三角形就是這麼一回事:假設我們有一個寬度為 W 的目標或者物體。然後我們將這個目標放在距離我們的相機為 D 的位置。我們用相機對物體進行拍照並且測量物體的畫素寬度 P 。這樣我們就得出了相機焦距的公式:
F = (P x D) / W
舉個例子,假設我在離相機距離 D = 24 英寸的地方放一張標準的 8.5 x 11 英寸的 A4 紙(橫著放;W = 11)並且拍下一張照片。我測量出照片中 A4 紙的畫素寬度為 P = 249 畫素。
因此我的焦距 F 是:
F = (248px x 24in) / 11in = 543.45
當我繼續將我的相機移動靠近或者離遠物體或者目標時,我可以用相似三角形來計算出物體離相機的距離:
D’ = (W x F) / P
為了更具體,我們再舉個例子,假設我將相機移到距離目標 3 英尺(或者說 36 英寸)的地方並且拍下上述的 A4 紙。通過自動的圖形處理我可以獲得圖片中 A4 紙的畫素距離為 170 畫素。將這個代入公式得:
D’ = (11in x 543.45) / 170 = 35 英寸
或者約 36 英寸,合 3 英尺。
從以上的解釋中,我們可以看到,要想得到距離,我們就要知道攝像頭的焦距和目標物體的尺寸大小,這兩個已知條件根據公式:
D’ = (W x F) / P
得出目標到攝像機的距離D,其中P是指畫素距離,W是A4紙的寬度,F是攝像機焦距。
在原文中,是通過預先拍照,根據第一張照片算出攝像頭的焦距,在根據已知的焦距算出接下來的照片中白紙到攝像機的距離,這樣不太直觀,而且需要預先拍照,我將源程式改為實時測距,簡單來說就是將原來的讀入照片變為讀攝像頭,這樣的效果看起來比較直觀.源程式如下:
#!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.jpg", "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 = np.int0(cv2.cv.BoxPoints(marker))
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()
程式效果圖如下:
在這張圖裡我攝像頭距離桌面大概100cm,可以看到圖中距離為96cm,可以看到精度還可以。
需要注意的是, 如果使用的是opencv3的版本,
1. 需要將find_marker函式中
(cnts, _) = cv2.findContours(edged.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
改為:
(_, cnts, _) = cv2.findContours(edged.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
因為 In Opencv 3 API version the cv2.findCoutours() returns 3 object
- image
- contours
- hierarchy
2. 需要將:
box = np.int0(cv2.cv.BoxPoints(marker))
改為:
box = cv2.boxPoints(marker)
box = np.int0(box)
以上兩個地方是安裝不同版本opencv需要修改的地方。存在的問題:
1. 程式在執行時在未檢測到A4紙時有時候會報錯:
Traceback (most recent call last):File "video_paper_distance.py", line 86, in <module>
marker = find_marker(frame)
File "video_paper_distance.py", line 18, in find_marker
c = max(cnts, key = cv2.contourArea)
ValueError: max() arg is an empty sequence
目前關於這個錯誤,我還沒有解決,猜測主要是由於沒有檢測到目標造成max()函式為空的原因,不過沒有深究。
2. 程式是通過第一張圖已知目標到相機的距離來計算攝像頭焦距,然後再通過焦距計算接下來目標到攝像頭的距離,在這裡焦距是一個關鍵的引數,所以我準備嘗試通過對攝像頭的標定直接獲取相機的畫素焦距,我是通過ros的一個包實現了對相機的標定,不過通過相機標定得出的畫素焦距計算出來的距離並沒有通過第一張圖片計算出的焦距計算出來的距離準確,這個具體原因也沒有搞明白,可能是我標定的結果不夠準確?
3. 在通過攝像頭測距時, 得出的距離也是準確且隨著攝像頭距離桌面遠近而線性變化的,但距離偶爾會出現突變,目前也沒找到是什麼原因造成的.
ros相機標定主要參考的是這篇部落格,博主是白巧克力亦唯心,ROS大神:
這裡主要記錄的是,通過攝像機標定,得到的3*3的內參數矩陣,其中M[1][1]和M[2][2]分別為我們要求的相機的x,y軸的畫素焦距。
二 使用相機計算人到相機的距離
在第一部分中我們已經計算出了A4紙距離相機的距離,在具體應用中,我需要計算的是人距離相機的距離,來實現機器人對目標人距離的判斷,應用與對目標人的跟隨。在這裡主要的思路是先通過opencv中的HOG方法檢測到人,再根據人的預估身高和攝像頭焦距計算人到攝像機的距離。在這裡選擇身高的原因在於人的身高在不同方向上變化較小,而且我們的攝像頭高度是固定的,所以選擇身高。
1.首先要使用opencv進行行人檢測:
#!usr/bin/python
# -*- coding: utf-8 -*-
# import the necessary packages
from __future__ import print_function
from imutils.object_detection import non_max_suppression
from imutils import paths
import numpy as np
import argparse
import imutils
import cv2
cap = cv2.VideoCapture(0)
# initialize the HOG descriptor/person detector
hog = cv2.HOGDescriptor()
# 使用opencv預設的SVM分類器
hog.setSVMDetector(cv2.HOGDescriptor_getDefaultPeopleDetector())
while(1):
# get a frame
ret, frame = cap.read()
frame = imutils.resize(frame, width=min(400, frame.shape[1]))
# detect people in the image
(rects, weights) = hog.detectMultiScale(frame, winStride=(4, 4),
padding=(8, 8), scale=1.05)
rects = np.array([[x, y, x + w, y + h] for (x, y, w, h) in rects])
# 非極大抑制 消除多餘的框 找到最佳人體
pick = non_max_suppression(rects, probs=None, overlapThresh=0.65)
# 畫出邊框
for (xA, yA, xB, yB) in pick:
cv2.rectangle(frame, (xA, yA), (xB, yB), (0, 255, 0), 2)
# show a frame
cv2.imshow("capture", frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
2.將行人檢測與測距程式碼結合:
while camera.isOpened():
# get a frame
(grabbed, frame) = camera.read()
# 如果不能抓取到一幀,說明我們到了視訊的結尾
if not grabbed:
break
frame = imutils.resize(frame, width=min(400, frame.shape[1]))
#marker = find_marker(frame)
marker = find_person(frame)
#inches = distance_to_camera(KNOWN_WIDTH, focalLength, marker[1][0])
for (xA, yA, xB, yB) in marker:
cv2.rectangle(frame, (xA, yA), (xB, yB), (0, 255, 0), 2)
ya_max = yA
yb_max = yB
pix_person_height = yb_max - ya_max
if pix_person_height == 0:
#pix_person_height = 1
continue
print (pix_person_height)
#print (pix_person_height)
inches = distance_to_camera(KNOW_PERSON_HEIGHT, focalLength, pix_person_height)
print("%.2fcm" % (inches *30.48/ 12))
# draw a bounding box around the image and display it
#box = np.int0(cv2.cv.BoxPoints(marker))
#cv2.drawContours(frame, [box], -1, (0, 255, 0), 2)
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
3.存在的問題:
目前使用HOG檢測行人的效果不是很好,會把類似人體形狀的物體都框出來,比如實驗室的三腳架等物體,受背景干擾較大。程式中存在一個bug就是在沒有檢測到人時,pix_person_height會為0,這樣分母為0時無法計算,在接下來我也要通過3個方面改進,首先要想辦法進一步改進人體檢測,使用YOLO的方法目前是比較好的,但在CPU下速度較慢。然後要改進的是精度,這裡需要主要的是選擇攝像頭要選擇固定焦距的攝像頭,自動變焦攝像頭焦距會變化,測量的距離也會變。最後就是儘可能完善程式,減少bug。
4 . 將要進行的工作
通過程式可以看到使用單目攝像頭檢測人到攝像頭的距離,其中一個影響較大的因素是對人體的準確檢測,如果想要使測量的距離準確(完全準確是不可能的,但要達到可以用於機器人跟隨人的功能的程度),那就要儘可能的準確的檢測出人,通過我的測試,在準確知道目標人的身高前提下,在離攝像頭固定距離上對人拍照,然後手動對人進行畫框,標定出目標人的在畫面中的高度,通過計算,得到的距離比較準確,其精度完全是可以接受的,所以接下來的工作主要是如何通過程式來準確的框出目標人來獲取其在影象中的高度。
程式的原始碼已經上面已經貼出,也可以到下載頁面下載。