model51.py-20170919

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Mon Sep 18 14:12:01 2017

@author: vicky
"""

import numpy as np
import cv2
import matplotlib.pyplot as plt
import sys
from matplotlib.animation import FuncAnimation
from matplotlib import animation

cap1 = cv2.VideoCapture('/Users/vicky/Desktop/第五问视频/4p-c0.mp4')
cap2 = cv2.VideoCapture('/Users/vicky/Desktop/第五问视频/4p-c1.mp4')
cap3 = cv2.VideoCapture('/Users/vicky/Desktop/第五问视频/4p-c2.mp4')
cap4 = cv2.VideoCapture('/Users/vicky/Desktop/第五问视频/4p-c3.mp4')
#创建一个3*3的椭圆核
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(5,5))#形态学运算，膨胀
#创建BackgroundSubtractorMOG2
fgbg = cv2.createBackgroundSubtractorMOG2()#视频的背景分割,Foreground Segmentation Algorithm
size = (int(cap1.get(cv2.CAP_PROP_FRAME_WIDTH)), int(cap1.get(cv2.CAP_PROP_FRAME_HEIGHT)))
fourcc=cv2.VideoWriter_fourcc('X','V','I','D')
out = cv2.VideoWriter('output.avi',fourcc,20.0,size)

Z=[]
firstFrame1 = None
firstFrame2 = None
firstFrame3 = None
firstFrame4 = None

top=np.zeros((56,56))

fig = plt.figure()
ax1 = fig.add_subplot(1, 1, 1, xlim=(0, 30), ylim=(0, 45))
line, = ax1.plot([], [], 'ro')

while(1):
    ret1, frame1 = cap1.read(0)
    ret2, frame2 = cap2.read(0)
    ret3, frame3 = cap3.read(0)
    ret4, frame4 = cap4.read(0)
    if ret1 == True:
        fgmask1 = fgbg.apply(frame1)
        fgmask1 = cv2.morphologyEx(fgmask1, cv2.MORPH_OPEN, kernel)
        blured1 = cv2.bilateralFilter(fgmask1, 10, 5, 5)
        blured2 = cv2.GaussianBlur(fgmask1, (3, 3), 0)
        fgmask1 = blured2
        fgmask1 = closing = cv2.morphologyEx(fgmask1, cv2.MORPH_CLOSE, kernel)
        #寻找视频中的轮廓
        im1, contours1, hierarchy1 = cv2.findContours(fgmask1, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        for c in contours1:
            perimeter = cv2.arcLength(c,True)
            #Z.append(perimeter)
            if perimeter >250:   
                # CV2.moments會傳回一系列的moments值，我們只要知道中點X, Y的取得方式是如下進行即可。
                M1 = cv2.moments(c)
                print
                cX = int(M1['m10'] / M1['m00'])
                cY = int(M1['m01'] / M1['m00'])
                # 在中心點畫上黃色實心圓
                cv2.circle(frame1, (cX, cY), 5, (1, 227, 254), -1)
                
                #找到一个直矩形（不会旋转）
                x,y,w,h1 = cv2.boundingRect(c)
                #画出这个矩形
                cv2.rectangle(frame1,(x,y),(x+w,y+h1),(0,255,0),2)
                
                # 用红色表示有旋转角度的矩形框架
                rect = cv2.minAreaRect(c)
                box = cv2.boxPoints(rect)
                box = np.int0(box)
                cv2.drawContours(frame1, [box], 0, (0, 0, 255), 2)
                #cv2.imwrite('contours.png', frame)               
                #单目测距
                d1=0.01*h1/1.8
                
                    
        grid=np.zeros((30,45))
        c1=[0,0,0]#相机位置
        for i in range(30):
            for j in range(45):
                if (np.linalg.norm(np.array([i,j,1.8])-np.array(c1))<1.1*d1 and
                    np.linalg.norm(np.array([i,j,1.8])-np.array(c1))>0.9*d1):
                    grid[i,j]=np.linalg.norm(np.array([i,j,1.8])-np.array(c1))
                # if (np.linalg.norm(np.array([i,j,1.8])-np.array(c2))<1.1*d2 and
                #     np.linalg.norm(np.array([i,j,1.8])-np.array(c2))>0.9*d2):
                #     grid[i,j]=np.linalg.norm(np.array([i,j,1.8])-np.array(c2))
                # if (np.linalg.norm(np.array([i,j,1.8])-np.array(c3))<1.1*d3 and
                #     np.linalg.norm(np.array([i,j,1.8])-np.array(c3))>0.9*d3):
                #     grid[i,j]=np.linalg.norm(np.array([i,j,1.8])-np.array(c3))
                # if (np.linalg.norm(np.array([i,j,1.8])-np.array(c4))<1.1*d4 and
                #     np.linalg.norm(np.array([i,j,1.8])-np.array(c4))>0.9*d4):
                #     grid[i,j]=np.linalg.norm(np.array([i,j,1.8])-np.array(c4))
        
#        xdata1=max(grid)
#        ydata1=max(grid)

        out.write(fgmask1)
        cv2.imshow('frame1',frame1)
        cv2.imshow('fgmask1',fgmask1)
    
    else:
        break

    if cv2.waitKey(25) & 0xFF == ord('q'):
        break
    
out.release()
cap1.release()
cv2.destroyAllWindows()