Related
I get this error from the following Pytorch code:
RuntimeError: one of the variables needed for gradient computation has been modified by an inplace operation: [torch.DoubleTensor [3]] is at version 10; expected version 9 instead.
As it is seen the code does not have inplace operations.
import torch
device = torch.device('cpu')
class MesNet(torch.nn.Module):
def __init__(self):
super(MesNet, self).__init__()
self.cov_lin = torch.nn.Sequential(torch.nn.Linear(6, 5)).double()
def forward(self, u):
z_cov = self.cov_lin(u.transpose(0, 2).squeeze(-1))
return z_cov
class UpdateModel(torch.nn.Module):
def __init__(self):
torch.nn.Module.__init__(self)
self.P_dim = 18
self.Id3 = torch.eye(3).double()
def run_KF(self):
N = 10
u = torch.randn(N, 6).double()
v = torch.zeros(N, 3).double()
model = MesNet()
measurements_covs_l = model(u.t().unsqueeze(0))
# remember to remove this afterwards
torch.autograd.set_detect_anomaly(True)
for i in range(1, N):
v[i] = self.update_pos(v[i].detach(), measurements_covs_l[i-1])
criterion = torch.nn.MSELoss(reduction="sum")
targ = torch.rand(10, 3).double()
loss = criterion(v, targ)
loss = torch.mean(loss)
loss.backward()
return v, p
def update_pos(self, v, measurement_cov):
Omega = torch.eye(3).double()
H = torch.ones((5, self.P_dim)).double()
R = torch.diag(measurement_cov)
Kt = H.t().mm(torch.inverse(R))
# it is indicating inplace error even with this:
# Kt = H.t().mm(R)
dx = Kt.mv(torch.ones(5).double())
dR = self.trans(dx[:9].clone())
v_up = dR.mv(v)
return v_up
def trans(self, xi):
phi = xi[:3].clone()
angle = torch.norm(phi.clone())
if angle.abs().lt(1e-10):
skew_phi = torch.eye(3).double()
J = self.Id3 + 0.5 * skew_phi
Rot = self.Id3 + skew_phi
else:
axis = phi / angle
skew_axis = torch.eye(3).double()
s = torch.sin(angle)
c = torch.cos(angle)
Rot = c * self.Id3
return Rot
net = UpdateModel()
net.run_KF()
I think the issue is that you are overwriting v[i] elements.
You could instead construct an auxiliary list v_ from the loop, then convert it tensor:
v_ = [v[0]]
for i in range(1, N):
v_.append(self.update_pos(v[i].detach(), measurements_covs_l[i-1]))
v = torch.stack(v_)
i have been given a matrix space of 400,400 with few obstacles inside . I have eight set of actions namely up, down, left right with edge cost of 1 and 4 diagonals with edge cost of 1.42. i took initial node as (5,5) and final node as (195,295). I'm able to reach the goal using Dijkstra's algorithm quite quickly but am not able to backtrack and have been stuck for a long time. Can someone please help me. Here's my code
import numpy as np
import matplotlib.pyplot as plt
import time
import math
blank_list = []
for entries in range(160000):
entries = math.inf
blank_list.append(entries)
initial_matrix = np.array(blank_list, dtype = object).reshape(400, 400)
rows = initial_matrix.shape[0]
cols = initial_matrix.shape[1]
flag = False
# Obstacles
for h in range(0, rows):
for k in range(0, cols):
if (k-225)**2 + (h-50)**2 <= 625:
initial_matrix[h][k] = flag
for h in range(0, rows):
for k in range(0, cols):
if h<= 13*k -140 and h<= 185 and h<= -(7/5)*k +290 and h<= (6/5)*k +30 and h<= -(6/5)*k +210 and h<= k + 100:
initial_matrix[h][k] = flag
for h in range(0, rows):
for k in range(0, cols):
if ((k-150)**2)/1600 + ((h-100)**2)/400 <= 1:
initial_matrix[h][k] = flag
# move subfunctions
def move_right(i,j, map_):
if not map_[i,j+1]:
return None
if j == 300:
return None
else:
cost = initial_matrix[i][j] + 1
j = j + 1
return (i,j) , cost
def move_left(i,j,map_):
if not map_[i,j-1]:
return None
if j == 0:
return None
else:
cost = initial_matrix[i][j] + 1
j = j - 1
return (i,j) , cost
def move_up(i,j,map_):
if not map_[i-1,j]:
return None
if i == 0:
return None
else:
cost = initial_matrix[i][j] + 1
i = i - 1
return (i,j) , cost
def move_down(i,j,map_):
if not map_[i+1,j]:
return None
if i == 200:
return None
else:
cost = initial_matrix[i][j] + 1
i = i + 1
return (i,j) , cost
def move_upright(i,j,map_):
if not map_[i-1,j+1]:
return None
if j == 300 or i == 0:
return None
else:
cost = 1.42 + initial_matrix[i][j]
j = j + 1
i = i - 1
return (i,j) , cost
def move_downright(i,j,map_):
if not map_[i+1,j+1]:
return None
if j == 300 or i == 200:
return None
else:
cost = 1.42 + initial_matrix[i][j]
j = j + 1
i = i + 1
return (i,j) , cost
def move_upleft(i,j,map_):
if map_[i-1,j-1]:
return None
if j == 0 or i == 0:
return None
else:
cost = 1.42 + initial_matrix[i][j]
j = j - 1
i = i - 1
return (i,j) , cost
def move_downleft(i,j,map_):
if not map_[i+1,j-1]:
return None
if j == 0 or i == 200:
return None
else:
cost = 1.42 + initial_matrix[i][j]
j = j - 1
i = i + 1
return (i,j) , cost
def get_neighbours(i,j):
neighbours_cost = []
index = []
action_up = move_up(i,j,initial_matrix)
if action_up is not None:
neighbours_cost.append(action_up[1])
index.append(action_up[0])
action_down = move_down(i,j,initial_matrix)
if action_down is not None:
neighbours_cost.append(action_down[1])
index.append(action_down[0])
action_left = move_left(i,j,initial_matrix)
if action_left is not None:
neighbours_cost.append(action_left[1])
index.append(action_left[0])
action_right = move_right(i,j,initial_matrix)
if action_right is not None:
neighbours_cost.append(action_right[1])
index.append(action_right[0])
action_upright = move_upright(i,j,initial_matrix)
if action_upright is not None:
neighbours_cost.append(action_upright[1])
index.append(action_upright[0])
action_downright = move_downright(i,j,initial_matrix)
if action_downright is not None:
neighbours_cost.append(action_downright[1])
index.append(action_downright[0])
action_upleft = move_upleft(i,j,initial_matrix)
if action_upleft is not None:
neighbours_cost.append(action_upleft[1])
index.append(action_upleft[0])
action_downleft = move_downleft(i,j,initial_matrix)
if action_downleft is not None:
neighbours_cost.append(action_downleft[1])
index.append(action_downleft[0])
return neighbours_cost, index
'''def get_current_value(i,j):
current_value = initial_matrix[i][j]
return current_value'''
'''def locate_value_index(mat):
i,j = np.where(mat == node)
i = int(i)
j = int(j)
return (i,j)'''
def sort_list1(list_a,list_b):
list_b = [i[1] for i in sorted(zip(list_a, list_b))]
def sort_list2(list_a):
list_a.sort()
goal = (195,295)
ind = (5,5)
initial_matrix[ind[0]][ind[1]] = 0
node_cost = 0
explore_queue = []
index_queue = []
index_queue.append(ind)
explore_queue.append(node_cost)
visited = set()
breakwhile = False
start_time = time.clock()
while len(index_queue) != 0 and not breakwhile:
node = index_queue[0]
visited.add(node)
#for i in range(len(index_queue)):
index_queue.pop(0)
explore_queue.pop(0)
#print(visited)
#val = get_current_value(ind[0],ind[1])
pair = get_neighbours(node[0],node[1])
neighbours_cost = pair[0]
index = pair[1]
#print(index)
#print(neighbours_cost)
#######
if node == goal:
print('goal reached')
breakwhile = True
#######
for i in range(len(index)):
if index[i] not in visited:
old_cost = initial_matrix[index[i][0]][index[i][1]]
if neighbours_cost[i] < old_cost:
initial_matrix[index[i][0]][index[i][1]] = neighbours_cost[i]
if old_cost != math.inf:
ind_node = index_queue.index((index[i][0], index[i][1]))
index_queue.pop(ind_node)
explore_queue.pop(ind_node)
index_queue.append(index[i])
explore_queue.append(initial_matrix[index[i][0]][index[i][1]])
sort_list1(explore_queue,index_queue)
sort_list2(explore_queue)
end_time = time.clock()
print(end_time - start_time)
I figured out the solution. it is to create dictionary with all the index values and iterating them over a while loop
I coded a general convolution function in Python for CNNs.
As it turned out the time taken for this function was almost 5x more than the Keras Conv2D takes.
So I was curious if anyone knows why is there a speed difference ?
(It took almost 10-15min for 1 epoch of MNIST Dataset for my convolution function. Whereas Keras does it in almost 3-4min)
Heres my Conv class :
class Convolutional2D(Layer):
def __init__(self, kernel_size, feature_maps):
self.kernel_size = kernel_size
self.feature_maps = feature_maps
self.b = np.zeros((feature_maps))#np.random.rand(feature_maps)
def connect(self, to_layer):
if len(to_layer.layer_shape) == 2:
kernel_shape = [self.feature_maps, self.kernel_size, self.kernel_size]
self.layer_shape = [self.feature_maps] + list(np.array(to_layer.layer_shape)-self.kernel_size+1)
else:
kernel_shape = [self.feature_maps, to_layer.layer_shape[0], self.kernel_size, self.kernel_size]
self.layer_shape = [self.feature_maps] + list(np.array(to_layer.layer_shape[1:])-self.kernel_size+1)
self.kernel = np.random.random(kernel_shape)
super().init_adam_params(self.kernel, self.b)
def convolve(self, x, k, mode='forward'):
if mode == 'forward':
ksize = k.shape[-1]
if len(x.shape) == 3:
out = np.zeros((x.shape[0], k.shape[0], x.shape[1]-k.shape[1]+1, x.shape[2]-k.shape[2]+1))
else:
out = np.zeros((x.shape[0], k.shape[0], x.shape[2]-k.shape[2]+1, x.shape[3]-k.shape[3]+1))
for i in range(out.shape[2]):
for j in range(out.shape[3]):
if len(x.shape) == 3:
window = x[:,i:i+ksize,j:j+ksize]
m = np.reshape(window, (window.shape[0], 1, window.shape[1], window.shape[2]))*k
m = np.sum(m, axis=(2,3))
else:
window = x[:,:,i:i+ksize,j:j+ksize]
m = np.reshape(window, (window.shape[0], 1, window.shape[1], window.shape[2], window.shape[3]))*k
m = np.sum(m, axis=(2,3,4))
out[:,:,i,j] = m
return out
elif mode == 'backward_i':
if len(k.shape) == 3:
out = np.zeros((x.shape[0], x.shape[2]+k.shape[1]-1, x.shape[3]+k.shape[2]-1))
x = np.pad(x, ((0, 0), (0, 0), (k.shape[1]-1, k.shape[1]-1), (k.shape[2]-1, k.shape[2]-1)))
else:
out = np.zeros((x.shape[0], k.shape[1], x.shape[2]+k.shape[2]-1, x.shape[3]+k.shape[3]-1))
x = np.pad(x, ((0, 0), (0, 0), (k.shape[2]-1, k.shape[2]-1), (k.shape[3]-1, k.shape[3]-1)))
fk = np.transpose(k, axes=(1,0,2,3))
x = np.reshape(x, (x.shape[0], 1, x.shape[1], x.shape[2], x.shape[3]))
ksize = k.shape[-1]
for i in range(out.shape[-2]):
for j in range(out.shape[-1]):
if len(k.shape) == 3:
window = x[:,:,i:i+ksize,j:j+ksize]
m = window*k
m = np.sum(m, axis=(1,2,3))
out[:,i,j] = m
else:
window = x[:,:,:,i:i+ksize,j:j+ksize]
m = window*fk
m = np.sum(m, axis=(2,3,4))
out[:,:,i,j] = m
return out
elif mode == 'backward_k':
if len(x.shape) == 3:
out = np.zeros((k.shape[1], x.shape[1]-k.shape[2]+1, x.shape[2]-k.shape[3]+1))
else:
out = np.zeros((k.shape[1], x.shape[1], x.shape[2]-k.shape[2]+1, x.shape[3]-k.shape[3]+1))
x = np.transpose(x, axes=(1,0,2,3))
x = np.reshape(x, (x.shape[0], x.shape[1], x.shape[2], x.shape[3]))
ksize = k.shape[-1]
k = np.transpose(k, axes=(1,0,2,3))
if len(x.shape) != 3:
fk = np.reshape(k, (k.shape[0], 1, k.shape[1], k.shape[2], k.shape[3]))
for i in range(out.shape[-2]):
for j in range(out.shape[-1]):
if len(x.shape) == 3:
window = x[:,i:i+ksize,j:j+ksize]
m = window*k
m = np.sum(m, axis=(1,2,3))
out[:,i,j] = m
else:
window = x[:,:,i:i+ksize,j:j+ksize]
m = window*fk
m = np.sum(m, axis=(2,3,4))
out[:,:,i,j] = m
return out
def forward(self, x):
return self.convolve(x, self.kernel)
def backward(self, x, loss_grad, params):
if len(self.kernel.shape) == 3:
flipped_kernel = np.flip(self.kernel, axis=(1,2))
flipped_loss_grad = np.flip(loss_grad, axis=(1,2))
else:
flipped_kernel = np.flip(self.kernel, axis=(2,3))
flipped_loss_grad = np.flip(loss_grad, axis=(2,3))
i_grad = self.convolve(loss_grad, flipped_kernel, mode='backward_i')
k_grad = self.convolve(x, flipped_loss_grad, mode='backward_k')
self.vw = params['beta1']*self.vw + (1-params['beta1'])*k_grad
self.sw = params['beta2']*self.sw + (1-params['beta2'])*(k_grad**2)
self.kernel += params['lr']*self.vw/np.sqrt(self.sw+params['eps'])
return i_grad
def get_save_data(self):
return {'type':'Convolutional2D', 'shape':np.array(self.layer_shape).tolist(), 'data':[self.kernel_size, self.feature_maps, self.kernel.tolist()]}
def load_saved_data(data):
obj = Convolutional2D(data['data'][0], data['data'][1])
obj.layer_shape = data['shape']
obj.kernel = np.array(data['data'][2])
obj.init_adam_params(obj.kernel, obj.b)
return obj
Keras and Pytorch are much more efficient because they take advantage of vectorization and the fact that matrix multiplication is very well optimized. They basically convert the convolution into a matrix multiplication by flattening the filter and creating a new matrix whose column values are the values of each block. They also take advantage of how the data is stored in memory. You can find more information in this article: https://towardsdatascience.com/how-are-convolutions-actually-performed-under-the-hood-226523ce7fbf
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
from scipy import linalg, optimize
%matplotlib inline
Data load
data = pd.read_csv("D:/Stat/TimeSeries/KRW_month_0617_1.csv",index_col="Date") / 100
para = open("D:/Stat/TimeSeries/KRW_month_0617_1.txt").readlines()[0:2]
data.index = pd.to_datetime(data.index)
Parameters
cond = []
params = []
time = []
for i in para:
j = i.split()
for k in j:
cond.append(k)
cond = cond[1:]
for i in range(len(cond)):
cond[i] = round(float(cond[i]),4)
params = cond[0:23]
time = cond[23:]
maturity = np.array(time[1:])
timegap = 1/cond[23]
Functions We need
def Paramcheck(Params, checkStationary = 1):
result = 0
Kappa = np.array([[params[20],0,0], [0,params[21],0], [0,0,params[22]]])
Sigma = np.array([[params[1],0,0], [params[2],params[3],0], [params[4],params[5],params[6]]])
State = np.array([params[7], params[8], params[9]])
Lambda = params[0]
SigmaEps = np.identity(10)
for i in range(10):
SigmaEps[i][i] = params[i+10]
for i in range(len(Sigma)):
if Sigma[i][i] < 0:
result = 1
for j in SigmaEps:
if np.any(SigmaEps) < 0:
result = 1
if Lambda < 0.05 or Lambda > 2:
result = 1
elif State[0] < 0:
result = 1
elif Kappa[0][0] < 0:
result = 1
if result == 0 and checkStationary > 0:
if max(np.linalg.eigvals(-Kappa).real) > 0:
result = 2
return result
def CheckDet(x):
if x == np.inf or x == np.nan:
result = 1
elif x < 0:
result = 2
elif abs(x) < 10**-250:
result = 3
else:
result = 0
return result
def NS_factor(lambda_val, maturity):
col1 = np.ones(len(maturity))
col2 = (1 - np.exp(-lambda_val*maturity))/(lambda_val*maturity)
col3 = col2 - np.exp(-lambda_val*maturity)
factor = np.array([col1,col2,col3]).transpose()
return factor
def DNS_Kalman_filter(Params, *args):
N = Paramcheck(Params)
if N == 0:
Kappa = np.array([[params[20],0,0], [0,params[21],0], [0,0,params[22]]])
Sigma = np.array([[params[1],0,0], [params[2],params[3],0],
[params[4],params[5],params[6]]])
State = np.array([params[7], params[8], params[9]])
Lambda = params[0]
SigmaEps = np.identity(10)
for i in range(10):
SigmaEps[i][i] = params[i+10]
Obs_Yield = args[0]
Obs_Date = args[1]
Timegap = args[2]
Obs_Mty = args[3]
Finalstate = args[4]
Mty_length = len(Obs_Mty)
B = NS_factor(lambda_val = Lambda,maturity = Obs_Mty)
H_large = SigmaEps **2
N_obs = len(Obs_Date)
LLH_vec = np.zeros(N_obs)
phi1 = linalg.expm(-Kappa*Timegap)
phi0 = (np.identity(3)-phi1) # State
Eigenvalues = np.linalg.eig(Kappa)[0]
Eigen_vec = np.linalg.eig(Kappa)[1]
Eigen_vec_inv = np.linalg.inv(Eigen_vec)
S = Eigen_vec_inv # Sigma # Sigma.transpose() # Eigen_vec_inv.transpose()
Atilde = np.dot(Sigma[0], Sigma[0])
Btilde = np.dot(Sigma[1], Sigma[1])
Ctilde = np.dot(Sigma[2], Sigma[2])
Dtilde = np.dot(Sigma[0], Sigma[1])
Etilde = np.dot(Sigma[0], Sigma[2])
Ftilde = np.dot(Sigma[1], Sigma[2])
res1= Atilde* Obs_Mty* Obs_Mty/6
res2= Btilde*(1/(2*Lambda**2) - (1-np.exp(-Lambda*Obs_Mty))/(Lambda**3*Obs_Mty) + (1-
np.exp(-2*Lambda*Obs_Mty))/(4*Lambda**3*Obs_Mty))
res3= Ctilde*(1/(2*Lambda**2) + np.exp(-Lambda*Obs_Mty)/(Lambda**2)-
Obs_Mty*np.exp(-2*Lambda*Obs_Mty)/(4*Lambda) -
3*np.exp(-2*Lambda*Obs_Mty)/(4*Lambda**2) - 2*(1-np.exp(-
Lambda*Obs_Mty))/(Lambda**3*Obs_Mty) + 5*(1-
np.exp(-2*Lambda*Obs_Mty))/(8*Lambda**3*Obs_Mty))
res4= Dtilde*(Obs_Mty/(2*Lambda) + np.exp(-Lambda*Obs_Mty)/(Lambda**2) - (1-np.exp(-
Lambda*Obs_Mty))/(Lambda**3*Obs_Mty))
res5= Etilde*(3*np.exp(-Lambda*Obs_Mty)/(Lambda**2) + Obs_Mty/(2*Lambda)+Obs_Mty*np.exp(-
Lambda*Obs_Mty)/(Lambda) - 3*(1-np.exp(-Lambda*Obs_Mty))/(Lambda**3*Obs_Mty))
res6= Ftilde*(1/(Lambda**2) + np.exp(-Lambda*Obs_Mty)/(Lambda**2) -
np.exp(-2*Lambda*Obs_Mty)/(2*Lambda**2) - 3*(1-np.exp(-
Lambda*Obs_Mty))/(Lambda**3*Obs_Mty) + 3*(1-
np.exp(-2*Lambda*Obs_Mty))/(4*Lambda**3*Obs_Mty))
val = res1 + res2 + res3 + res4 + res5 + res6
V_mat = np.zeros([3,3])
V_lim = np.zeros([3,3])
for i in range(3):
for j in range(3):
V_mat[i][j] = S[i][j]*(1-np.exp(-(Eigenvalues[i] +
Eigenvalues[j])*Timegap))/(Eigenvalues[i] + Eigenvalues[j])
V_lim[i][j] = S[i][j]/(Eigenvalues[i] + Eigenvalues[j])
Q = (Eigen_vec # V_mat # Eigen_vec.transpose()).real
Sigma_lim = (Eigen_vec # V_lim # Eigen_vec.transpose()).real
for i in range(N_obs):
y = Obs_Yield[i]
xhat = phi0 + phi1 # State
y_implied = B # xhat
v = y - y_implied + val
Sigmahat = phi1 # Sigma_lim # phi1.transpose() + Q
F = B # Sigmahat # B.transpose() + H_large
detF = np.linalg.det(F)
if CheckDet(detF) > 0:
N = 3
break
Finv = np.linalg.inv(F)
State = xhat + Sigmahat # B.transpose() # Finv # v
Sigma_lim = Sigmahat - Sigmahat # B.transpose() # Finv # B # Sigmahat
LLH_vec[i] = np.log(detF) + v.transpose() # Finv # v
if N == 0:
if Finalstate:
yDate = Obs_Date[-1]
result = np.array([yDate,State])
else:
result = 0.5 * (sum(LLH_vec) + Mty_length*N_obs*np.log(2*np.pi))
else:
result = 7000000
return result
I made a code that does Arbitrage Free Nelson-Siegel model. Data is return rates of bond (1Y,1.5Y, ... ,20Y). I wanna optimize that function with scipy optimize.minimize function with fixed *args.
Suppose that Initial parmas are verified that it's close to optimized params from empirical experiments using Dynamic Nelson-Siegel Model.
LLC_new = 0
while True:
LLC_old = LLC_new
OPT = optimize.minimize(x0=params,fun=DNS_Kalman_filter, args=
(data.values,data.index,timegap,maturity,0))
params = OPT.x
LLC_new = round(OPT.fun,5)
print("Current LLC: %0.5f" %LLC_new)
if LLC_old == LLC_new:
OPT_para = params
FinalState = DNS_Kalman_filter(params,data.values,data.index,timegap,maturity,True)
break
Result is
Current LLC: -7613.70146
Current LLC: -7613.70146
LLC(log-likelihood value) isn't maximized. It's not a result I desire using Optimizer.
Is there any solution for that?
In R, there is optim() function works as similar as scipy.optimize.minimize() which works really well. I also have a R code for that very similar to this Python code.
I use a code that makes use of kalman filter to stabilize a video but I am not able to record a video to be saved. I thought about saving all the frames in a file and creating a video later.
If someone has a better idea, or how to modify the code to be possible to use cv2.videoWriter.
import cv2
import numpy as np
from matplotlib import pyplot as plt
ini = time.time()
class FrameInfo:
def __init__(self):
self.img = None
self.img_gray = None
self.features = []
self.number = 0
self.trajectory = Trajectory()
#property
def width(self):
return self.img_gray.shape[1]
#property
def height(self):
return self.img_gray.shape[0]
#property
def width_height(self):
return self.img_gray.shape[::-1]
videoInPath = "../data/MVI_5408.avi"
camera = cv2.VideoCapture(videoInPath)
videoOutPath = videoInPath +'sab.avi'
N_FRAMES = int(camera.get(cv2.CAP_PROP_FRAME_COUNT))
FPS = camera.get(cv2.CAP_PROP_FPS)
VID_WIDTH = int(camera.get(cv2.CAP_PROP_FRAME_WIDTH))
VID_HEIGHT = int(camera.get(cv2.CAP_PROP_FRAME_HEIGHT))
print "N_FRAMES: " + str(N_FRAMES)
print "FPS: " + str(FPS)
frame = None
prev_frame = None
trajectory = Trajectory(0, 0, 0)
org_trajectories = []
stb_trajectories = []
crop = 40
crop_rate = crop / 20
filter = KalmanFilter(Trajectory(4e-2, 4e-2, 4e-2), Trajectory(crop_rate,
crop_rate, crop_rate), Trajectory(1, 1, 1))
#surf = cv2.SURF(4000)
prev_trans = None
frame_number = 0
frame_width = int(1336.0 / 2)
frame_height = int(768.0 / 2)
def resize(img):
return cv2.resize(img, (frame_width, frame_height),
interpolation=cv2.INTER_LANCZOS4)
lk_params = dict(winSize=(19, 19),
maxLevel=2,
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))
feature_params = dict(maxCorners=100,
qualityLevel=0.01,
minDistance=8,
blockSize=19)
crop_rate = 0.9
limits = [int(frame_width * (1 - crop_rate)), int(frame_height * (1 -
crop_rate)), 0.05]
output_video = cv2.VideoWriter(videoOutPath,
cv2.VideoWriter_fourcc(*'XVID'), # -1, #
int(video.get(cv2.cv.CV_CAP_PROP_FOURCC)),
FPS,
(frame_width - 2*limits[0], frame_height - 2*limits[1])
)
feature_cont = 0
flow_cont = 0
ransac_cont = 0
kalman_cont = 0
features_quant = []
percent = 0
inliers_quant = []
ouliers_quant = []
compensate_count = 0
for k in range(N_FRAMES-1):
# Capture frame_img-by-frame_img
ret, frame_img = camera.read()
if frame_img is None:
break
if frame is not None:
prev_frame = frame
frame_number += 1
if frame_number < 220:
continue
frame = FrameInfo()
frame.number = frame_number
frame.img = frame_img
frame.img = cv2.resize(frame_img, (0, 0), fx=(1336.0 / frame_img.shape[1]) / 2.0,
fy=(768.0 / frame_img.shape[0]) / 2.0,
interpolation=cv2.INTER_LANCZOS4)
frame.img_gray = cv2.cvtColor(frame.img, cv2.COLOR_BGR2GRAY)
feature_time_ini = time.time()
frame.features = cv2.goodFeaturesToTrack(frame.img_gray, **feature_params)
#frame.features = sift.detect(frame.img_gray,None)
features_quant.append(len(frame.features))
if prev_frame is None:
continue
feature_time_fim = time.time()
feature_cont += feature_time_fim - feature_time_ini
feature_time_ini = 0
feature_time_fim = 0
flow_time_ini = time.time()
new_features, _, _ = cv2.calcOpticalFlowPyrLK(prev_frame.img, frame.img, prev_frame.features, None, **lk_params)
new_features_for_validation, _, _ = cv2.calcOpticalFlowPyrLK(frame.img, prev_frame.img, new_features, None,
**lk_params)
flow_time_fim = time.time()
flow_cont += flow_time_fim - flow_time_ini
flow_time_ini = 0
flow_time_fim = 0
d = abs(prev_frame.features - new_features_for_validation).reshape(-1, 2).max(-1)
good = d < 1
# Select good_features points
good_new = np.array([x for x, s in zip(new_features, good) if s], dtype=np.float32)
good_old = np.array([x for x, s in zip(prev_frame.features, good) if s], dtype=np.float32)
# trans = cv2.estimateRigidTransform(good_old, good_new, fullAffine=False)
ransac_time_ini = time.time()
trans, inliers_indices,outliers_indices = RansacMotionEstimator(40, 1.0).estimate(good_old, good_new)
ransac_time_fim = time.time()
ransac_cont += ransac_time_fim - ransac_time_ini
ransac_time_ini = 0
ransac_time_fim = 0
inliers_quant.append(len(inliers_indices))
ouliers_quant.append(len(outliers_indices))
if trans is None and prev_trans is None:
print ("wuf? trans is None and prev_trans is none too")
continue
if trans is None:
trans = prev_trans
print ("wut? trans is None")
delta = Trajectory(get_x(trans), get_y(trans), get_rad_angle(trans))
trajectory += delta
kalman_time_ini = time.time()
filter.put(trajectory)
diff = filter.get() - trajectory
new_delta = delta + diff
kalman_time_fim = time.time()
org_trajectories.append(trajectory)
stb_trajectories.append(filter.get())
kalman_cont += kalman_time_fim - kalman_time_ini
kalman_time_ini = 0
kalman_time_fim = 0
fill_mat(trans, new_delta.x, new_delta.y, new_delta.angle)
compensate_time_ini = time.time()
#out = cv2.warpAffine(frame.img, trans, frame.width_height, flags=cv2.INTER_CUBIC, borderMode=cv2.BORDER_CONSTANT)
#out = cv2.warpAffine(frame_img, trans, frame.width_height, flags=cv2.INTER_LANCZOS4, borderMode=cv2.BORDER_REFLECT)
#out = cv2.warpAffine(frame_img, trans, frame.width_height)
out = cv2.warpAffine(frame_img, trans, frame.width_height, flags=cv2.INTER_CUBIC,borderMode=cv2.BORDER_REPLICATE)
compensate_time_fim = time.time()
compensate_count += compensate_time_fim - compensate_time_ini
compensate_time_ini = 0
compensate_time_fim = 0
#out = cv2.warpAffine(frame_img, trans, frame.width_height, flags=cv2.INTER_LANCZOS4, borderMode=cv2.BORDER_REFLECT)
#out = frame.img.copy()
prev_trans = trans.copy()
output_video.write(out)
camera.release()
output_video.release()
cv2.destroyAllWindows()