# 光流追蹤
# 光流追蹤的前提是：1. 對象的像素強度在連續幀之間不會改變；2. 相鄰像素具有相似的運動。
# - cv2.goodFeaturesToTrack() 確定要追蹤的特征點
# - cv2.calcOpticalFlowPyrLK() 追蹤視頻中的特征點

# 取第一幀，檢測其中的一些 Shi-Tomasi 角點，使用 Lucas-Kanade 光流迭代跟蹤這些點。
# 對于函數 cv2.calcOpticalFlowPyrLK() 傳遞前一幀、前一個點和下一幀。它返回下一個點以及一些狀態編號，如果找到下一個點，則值為 1，否則為零。
# 然后在下一步中迭代地將這些下一個點作為前一個點傳遞。

# USAGE
# python video_optical_flow.py

import imutils
import numpy as np
import cv2

cap = cv2.VideoCapture('images/slow_traffic_small.mp4')

# ShiTomasi角點檢測的參數
feature_params = dict(maxCorners=100,
                      qualityLevel=0.3,
                      minDistance=7,
                      blockSize=7)

# Lucas Kanada光流檢測的參數
lk_params = dict(winSize=(15, 15),
                 maxLevel=2,
                 criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

# 構建隨機顏色
color = np.random.randint(0, 255, (100, 3))

# 獲取第一幀并發現角點
ret, old_frame = cap.read()
old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)
p0 = cv2.goodFeaturesToTrack(old_gray, mask=None, **feature_params)

# 為繪制光流追蹤圖，構建一個Mask
mask = np.zeros_like(old_frame)

num = 0
while (1):
    ret, frame = cap.read()

    if not ret:
        break

    frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # 使用迭代Lucas Kanade方法計算稀疏特征集的光流
    # - old_gray: 上一幀單通道灰度圖
    # - frame_gray: 下一幀單通道灰度圖
    # - prePts：p0上一幀坐標pts
    # - nextPts: None
    # - winSize: 每個金字塔級別上搜索窗口的大小
    # - maxLevel: 最大金字塔層數
    # - criteria：指定迭代搜索算法的終止條件，在指定的最大迭代次數criteria.maxCount之后或搜索窗口移動小于criteria.epsilon
    p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None, **lk_params)

    # 選擇軌跡點
    good_new = p1[st == 1]
    good_old = p0[st == 1]

    # 繪制軌跡
    for i, (new, old) in enumerate(zip(good_new, good_old)):
        a, b = new.ravel()
        c, d = old.ravel()
        mask = cv2.line(mask, (a, b), (c, d), color[i].tolist(), 2)
        frame = cv2.circle(frame, (a, b), 5, color[i].tolist(), -1)
    img = cv2.add(frame, mask)

    cv2.imshow('frame', img)
    cv2.imwrite('videoof-imgs/' + str(num) + '.jpg', imutils.resize(img, 500))
    print(str(num))
    num = num + 1
    k = cv2.waitKey(30)  0xff
    if k == 27:
        break

    # 更新之前的幀和點
    old_gray = frame_gray.copy()
    p0 = good_new.reshape(-1, 1, 2)

cv2.destroyAllWindows()
cap.release()

# 優化后的光流追蹤—Lucas-Kanade tracker
# （當不見檢查下一個關鍵點的正確程度時，即使圖像中的任何特征點消失，光流也有可能找到下一個看起來可能靠近它的點。實際上對于穩健的跟蹤，角點應該在特定的時間間隔內檢測點。
# 找到特征點后，每 30 幀對光流點的向后檢查，只選擇好的。）
# Lucas Kanade稀疏光流演示。使用GoodFeatures跟蹤用于跟蹤初始化和匹配驗證的回溯幀之間。
# Lucas-Kanade sparse optical flow demo. Uses goodFeaturesToTrack for track initialization and back-tracking for match verification between frames.

# Usage
# pyhton lk_track.py images/slow_traffic_small.mp4
# 按 ESC鍵退出

from __future__ import print_function

import imutils
import numpy as np
import cv2


def draw_str(dst, target, s):
    x, y = target
    cv2.putText(dst, s, (x + 1, y + 1), cv2.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 0), thickness=2, lineType=cv2.LINE_AA)
    cv2.putText(dst, s, (x, y), cv2.FONT_HERSHEY_PLAIN, 1.0, (255, 255, 255), lineType=cv2.LINE_AA)


lk_params = dict(winSize=(15, 15),
                 maxLevel=2,
                 criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

feature_params = dict(maxCorners=500,
                      qualityLevel=0.3,
                      minDistance=7,
                      blockSize=7)


class App:
    def __init__(self, video_src):
        self.track_len = 10
        self.detect_interval = 30
        self.tracks = []
        self.cam = cv2.VideoCapture(video_src)
        self.frame_idx = 0

    def run(self):
        while True:
            _ret, frame = self.cam.read()
            if not _ret:
                break

            frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
            vis = frame.copy()

            if len(self.tracks) > 0:
                img0, img1 = self.prev_gray, frame_gray
                p0 = np.float32([tr[-1] for tr in self.tracks]).reshape(-1, 1, 2)
                p1, _st, _err = cv2.calcOpticalFlowPyrLK(img0, img1, p0, None, **lk_params)
                p0r, _st, _err = cv2.calcOpticalFlowPyrLK(img1, img0, p1, None, **lk_params)
                d = abs(p0 - p0r).reshape(-1, 2).max(-1)
                good = d  1
                new_tracks = []
                for tr, (x, y), good_flag in zip(self.tracks, p1.reshape(-1, 2), good):
                    if not good_flag:
                        continue
                    tr.append((x, y))
                    if len(tr) > self.track_len:
                        del tr[0]
                    new_tracks.append(tr)
                    cv2.circle(vis, (x, y), 2, (0, 255, 0), -1)
                self.tracks = new_tracks
                cv2.polylines(vis, [np.int32(tr) for tr in self.tracks], False, (0, 255, 0))
                draw_str(vis, (20, 20), 'track count: %d' % len(self.tracks))

            if self.frame_idx % self.detect_interval == 0:
                mask = np.zeros_like(frame_gray)
                mask[:] = 255
                for x, y in [np.int32(tr[-1]) for tr in self.tracks]:
                    cv2.circle(mask, (x, y), 5, 0, -1)
                p = cv2.goodFeaturesToTrack(frame_gray, mask=mask, **feature_params)
                if p is not None:
                    for x, y in np.float32(p).reshape(-1, 2):
                        self.tracks.append([(x, y)])

            self.prev_gray = frame_gray
            cv2.imshow('lk_track', vis)
            print(self.frame_idx)
            cv2.imwrite('videoOof-imgs/' + str(self.frame_idx) + '.jpg', imutils.resize(vis, 500))
            self.frame_idx += 1

            ch = cv2.waitKey(1)
            if ch == 27:
                break

def main():
    import sys
    try:
        video_src = sys.argv[1]
    except:
        video_src = 0

    App(video_src).run()
    print('Done')


if __name__ == '__main__':
    print(__doc__)
    main()
    cv2.destroyAllWindows()

# OpenCV中的密集光流
# Lucas-Kanade 方法計算稀疏特征集的光流（使用 Shi-Tomasi 算法檢測到的角點）。
# OpenCV 提供了另一種算法: Gunner Farneback 來尋找密集光流。它計算幀中所有點的光流。
# 通過cv2.calcOpticalFlowFarneback() 將得到一個帶有光流向量 (u,v) 的 2 通道陣列。可以找到它們的大小和方向，然后對結果進行顏色編碼以實現更好的可視化。
# 在HSV圖像中，方向對應于圖像的色調，幅度對應于價值平面。

import cv2
import imutils
import numpy as np

cap = cv2.VideoCapture('images/slow_traffic_small.mp4')

ret, frame1 = cap.read()
prvs = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)
hsv = np.zeros_like(frame1)
hsv[..., 1] = 255

num = 0
while (1):
    ret, frame2 = cap.read()

    if not ret:
        break
    next = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY)

    # 使用迭代Gunner Farneback 方法計算密集特征的光流
    # - prvs: 上一幀單通道灰度圖
    # - next: 下一幀單通道灰度圖
    # - flow: 流 None
    # - pyr_scale: 0.5經典金字塔，構建金字塔縮放scale
    # - level：3 初始圖像的金字塔層數
    # - winsize：3 平均窗口大小，數值越大，算法對圖像的魯棒性越強
    # - iterations：15 迭代次數
    # - poly_n：5 像素鄰域的參數多邊形大小，用于在每個像素中找到多項式展開式；較大的值意味著圖像將使用更平滑的曲面進行近似，從而產生更高的分辨率、魯棒算法和更模糊的運動場；通常多邊形n=5或7。
    # - poly_sigma：1.2 高斯標準差，用于平滑導數
    # - flags: 可以是以下操作標志的組合：OPTFLOW_USE_INITIAL_FLOW：使用輸入流作為初始流近似值。OPTFLOW_FARNEBACK_GAUSSIAN: 使用GAUSSIAN過濾器而不是相同尺寸的盒過濾器；
    flow = cv2.calcOpticalFlowFarneback(prvs, next, None, 0.5, 3, 15, 3, 5, 1.2, 0)

    mag, ang = cv2.cartToPolar(flow[..., 0], flow[..., 1])
    hsv[..., 0] = ang * 180 / np.pi / 2
    hsv[..., 2] = cv2.normalize(mag, None, 0, 255, cv2.NORM_MINMAX)
    rgb = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)

    cv2.imshow('Origin VS frame2', np.hstack([frame2, rgb]))
    cv2.imwrite('dof-imgs/' + str(num) + '.jpg', imutils.resize(np.hstack([frame2, rgb]), 600))
    k = cv2.waitKey(30)  0xff
    num = num + 1
    if k == 27:
        break
    elif k == ord('s'):
        cv2.imwrite('dof-imgs/origin VS dense optical flow HSVres' + str(num) + ".jpg",
                    imutils.resize(np.hstack([frame2, rgb]), width=800))
    prvs = next

cap.release()
cv2.destroyAllWindows()