ego-motion compensation

gistfile1.py

import cv2
import sys
import numpy as np

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

feature_params = dict(maxCorners=1000,
                      qualityLevel=0.01,
                      minDistance=8,
                      blockSize=3)
                      
subpix_params = dict(zeroZone=(-1, -1), winSize=(10, 10),
                     criteria=(cv2.TERM_CRITERIA_COUNT | cv2.TERM_CRITERIA_EPS, 20, 0.03))


def identity_homography():
    # return np.zeros((3,3), dtype=np.float32)
    return np.eye(3)


def curl(h):
    return abs(h[1, 0] - h[0, 1])


def deformation(h):
    return abs(h[0, 0] - h[1, 1])

def calculate_points_lk(srcgray, dstgray):
    points0 = cv2.goodFeaturesToTrack(dstgray, **feature_params)
    cv2.cornerSubPix(dstgray, points0, **subpix_params)
    if points0 is None:
        return None, None

    points1, status, err = cv2.calcOpticalFlowPyrLK(srcgray, dstgray, points0, None, **lk_params)
    return points0, points1

def egomotion(cam, curl_threshold=0.03, deformation_threshold=0.02):
    H = [identity_homography()]
    count = 0
    ret, src = cam.read()
    srcgray = cv2.cvtColor(src, cv2.COLOR_BGR2GRAY)
    srcgray = cv2.equalizeHist(srcgray)
    while True:
        ret, dst = cam.read()
        dstgray = cv2.cvtColor(dst, cv2.COLOR_BGR2GRAY)
        dstgray = cv2.equalizeHist(dstgray)
        
        points0, points1 = calculate_points_lk(srcgray, dstgray)

        if points0 is None or points1 is None:
            continue

        h, status_homo = cv2.findHomography(points0, points1, cv2.RANSAC, 1)
        height, width = dst.shape[:2]
        prev_h = H[count]
        new_h = np.dot(h, prev_h)
        c = curl(new_h)
        d = deformation(new_h)
        # print c,d
        if c > curl_threshold or d > deformation_threshold:
            new_h = identity_homography()

        dstwarp = cv2.warpPerspective(dst, prev_h, (width, height))
        graywarp = cv2.warpPerspective(dstgray, prev_h, (width, height))
        yield dstwarp, graywarp, prev_h

        srcgray = dstgray
        src = dst
        H.append(new_h)
        count += 1

if __name__ == "__main__":
    filename = sys.argv[1]
    cap = cv2.VideoCapture()
    cap.open(filename)
    for image,imagegray,homography in egomotion(cap):
        cv2.imshow("Ego motion", image)
        ch = 0xFF & cv2.waitKey(10)