Related
I have the following code, which uses gradient descent to find the global minimum of y = (x+5)^2:
cur_x = 3 # the algorithm starts at x=3
rate = 0.01 # learning rate
precision = 0.000001 # this tells us when to stop the algorithm
previous_step_size = 1
max_iters = 10000 # maximum number of iterations
iters = 0 # iteration counter
df = lambda x: 2*(x+5) # gradient of our function
while previous_step_size > precision and iters < max_iters:
prev_x = cur_x # store current x value in prev_x
cur_x = cur_x - rate * df(prev_x) # grad descent
previous_step_size = abs(cur_x - prev_x) # change in x
iters = iters+1 # iteration count
print("Iteration",iters,"\nX value is",cur_x) # print iterations
print("The local minimum occurs at", cur_x)
The procedure is fairly simple, and among the most intuitive and brief for solving such a problem (at least, that I'm aware of).
I'd now like to apply this to solving a system of nonlinear equations. Namely, I want to use this to solve the Time Difference of Arrival problem in three dimensions. That is, given the coordinates of 4 observers (or, in general, n+1 observers for an n dimensional solution), the velocity v of some signal, and the time of arrival at each observer, I want to reconstruct the source (determine it's coordinates [x,y,z].
I've already accomplished this using approximation search (see this excellent post on the matter: ), and I'd now like to try doing so with gradient descent (really, just as an interesting exercise). I know that the problem in two dimensions can be described by the following non-linear system:
sqrt{(x-x_1)^2+(y-y_1)^2}+s(t_2-t_1) = sqrt{(x-x_2)^2 + (y-y_2)^2}
sqrt{(x-x_2)^2+(y-y_2)^2}+s(t_3-t_2) = sqrt{(x-x_3)^2 + (y-y_3)^2}
sqrt{(x-x_3)^2+(y-y_3)^2}+s(t_1-t_3) = sqrt{(x-x_1)^2 + (y-y_1)^2}
I know that it can be done, however I cannot determine how.
How might I go about applying this to 3-dimensions, or some nonlinear system in general?
I have a 4-4 differential equations system in a function (subsystem4) and I solved it with odeint funtion. I managed to plot the results of the system. My problem is that I want to plot and some other equations (e.g. x,y,vcxdot...) which are included in the same function (subsystem4) but I get NameError: name 'vcxdot' is not defined. Also, I want to use some of these equations (not only the results of the equation's system) as inputs in a following differential equations system and plot all the equations in the same period of time (t). I have done this using Matlab-Simulink but it was much easier because of Simulink blocks. How can I have access to and plot all the equations of a function (subsystem4) and use them as input in a following system? I am new in python and I use Python 2.7.12. Thank you in advance!
import numpy as np
from scipy.integrate import odeint
import matplotlib.pyplot as plt
def subsystem4(u,t):
added_mass_x = 0.03 # kg
added_mass_y = 0.04
mb = 0.3 # kg
m1 = mb-added_mass_x
m2 = mb-added_mass_y
l1 = 0.07 # m
l2 = 0.05 # m
J = 0.00050797 # kgm^2
Sa = 0.0110 # m^2
Cd = 2.44
Cl = 3.41
Kd = 0.000655 # kgm^2
r = 1000 # kg/m^3
f = 2 # Hz
c1 = 0.5*r*Sa*Cd
c2 = 0.5*r*Sa*Cl
c3 = 0.5*mb*(l1**2)
c4 = Kd/J
c5 = (1/(2*J))*(l1**2)*mb*l2
c6 = (1/(3*J))*(l1**3)*mb
vcx = u[0]
vcy = u[1]
psi = u[2]
wz = u[3]
x = 3 + 0.3*np.cos(t)
y = 0.5 + 0.3*np.sin(t)
xdot = -0.3*np.sin(t)
ydot = 0.3*np.cos(t)
xdotdot = -0.3*np.cos(t)
ydotdot = -0.3*np.sin(t)
vcx = xdot*np.cos(psi)-ydot*np.sin(psi)
vcy = ydot*np.cos(psi)+xdot*np.sin(psi)
psidot = wz
vcxdot = xdotdot*np.cos(psi)-xdot*np.sin(psi)*psidot-ydotdot*np.sin(psi)-ydot*np.cos(psi)*psidot
vcydot = ydotdot*np.cos(psi)-ydot*np.sin(psi)*psidot+xdotdot*np.sin(psi)+xdot*np.cos(psi)*psidot
g1 = -(m1/c3)*vcxdot+(m2/c3)*vcy*wz-(c1/c3)*vcx*np.sqrt((vcx**2)+(vcy**2))+(c2/c3)*vcy*np.sqrt((vcx**2)+(vcy**2))*np.arctan2(vcy,vcx)
g2 = (m2/c3)*vcydot+(m1/c3)*vcx*wz+(c1/c3)*vcy*np.sqrt((vcx**2)+(vcy**2))+(c2/c3)*vcx*np.sqrt((vcx**2)+(vcy**2))*np.arctan2(vcy,vcx)
A = 12*np.sin(2*np.pi*f*t+np.pi)
if A>=0.1:
wzdot = ((m1-m2)/J)*vcx*vcy-c4*wz**2*np.sign(wz)-c5*g2-c6*np.sqrt((g1**2)+(g2**2))
elif A<-0.1:
wzdot = ((m1-m2)/J)*vcx*vcy-c4*wz**2*np.sign(wz)-c5*g2+c6*np.sqrt((g1**2)+(g2**2))
else:
wzdot = ((m1-m2)/J)*vcx*vcy-c4*wz**2*np.sign(wz)-c5*g2
return [vcxdot,vcydot,psidot,wzdot]
u0 = [0,0,0,0]
t = np.linspace(0,15,1000)
u = odeint(subsystem4,u0,t)
vcx = u[:,0]
vcy = u[:,1]
psi = u[:,2]
wz = u[:,3]
plt.figure(1)
plt.subplot(211)
plt.plot(t,vcx,'r-',linewidth=2,label='vcx')
plt.plot(t,vcy,'b--',linewidth=2,label='vcy')
plt.plot(t,psi,'g:',linewidth=2,label='psi')
plt.plot(t,wz,'c',linewidth=2,label='wz')
plt.xlabel('time')
plt.legend()
plt.show()
To the immediate question of plotting the derivatives, you can get the velocities by directly calling the ODE function again on the solution,
u = odeint(subsystem4,u0,t)
udot = subsystem4(u.T,t)
and get the separate velocity arrays via
vcxdot,vcydot,psidot,wzdot = udot
In this case the function involves branching, which is not very friendly to vectorized calls of it. There are ways to vectorize branching, but the easiest work-around is to loop manually through the solution points, which is slower than a working vectorized implementation. This will again procude a list of tuples like odeint, so the result has to be transposed as a tuple of lists for "easy" assignment to the single array variables.
udot = [ subsystem4(uk, tk) for uk, tk in zip(u,t) ];
vcxdot,vcydot,psidot,wzdot = np.asarray(udot).T
This may appear to double somewhat the computation, but not really, as the solution points are usually interpolated from the internal step points of the solver. The evaluation of the ODE function during integration will usually happen at points that are different from the solution points.
For the other variables, extract the computation of position and velocities into functions to have the constant and composition in one place only:
def xy_pos(t): return 3 + 0.3*np.cos(t), 0.5 + 0.3*np.sin(t)
def xy_vel(t): return -0.3*np.sin(t), 0.3*np.cos(t)
def xy_acc(t): return -0.3*np.cos(t), -0.3*np.sin(t)
or similar that you can then use both inside the ODE function and in preparing the plots.
What Simulink most likely does is to collect all the equations of all the blocks and form this into one big ODE system which is then solved for the whole state at once. You will need to implement something similar. One big state vector, and each subsystem knows its slice of the state resp. derivatives vector to get its specific state variables from and write the derivatives to. The computation of the derivatives can then use values communicated among the subsystems.
What you are trying to do, solving the subsystems separately, works only for resp. will likely result in a order 1 integration method. All higher order methods need to be able to simultaneously shift the state in some direction computed from previous stages of the method, and evaluate the whole system there.
I am trying to calculate an essential and a projection matrix from two images. I will then use them to project a 3D object onto the image. The two images I used are
I picked a few pixel correspondences, and fed that to a SVD based least square mechanism which the books say gives me the essential matrix. I used the code below for this task (code is based mostly on Eric Solem's Programming Computer Vision with Python book):
import scipy.linalg as lin
import pandas as pd
def skew(a):
return np.array([[0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0]])
def essential(x1,x2):
n = x1.shape[1]
A = np.zeros((n,9))
for i in range(n):
A[i] = [ x1[0,i]*x2[0,i], \
x1[0,i]*x2[1,i], \
x1[0,i]*x2[2,i], \
x1[1,i]*x2[0,i], \
x1[1,i]*x2[1,i], \
x1[1,i]*x2[2,i], \
x1[2,i]*x2[0,i], \
x1[2,i]*x2[1,i], \
x1[2,i]*x2[2,i]]
U,S,V = lin.svd(A)
F = V[-1].reshape(3,3)
return F
def compute_P_from_essential(E):
U,S,V = lin.svd(E)
if lin.det(np.dot(U,V))<0: V = -V
E = np.dot(U,np.dot(np.diag([1,1,0]),V))
Z = skew([0,0,-1])
W = np.array([[0,-1,0],[1,0,0],[0,0,1]])
P2 = [np.vstack((np.dot(U,np.dot(W,V)).T,U[:,2])).T,
np.vstack((np.dot(U,np.dot(W,V)).T,-U[:,2])).T,
np.vstack((np.dot(U,np.dot(W.T,V)).T,U[:,2])).T,
np.vstack((np.dot(U,np.dot(W.T,V)).T,-U[:,2])).T]
return P2
points = [ \
[266,163,296,160],[265,237,297,266],\
[76,288,51,340],[135,31,142,4],\
[344,167,371,156],[48,165,71,164],\
[151,68,166,56],[237,26,259,19],\
[226,147,254,140]]
df = pd.DataFrame(points)
df['uno'] = 1.
x1 = np.array(df[[0,1,'uno']].T)
x2 = np.array(df[[2,3,'uno']].T)
print x1
print x2
E = essential(x1,x2)
P = compute_P_from_essential(E)
import pandas as pd
x0 = 3.; y0 = 1.; z0 = 1.
print df.shape
e = 1
cube = [[x0,y0,z0],[x0+e,y0,z0],[x0+e,y0+e,z0],[x0,y0+e,z0],
[x0,y0,z0+e],[x0+e,y0,z0+e],[x0+e,y0+e,z0+e],[x0,y0+e,z0+e]]
cube = pd.DataFrame(cube)
cube['1'] = 1.
xx = np.dot(P[1], cube.T) * 100.
xx[1,:] = 360-xx[1,:]
#xx = xx / xx[2]
print xx[0].shape
plt.plot(xx[0], xx[1],'.')
plt.xlim(0,640)
plt.ylim(0,360)
I calculated the essential matrix, then the projection matrix, then used that to project a 3D cube. The result:
This looks skewed, I am not sure why this happened. Any ideas on how to fix this?
Thanks,
First of all, it looks like you are computing the essential matrix using exactly 9 points. You can do this using only 8 (since scale is a free parameter, you can multiply the essential by a scalar and it will stay the same so you can fix one of the parameters and just use 8 points, but I digress.) However, in practice this is a very bad idea because your 8 points might have poor spatial configuration. So what you want to do is to select N matches (600 for example), and use an algorithm like RANSAC to determine the best Essential matrix. But aside from that, what I'd recommend to debug such applications is this: compute the Fundalental matrix F based on the Essential you just computed. Now you can select a point in image 1 and then display the corresponding epipolar line in the second one. That will help you visually evaluate and thus debug the estimation of the Essential.
Like it's explained here and here the orde of the eigenvalues (and relative eigenvectors and their sign too) are library dependent and (according to the first linked question) it shouldn't be a problem. In addition, eigenvectors relative to almost-zero eigenvalues can be considered as garbage. So far so good.
Now, consider the MATLAB code below that I want to rewrite in C++ using Eigen library:
%supposing K is 3x3 matrix
[V_K,D_K] = eig(K);
d_k = diag(D_K);
ind_k = find(d_k > 1e-8);
d_k(ind_k) = d_k(ind_k).^(-1/2);
K_half = V_K*diag(d_k)*V_K';
And my C++ implementation:
EigenSolver<Matrix3f> es (K,true);
auto v = es.eigenvalues();
//set to zero if eigenvalues too smal, otherwise v^(-1/2)
v = (v.array().real() > 1e-8).select(v.cwiseSqrt().cwiseInverse(), 0);
auto KHalf = es.eigenvectors()*v.asDiagonal()*es.eigenvectors().inverse();
The problem is that K_half values are different from KHafl, as you can see from the printed result:
Matlab:
V_K =
0.5774 0.8428 -0.0415
0.5774 -0.3806 -0.7468
0.5774 -0.3806 0.6638
D_K =
17.0000 0 0
0 2.0000 0
0 0 -0.0000
K_half =
0.5831 -0.1460 -0.1460
-0.1460 0.1833 0.1833
-0.1460 0.1833 0.1833
eigenvalues=
(2,0)
(17,0)
(0,0)
eigenvectors=
(-0.842777,0) (0.57735,0) (-0.041487,0)
(0.380609,0) (0.57735,0) (-0.746766,0)
(0.380609,0) (0.57735,0) (0.663792,0)
KHalf=
(0.0754555,-3.9918e-310) (0.0764066,1.9959e-310) (0.0906734,1.9959e-310)
(-0.144533,0) (0.186401,0) (0.200668,0)
(-0.144533,0) (0.186401,0) (0.200668,0)
The problem is that I don't know if this difference is going to be a difference for the rest of algorithm or not (which I post at the end of the question for completeness). From what I understand there is no way to guarantee that the eigenvectors are the same from the two libraries (since there exists multiple eigenvectors and they are costant-invariant). Do I have to worry about this? Eventually, how can I solve it?
The rest of the Matlab algorithm:
% p and b int parameters , W and H returned
%create indices for the t random points for each hash bit
%then form weight matrix
for i = 1:b
rp = randperm(p);
I_s(i,:) = rp(1:t);
e_s = zeros(p,1);
e_s(I_s(i,:)) = 1;
W(:,i) = sqrt((p-1)/t)*K_half*e_s;
end
H = (K*W)>0;
W = real(W);
Thanks to both answer's comments I figured out the problem:
Eigen::MatrixXcf KHalf = es.eigenvectors()*v.asDiagonal()*es.eigenvectors().transpose();
(using transpose() and Eigen::MatrixXcf made the trick)
I am trying to solve a set of M simultaneous eqns with M variables. I input a M X 2 matrix in as an initial guess to my function and it returns a M X 2 matrix, where each entry would equal zero if my guess was correct. Thus my function can be represented as f_k(u1,u2,...uN) = 0 for k=1,2,...N. Below is the code for my function, (for simplicities sake I have left out the modules that go with this code, i.e. p. or phi. for instance. I was more wondering if anyone else has had this error before)
M = len(p.x_lat)
def main(u_A):
## unpack u_A
u_P = u_total[:,0]
u_W = u_total[:,1]
## calculate phi_A for all monomeric species
G_W = exp(-u_W)
phi_W = zeros(M)
phi_W[1:] = p.phi_Wb * G_W[1:]
## calculate phi_A for all polymeric species
G_P = exp(-u_P)
G_P[0] = 0.
G_fwd = phi.fwd_propagator(G_P,p.Np,0) #(function that takes G_P and propagates outward)
G_bkwd = phi.bkwd_propagator(G_P,p.Np,0) #(function that takes G_P and propagates inward)
phi_P = phi.phi_P(G_fwd,G_bkwd,p.norm_graft_density,p.Np) #(function that takes the two propagators and combines them to calculate a segment density at each point)
## calculate u_A components
u_intW = en.u_int_AB(p.chi_PW,phi_P,p.phi_Pb) + en.u_int_AB(p.chi_SW,p.phi_S,p.phi_Sb) #(fxn that calculates new potential from the new segment densities)
u_intW[0] = 0.
u_Wprime = u_W - u_intW
u_intP = en.u_int_AB(p.chi_PW,phi_W,p.phi_Wb) + en.u_int_AB(p.chi_PS,p.phi_S,p.phi_Sb) #(fxn that calculates new potential from the new segment densities)
u_intP[0] = 0.
u_Pprime = u_P - u_intP
## calculate f_A
phi_total = p.phi_S + phi_W + phi_P
u_prime = 0.5 * (u_Wprime + u_Pprime)
f_total = zeros( (M, 2) )
f_total[:,0] = 1. - 1./phi_total + u_prime - u_Wprime
f_total[:,1] = 1. - 1./phi_total + u_prime - u_Pprime
return f_total
I researched ways of solving nonlinear equations such as this one using python. I came across the scipy.optimize library with the several options for solvers http://docs.scipy.org/doc/scipy-0.13.0/reference/optimize.nonlin.html. I first tried to use the newton_krylov solver and received the following error message:
ValueError: Jacobian inversion yielded zero vector. This indicates a bug in the Jacobian approximation.
I also tried broyden1 solver and it never converged but simply stayed stagnant. Code for implementation of both below:
sol = newton_krylov(main, guess, verbose=1, f_tol=10e-7)
sol = broyden1(main, guess, verbose=1, f_tol=10e-7)
My initial guess is given below here:
## first guess of u_A(x)
u_P = zeros(M)
u_P[1] = -0.0001
u_P[M-1] = 0.0001
u_W = zeros(M)
u_W[1] = 0.0001
u_W[M-1] = -0.0001
u_total = zeros( (M,2) )
u_total[:,0] = u_P
u_total[:,1] = u_W
guess = u_total
Any help would be greatly appreciated!