python opencv单层光流法(高斯牛顿法求解)

使用jupyter (参考视觉SLAM十四讲第二版第八章源码)

1.使用opencv自带函数

import cv2 as cv
import numpy as np
from  matplotlib import pyplot as plt
%matplotlib inline
# 读取图片
im1 = cv.imread('./data/LK1.png',0)
im2 = cv.imread('./data/LK2.png',0)

# 提取关键点
gftt = cv.GFTTDetector_create(500,0.01,20)
kp1 = gftt.detect(im1)
kp = []
for p in kp1:
     kp.append(p.pt)
kp1 = np.array(kp,dtype = np.float32)

# 光流法提取配准点
kp2, st, _ = cv.calcOpticalFlowPyrLK(im1, im2, kp1, None, None, None, (21,21),0)

# 可视化
#im_opencv = im2.copy()
im_opencv = cv.cvtColor(im2,cv.COLOR_GRAY2BGR)
for i in range(kp1.shape[0]):
    if st[i]:
        im_opencv = cv.circle(im_opencv, (kp2[i][0].astype(int),kp2[i][1].astype(int)), 2,(0, 250, 0), -1)
        im_opencv = cv.line(im_opencv,(kp1[i][0].astype(int),kp1[i][1].astype(int)),(kp2[i][0].astype(int),kp2[i][1].astype(int)),(0,255,0))
plt.imshow(im_opencv)
plt.show()

 

2.手写高斯牛顿法求解(运行时间较长)

import cv2 as cv
import numpy as np
from  matplotlib import pyplot as plt
%matplotlib notebook
# 读取图片
im1 = cv.imread('./data/LK1.png',0)
im2 = cv.imread('./data/LK2.png',0)

# 提取关键点
gftt = cv.GFTTDetector_create(500,0.01,20)
kp1 = gftt.detect(im1)
kp = []
for p in kp1:
     kp.append(p.pt)
kp1 = np.array(kp,dtype = np.float32)

def getpix(im, x, y):
    # 边界约束
    if x < 0: x = 0
    if y < 0: y = 0
    if (x+1 >= im.shape[1]): x = im.shape[1] - 2
    if (y+1 >= im.shape[0]): y = im.shape[0] - 2 
    # 近似插值比例
    xx = x - np.floor(x)  
    yy = y - np.floor(y)
    # 周围像素点 * 权重  插值像素值 
    pix0 = im[ int(y)  , int(x)  ] * (1-xx)* (1-yy)
    pix1 = im[ int(y)  , int(x)+1] * xx    * (1-yy)
    pix2 = im[ int(y)+1, int(x)  ] * (1-xx)* yy
    pix3 = im[ int(y)+1, int(x)+1] * xx    * yy
    return (pix0 + pix1 + pix2 + pix3)

# 高斯牛顿 单层光流
half_size = 21
kp2 = []
for p in kp1:
    #print(p)
    flag = 1      # 标志位
    dx = 0
    dy = 0
    last_cost = 0
    for iter in range(10):
        cost = 0
        H = 0
        b = 0
        # 小块像素 
        for x in range(-half_size,half_size):
            for y in range(-half_size,half_size):
                e = getpix(im1,p[0]+x,p[1]+y) - getpix(im2,p[0]+x+dx,p[1]+y+dy) # 误差
                cost += e*e
                J = [[0.5 * (getpix(im2, p[0] + dx + x + 1, p[1] + dy + y) - getpix(im2, p[0] + dx + x - 1, p[1] + dy + y))],
                     [0.5 * (getpix(im2, p[0] + dx + x, p[1] + dy + y + 1) - getpix(im2, p[0] + dx + x, p[1] + dy + y - 1))]]
                J = np.array(J)          # 以上建立雅克比矩阵
                H += np.dot(J,J.T)       # 得到H
                b += J * e        # 得到b 

        if iter>0 and cost>=last_cost:# 误差变大
            #print('cost')
            break
        last_cost = cost
        if np.linalg.det(H) == 0:     # 奇异矩阵 
            flag = 0
            break
        x = np.linalg.solve(H,b)      # 求解方程组 得到增量
        dx += x[0][0]                 # 更新坐标增量
        dy += x[1][0]
        if np.linalg.norm(x) < 1e-2:  # 增量很小 
            break
    pp = [p[0] + dx,p[1] + dy]
    #print(p)
    #print(pp)
    #break
    if flag == 1: kp2.append(pp)
    else:         kp2.append([0,0])
kp2 = np.array(kp2)
#print(kp2)

im2 = cv.cvtColor(im2,cv.COLOR_GRAY2BGR)
for i in range(kp2.shape[0]):
    if kp2[i][0]!=0:
        im2 = cv.circle(im2, (kp2[i][0].astype(int),kp2[i][1].astype(int)), 2,(0, 250, 0), -1)
        im2 = cv.line(im2,(kp1[i][0].astype(int),kp1[i][1].astype(int)),(kp2[i][0].astype(int),kp2[i][1].astype(int)),(0,255,0))
plt.imshow(im2)
plt.show()
#print(kp2)