I'm trying to make a neural network that calculates the right input angles for a rotation matrix. I'm having the classic linear network structure and at the last step the output is put into my function for the rotation, which returns a point in space as a list.
Here's the code I wrote for it:
import torch
import torch.nn as nn
import torch.nn.functional as F
from torch.autograd import Variable as V
import torch.optim as opt
import numpy as np
import matplotlib.pyplot as plt
cam_pos = np.array([500, 160, 1140, 1]) # with respect to vehicle coordinates
img_res = (1280, 1080)
aspect_ratio = img_res[0] / img_res[1]
# in px
cx = 636 / aspect_ratio
cy = 548 / aspect_ratio
fx = 241 / aspect_ratio
fy = 238 / aspect_ratio
u = 872
v = 423
D = 1900 # mm
img_pt = np.array([u, v, 1, 1/D]).T
camera_matrix = np.array([[fx, 0, cx, 0],
[0, fy, cy, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
class Network(nn.Module):
def __init__(self):
super(Network, self).__init__()
self.lin1 = nn.Linear(3,10)
self.lin2 = nn.Linear(10,10)
self.lin3 = nn.Linear(10,3)
self.angle_list = []
def forward(self, x):
x = F.relu(self.lin1(x))
x = F.relu(self.lin2(x))
x = self.lin3(x)
self.angle_list.append(list(x.detach().numpy()))
return torch.tensor(self.cam_function(x), requires_grad=True)
def rot_x(self, alpha):
return np.array([ [1, 0, 0, 0],
[0, np.cos(alpha), -np.sin(alpha), 0],
[0, np.sin(alpha), np.cos(alpha), 0],
[0, 0, 0, 1]
])
def rot_y(self, beta):
return np.array([ [np.cos(beta), 0, np.sin(beta), 0],
[0, 1, 0, 0],
[-np.sin(beta), 0, np.cos(beta), 0],
[0, 0, 0, 1]
])
def rot_z(self, gamma):
return np.array([ [np.cos(gamma), -np.sin(gamma), 0, 0],
[np.sin(gamma), np.cos(gamma), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
def cam_function(self, net_output):
net_output = net_output.detach().numpy()
x = net_output[0]
y = net_output[1]
z = net_output[2]
rot_m = np.dot(self.rot_z(z), np.dot(self.rot_y(y), self.rot_x(x)))
extrinsic_matrix = np.array([ [rot_m[0][0], rot_m[0][1], rot_m[0][2], cam_pos[0]],
[rot_m[1][0], rot_m[1][1], rot_m[1][2], cam_pos[1]],
[rot_m[2][0], rot_m[2][1], rot_m[2][2], cam_pos[2]],
[0, 0, 0, 1 ]])
cam_output = img_pt * D * np.linalg.inv(camera_matrix) * extrinsic_matrix / 1000
cam_output = [cam_output[0][0], cam_output[1][1], cam_output[2][2]]
return cam_output
model = Network()
loss_function = nn.CrossEntropyLoss()
optimizer = opt.SGD(model.parameters(), lr=1e-3)
target = torch.tensor([1.636, 1.405, 0.262]).float()
dummy_data = torch.tensor([0, 0, 0]).float()
losses = []
for epoch in range(5000):
model.train()
prediction= model(dummy_data)
loss = loss_function(prediction, target)
losses.append(loss.item())
optimizer.zero_grad()
loss.backward()
optimizer.step()
And with that I'm getting a constant value of the loss and the output as well.
7.3858967314779305
tensor([7.9938, 3.9272, 1.8514], dtype=torch.float64, requires_grad=True)
7.3858967314779305
tensor([7.9938, 3.9272, 1.8514], dtype=torch.float64, requires_grad=True)
7.3858967314779305
tensor([7.9938, 3.9272, 1.8514], dtype=torch.float64, requires_grad=True)
Can someone help me please? If this works I would then extract the "angles" the NN used for the rotation matrix
Do not use numpy in cam_function, use torch.tensor. Using numpy, the gradient does not flow when using backward.
Related
I'm following the code presented here:
Compute homography for a virtual camera with opencv
As a note, I made a tiny modification to the code: in the translation matrix, I'm left-multiplying the first 3 rows of the last column by -R to get the translation in the global frame. I also changed the translation matrix definition to use "-dist" because in the global frame, movement toward the camera would be in the negative z direction.
When I turn the X rotation to 0, I get a weird... aliased version of the loaded image that appears ABOVE the horizon line, where there should be nothing.
My question:
Why? Is this just a weird artifact of how the homography is calculated? How can I get rid of it? I know for x=0 (in the presented code) I can just ignore/erase anything above the horizon line, but my use case the x rotation might be -10 to 10 degrees or so - how can I calculate where the horizon line would be in those cases (so I can ignore image data above it) - or is there a mathematical solution the computing the homography that will bypass this problem all together?
Thanks!
EDIT: Adding in code/image in question:
import cv2
import numpy as np
rotXdeg = 90
rotYdeg = 90
rotZdeg = 90
f = 500
dist = 500
def onRotXChange(val):
global rotXdeg
rotXdeg = val
def onRotYChange(val):
global rotYdeg
rotYdeg = val
def onRotZChange(val):
global rotZdeg
rotZdeg = val
def onFchange(val):
global f
f=val
def onDistChange(val):
global dist
dist=val
if __name__ == '__main__':
#Read input image, and create output image
src = cv2.imread('/path/to/image.jpg')
dst = np.ndarray(shape=src.shape,dtype=src.dtype)
#Create user interface with trackbars that will allow to modify the parameters of the transformation
wndname1 = "Source:"
wndname2 = "WarpPerspective: "
cv2.namedWindow(wndname1, 1)
cv2.namedWindow(wndname2, 1)
cv2.createTrackbar("Rotation X", wndname2, rotXdeg, 180, onRotXChange)
cv2.createTrackbar("Rotation Y", wndname2, rotYdeg, 180, onRotYChange)
cv2.createTrackbar("Rotation Z", wndname2, rotZdeg, 180, onRotZChange)
cv2.createTrackbar("f", wndname2, f, 2000, onFchange)
cv2.createTrackbar("Distance", wndname2, dist, 2000, onDistChange)
#Show original image
cv2.imshow(wndname1, src)
h , w = src.shape[:2]
while True:
rotX = (rotXdeg - 90)*np.pi/180
rotY = (rotYdeg - 90)*np.pi/180
rotZ = (rotZdeg - 90)*np.pi/180
#Projection 2D -> 3D matrix
A1= np.matrix([[1, 0, -w/2],
[0, 1, -h/2],
[0, 0, 0 ],
[0, 0, 1 ]])
# Rotation matrices around the X,Y,Z axis
RX = np.matrix([[1, 0, 0, 0],
[0,np.cos(rotX),-np.sin(rotX), 0],
[0,np.sin(rotX),np.cos(rotX) , 0],
[0, 0, 0, 1]])
RY = np.matrix([[ np.cos(rotY), 0, np.sin(rotY), 0],
[ 0, 1, 0, 0],
[ -np.sin(rotY), 0, np.cos(rotY), 0],
[ 0, 0, 0, 1]])
RZ = np.matrix([[ np.cos(rotZ), -np.sin(rotZ), 0, 0],
[ np.sin(rotZ), np.cos(rotZ), 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])
#Composed rotation matrix with (RX,RY,RZ)
R = RX * RY * RZ
#Translation matrix on the Z axis change dist will change the height
T = np.matrix([[1,0,0,0],
[0,1,0,0],
[0,0,1,-dist],
[0,0,0,1]])
extractT = T[:3, 3:4]
solveT = -R[:3, :3]#extractT
T[:3, 3:4] = solveT
#Camera Intrisecs matrix 3D -> 2D
A2= np.matrix([[f, 0, w/2,0],
[0, f, h/2,0],
[0, 0, 1,0]])
# Final and overall transformation matrix
H = A2 * (T * (R * A1))
# Apply matrix transformation
cv2.warpPerspective(src, H, (w, h), dst, cv2.INTER_CUBIC)
#Show the image
cv2.imshow(wndname2, dst)
if (cv2.waitKey(1) == ord('q')):
break
Image:
I have designed a single layer neural network:
x=AF
y=xw
A is matrix(nn) Graph Adjacency Matrix ,F is matrix(n*2) and w is Wight.
This is the code:
import numpy as np
import networkx as nx
def relu(X):
return np.maximum(0,X)
A1 = np.matrix([
[0, 0, 1, 0,0,0],
[0, 0, 1, 0,0,0],
[0, 1, 0, 0,1,1],
[1, 0, 1, 0,0,0],
[0,1,0,0,1,0],
[0,1,1,0,1,0]],
dtype=float
)
G = nx.from_numpy_matrix(A1, create_using=nx.DiGraph)
nodes=list(G.nodes())
print(nodes)
print('edges',len(G.edges()))
UN_G=G.to_undirected()
A=nx.adjacency_matrix(UN_G)
print('un_edges',len(UN_G.edges()))
F = np.matrix([
[G.in_degree(i), G.out_degree(i)]
for i in range(A.shape[0])
], dtype=float)
I = np.matrix(np.eye(A.shape[0]))
A_prem = A + I
D_hat= np.array(np.sum(A_prem, axis=0))[0]
D_hat = np.matrix(np.diag(D_hat))
A_hat=D_hat**-1 * A_prem * D_hat**-1
x=A_hat*F
x=A*F
(x)
In the unsupervised loss function: A is sparse matrix
def loss_fn(y,A):
coo = A.tocoo()
tmp=zip(coo.row, coo.col, coo.data)
sum = tf.Variable(0.0)
for i,j,k in tmp:
sum=sum+k*tf.linalg.norm(y[i]-y[j])
return (sum)
In the weight training phase:
epchs=50
w = tf.Variable(tf.random.normal((2, 2)), name='w')
eta=0.3
for ephoc in range(epchs):
with tf.GradientTape(persistent=True) as tape:
tape.watch(w)
y = tf.nn.softmax(x # w)
loss=loss_fn(y,A)
print(ephoc,' ',loss)
dl_dw = tape.gradient(loss, w)
w.assign_sub(eta*dl_dw)
value of [dl_dw] and loss are nan. What is the problem with my code? Thank you for guiding me
I'm implementing a Kalman filter for an 2D tracked object. I'm measuring the position and the velocity of the object. For the moment, I assume I have all the data from the sensors at the same time, so my observation matrix H is
H = eye(4,4), a 4x4 identity matrix. (See code below)
However, in my final implementation I will have the data from the sensors at different times. So in some update loops I will have the velocity, and in others I will have the position. How would I write the H matrix in those cases?
Is it okay to write
[position loop]
[1, 0, 0, 0 ]
[0, 1, 0, 0 ]
[0, 0, 0, 0 ]
[0, 0, 0, 0 ]
[velocity loop]
[0, 0, 0, 0 ]
[0, 0, 0, 0 ]
[0, 0, 1, 0 ]
[0, 0, 0, 1 ]
Note that my state space variables are [x, y, vx, vy]
I wonder if using those matrices does not imply that my observations are zero, or something like that.
Can I leave the covariances matrices untouched? I guess not.
#Implementation of 2D filter with FilterPy.
import numpy as np
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
import matplotlib.pyplot as plt
# --------- PARAM -----------
dt = 0.1
v_dev = 0.3
pos_dev = 0.8
duration = 50
acceleration_noise = 0.3
# --------- MODEL ------------
transition_matrix = [[1,0,dt,0],[0,1,0,dt],[0,0,1,0],[0,0,0,1]]
transition_covariance = np.array([
[ 0.25*pow(dt, 4), 0, 0.5* pow(dt, 3), 0 ],
[ 0, 0.25*pow(dt, 4), 0, 0.5* pow(dt, 3)],
[ 0.5* pow(dt, 3), 0, dt*dt, 0],
[ 0, 0.5*dt*dt*dt, 0, dt*dt]]) * acceleration_noise *acceleration_noise # A large process noise favors the measurements. ()
#Transition matrix with acceleration componentn
observation_matrix = np.eye(4, 4)
initial_state = [0, 0, 0.5, 0.5]
initial_state_covariance = [[ pos_dev*pos_dev, 0, 0 ,0],[0, pos_dev*pos_dev, 0, 0],[0, 0, v_dev * v_dev, 0 ],[0, 0, 0, v_dev * v_dev ]]
observation_covariance = [[pos_dev * pos_dev , 0, 0 ,0],[0, pos_dev * pos_dev, 0, 0],[0, 0, v_dev * v_dev, 0 ],[0, 0, 0, v_dev * v_dev ]]
#-----------------------------
#---------- FAKE DATA ---------
ind = np.array( range( round(duration/dt) ) )
time = ind * dt
position = np.zeros( (2, len(ind)) )
position[0,:] = time
position[1,:] = 3 * np.sin(time)
noise = pos_dev * np.random.randn(2, len(ind))
noisy_pos = position + noise
vel = position[:,1:len(ind)] - position[:,0:len(ind)-1]
vel = vel / dt
vel_ind = np.zeros( (2, len(ind) -1 ) )
vel_ind[0,:] = position[0,0:len(ind)-1]
vel_ind[1,:] = position[1,0:len(ind)-1]
vel_noise = v_dev * np.random.randn(2, len(ind) - 1 )
noisy_vel = vel + vel_noise
observations = np.zeros((len(ind), 4))
observations[:,[0,1]] = np.transpose(noisy_pos)
observations[1:len(ind),[2,3]] = np.transpose(noisy_vel)
observations[0,[2,3]] = np.transpose(noisy_vel[[0,1],0] )
# KALMAN!
filtered_state_means = np.zeros((len(time), 4))
filtered_state_covariances = np.zeros( ( len(time), 4, 4) )
kf = KalmanFilter( dim_x = 4, dim_z = 4) # state space: x, y, vx, vy, measuring all
kf.x = np.array( initial_state )
kf.F = np.array( transition_matrix )
kf.H = np.array( observation_matrix )
kf.P = np.array( initial_state_covariance )
kf.Q = np.array( transition_covariance )
kf.R =np.array( observation_covariance ) #measurement covariance
for i in range(0, len(time) ):
# Ommitting some data points
if( i > no_gps_start and i < no_gps_end):
# No data from gps
kf.H = np.array( ([0, 0, 0, 0],[0, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]) )
else:
kf.H = observation_matrix
kf.predict()
kf.update(observations[i])
filtered_state_means[i] = kf.x
filtered_state_covariances[i] = kf.P
# Plotting everything
xmin = -2
xmax = 22
ymin = -4.3
ymax = 4.3
axisLimits = [xmin, xmax, ymin, ymax]
plt.figure(1)
plt.plot( position[0,:], position[1,:], linewidth=1 , color= '0.7')
plt.plot( noisy_pos[0,:], noisy_pos[1,:], '.')
plt.axis( axisLimits )
plt.figure(2)
plt.plot( position[0,:], position[1,:], linewidth=1 , color= '0.7')
plt.quiver( vel_ind[0,:], vel_ind[1,:], noisy_vel[0,:], noisy_vel[1,:], angles='xy', scale_units='xy', scale=10)
plt.axis( axisLimits )
plt.figure(3)
plt.plot( position[0,:], position[1,:], linewidth=1 , color= '0.7', zorder= 1)
plt.plot( filtered_state_means[:,0], filtered_state_means[:,1], linewidth = 1, zorder= 2)
plt.plot( noisy_pos[0,:], noisy_pos[1,:], '.', color = "#fd92f8", zorder= 0)
plt.plot( no_gps_x, no_gps_y, 'ro')
plt.show()
You are right, you are not allowed to modify the observation matrix in this way.
In your case the best solution would be a sequential Kalman Filter, which was developed exactly for handling of missing measurements. The measurement vector is replaced through a sequence of separate scalar measurements. The filter can proceed them independently and is not corrupted if one or more measurements do not exist at some point in time.
Have a look at Dan Simon's "Optimal State Estimation" Chapter 6.1 (you can try to find the book online). He derives alternative equations for the Kalman Filter, which are pretty easy to implement. The prediction step stays the same, you need to modify the update step.
Pros:
you don't need to compute the inverse matrix at all (nice for embedded systems)
if your H matrix has a lot of zeros the equivalent sequential expressions are very short and computationally efficient
Contras:
the R matrix (measurement covariance) has to be diagonal
I have a task to check if a matrix is a rotation matrix, I write code as follow:
import numpy as np
def isRotationMatrix(R):
# some code here
# return True or False
R = np.array([
[0, 0, 1],
[1, 0, 0],
[0, 1, 0],
])
print(isRotationMatrix(R)) # Should be True
R = np.array([
[-1, 0, 0],
[0, 1, 0],
[0, 0, 1],
])
print(isRotationMatrix(R)) # Should be False
I don't know how to implement the function isRotationMatrix.
My naive implement, it only works for a 3x3 matrix:
def isRotationMatrix(R_3x3):
should_be_norm_one = np.allclose(np.linalg.norm(R_3x3, axis=0), np.ones(shape=3))
x = R_3x3[:, 0].ravel()
y = R_3x3[:, 1].ravel()
z = R_3x3[:, 2].ravel()
should_be_perpendicular = \
np.allclose(np.cross(x, y), z) \
and np.allclose(np.cross(y, z), x) \
and np.allclose(np.cross(z, x), y)
return should_be_perpendicular and should_be_norm_one
I am using this definition of rotation matrix. A rotation matrix should satisfy the conditions M (M^T) = (M^T) M = I and det(M) = 1. Here M^T denotes transpose of M, I denotes identity matrix and det(M) represents determinant of matrix M.
You can use the following python code to check if the matrix is a rotation matrix.
import numpy as np
''' I have chosen `M` as an example. Feel free to put in your own matrix.'''
M = np.array([[0,-1,0],[1,0,0],[0,0,1]])
def isRotationMatrix(M):
tag = False
I = np.identity(M.shape[0])
if np.all((np.matmul(M, M.T)) == I) and (np.linalg.det(M)==1): tag = True
return tag
if(isRotationMatrix(M)): print 'M is a rotation matrix.'
else: print 'M is not a rotation matrix.'
A rotation matrix is a orthonormal matrix and its determinant should be 1.
My implement:
import numpy as np
def isRotationMatrix(R):
# square matrix test
if R.ndim != 2 or R.shape[0] != R.shape[1]:
return False
should_be_identity = np.allclose(R.dot(R.T), np.identity(R.shape[0], np.float))
should_be_one = np.allclose(np.linalg.det(R), 1)
return should_be_identity and should_be_one
if __name__ == '__main__':
R = np.array([
[0, 0, 1],
[1, 0, 0],
[0, 1, 0],
])
print(isRotationMatrix(R)) # True
R = np.array([
[-1, 0, 0],
[0, 1, 0],
[0, 0, 1],
])
print(isRotationMatrix(R)) # True
print(isRotationMatrix(np.zeros((3, 2)))) # False
How can I set the threshold for a single layer perceptron?
I have
import numpy as np
import sklearn
from sklearn.linear_model import Perceptron
xs = np.array([
# x1 x2
0, 0, #m1
0, 1,
1, 0,
1, 1
]).reshape(4, 2)
ys = np.array([1, 1, 0, 1]).reshape(4,)
ppn = Perceptron(max_iter=10, eta0=0.2, random_state=0)
ppn.fit(xs, ys)
What I wanna do is to train the ppn weights, with initial
weights=(0,0), eta=0.2, threshold=0,5
Eg. for m1, initial: ys=1:
net= w1*x1+w2*x2 = 0*1+0*1=0
f(net) = 1, if net>=threshold else 0,
f(0) = 0 # because, 0 < 0.5
error = 0.2*(1-0) = 0.2
weight_update_w1 = 0+0.2*1
The learning should stop, if once for all m's no
weight update is performed.
How can I set the threshold for ppn to 0.5?