python 单目视觉测距

# import the necessary packages
import numpy as np 
import cv2
import sys
reload(sys)
sys.setdefaultencoding('utf8')
cap = cv2.VideoCapture(0)
cap.set(3,640)
cap.set(4,480)
cap.set(1, 10.0) 
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
    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
KNOWN_WIDTH = 11.0
 
# initialize the list of images that we'll be using
IMAGE_PATHS = ["111.png"]
 
# 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


 # loop over the images
while True:
    ret,frame = cap.read()
    if ret == True:
        frame = cv2.flip(frame, 1)
        #a = out.write(frame)
        #cv2.imshow("frame", frame)
        #image = cv2.imread(IMAGE_PATHS[0])
        image=frame
        marker = find_marker(frame)
        focalLength = (marker[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH
    # load the image, find the marker in the image, then compute the
    # distance to the marker from the camera
        #image = cv2.imread(imagePath)
        marker = find_marker(frame)
        inches = distance_to_camera(KNOWN_WIDTH, focalLength, marker[1][0])
        dis=(inches / 12)*0.3048
        print dis
    # draw a bounding box around the image and display it
        box = np.int0(cv2.cv.BoxPoints(marker))
        cv2.drawContours(image, [box], -1, (0, 255, 0), 2)
        cv2.putText(image, "%.2f m" % float(dis),
            (image.shape[1] - 250, image.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX,
            2.0, (0, 255, 0), 3)
        cv2.imshow("image", image)
    #cv2.imwrite('111_test.jpg',image)
    #cv2.waitKey(0)
    if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    else:
        break


cap.release()
#out.release()
cv2.destroyAllWindows()