In order to develop my implementation of the particle filter algorithm, I need to generate hypotheses about the movements relating to the object to be tracked: if I set N samples and if I use a 2-by-1 state vector, then at each step I have to generate N pairs of random values (a 2-by-N matrix). Moreover, if I know the statistics of movements (mean and standard deviation), then I could use the mean and standard deviation to generate all N values. Finally, to model the uncertainty of the movement, I could generate a noise matrix (a 2-by-N matrix) and add it to the matrix of movements.
Based on these premises, I have implemented the algorithm running in matlab, and I used the following code in order to generate the hypotheses of movement.
ds_mean = [dx_mean dy_mean];
ds_stddev = [dx_stddev dy_stddev];
d = 5;
V = zeros(2,N);
V(1,:) = normrnd(ds_mean(1),ds_stddev(1),1,N); % hypotheses of movement on x axis
V(2,:) = normrnd(ds_mean(2),ds_stddev(2),1,N); % hypotheses of movement on y axis
E = d*randn(2,N); % weighted noise
M = V + E; % hypotheses of movement
A problem occurred when I had to implement the same algorithm using C++ and OpenCV: substantially, while the above matlab code generates good predictions (it works great), instead the same code written in C++ (see the code below) generates poor predictions (ie far away from the object). Why?
RNG m_rng;
x_mean = // ...
y_mean = // ...
x_stddev = // ...
y_stddev = // ...
Mat velocity(STATE_DIM, NUM_PARTICLES, DataType<double>::type);
m_rng.fill(velocity.row(0), RNG::NORMAL, x_mean, x_stddev);
m_rng.fill(velocity.row(1), RNG::NORMAL, y_mean, y_stddev);
Mat noise(STATE_DIM, NUM_PARTICLES, DataType<double>::type);
m_rng.fill(noise,RNG::NORMAL,0,1);
noise *= d; % weighted noise
movements = velocity + noise;
How to make sure that the C++ algorithm works as well as the algorithm implemented in matlab?
I think I just serendipitously answered your question here, or at least provided an alternative solution.
https://stackoverflow.com/a/13897938/1899861
I believe this will generate proper random numbers, and has been tested to death when compiled using Microsoft C on Intel processors (386, 486, Pentium).
FYI, 4.0 * atan(1.0) yields a much better value of PI than the constant in the above environment.
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'm writing a code in python to evolve the time-dependent Schrodinger equation using the Crank-Nicolson scheme. I didn't know how to deal with the potential so I looked around and found a way from this question, which I have verified from a couple other sources. According to them, for a harmonic oscillator potential, the C-N scheme gives
AΨn+1=A∗Ψn
where the elements on the main diagonal of A are dj=1+[(iΔt) / (2m(Δx)^2)]+[(iΔt(xj)^2)/4] and the elements on the upper and lower diagonals are a=−iΔt/[4m(Δx)^2]
The way I understand it, I'm supposed to give an initial condition(I've chosen a coherent state) in the form of the matrix Ψn and I need to compute the matrix Ψn+1 , which is the wave function after time Δt. To obtain Ψn+1 for a given step, I'm inverting the matrix A and multiplying it with the matrix A* and then multiplying the result with Ψn. The resulting matrix then becomes Ψn for the next step.
But when I'm doing this, I'm getting an incorrect animation. The wave packet is supposed to oscillate between the boundaries but in my animation, it is barely moving from its initial mean value. I just don't understand what I'm doing wrong. Is my understanding of the problem wrong? Or is it a flaw in my code?Please help! I've posted my code below and the video of my animation here. I'm sorry for the length of the code and the question but it's driving me crazy not knowing what my mistake is.
import numpy as np
import matplotlib.pyplot as plt
L = 30.0
x0 = -5.0
sig = 0.5
dx = 0.5
dt = 0.02
k = 1.0
w=2
K=w**2
a=np.power(K,0.25)
xs = np.arange(-L,L,dx)
nn = len(xs)
mu = k*dt/(dx)**2
dd = 1.0+mu
ee = 1.0-mu
ti = 0.0
tf = 100.0
t = ti
V=np.zeros(len(xs))
u=np.zeros(nn,dtype="complex")
V=K*(xs)**2/2 #harmonic oscillator potential
u=(np.sqrt(a)/1.33)*np.exp(-(a*(xs - x0))**2)+0j #initial condition for wave function
u[0]=0.0 #boundary condition
u[-1] = 0.0 #boundary condition
A = np.zeros((nn-2,nn-2),dtype="complex") #define A
for i in range(nn-3):
A[i,i] = 1+1j*(mu/2+w*dt*xs[i]**2/4)
A[i,i+1] = -1j*mu/4.
A[i+1,i] = -1j*mu/4.
A[nn-3,nn-3] = 1+1j*mu/2+1j*dt*xs[nn-3]**2/4
B = np.zeros((nn-2,nn-2),dtype="complex") #define A*
for i in range(nn-3):
B[i,i] = 1-1j*mu/2-1j*w*dt*xs[i]**2/4
B[i,i+1] = 1j*mu/4.
B[i+1,i] = 1j*mu/4.
B[nn-3,nn-3] = 1-1j*(mu/2)-1j*dt*xs[nn-3]**2/4
X = np.linalg.inv(A) #take inverse of A
plt.ion()
l, = plt.plot(xs,np.abs(u),lw=2,color='blue') #plot initial wave function
T=np.matmul(X,B) #multiply A inverse with A*
while t<tf:
u[1:-1]=np.matmul(T,u[1:-1]) #updating u but leaving the boundary conditions unchanged
l.set_ydata((abs(u))) #update plot with new u
t += dt
plt.pause(0.00001)
After a lot of tinkering, it came down to reducing my step size. That did the job for me- I reduced the step size and the program worked. If anyone is facing the same problem as I am, I recommend playing around with the step sizes. Provided that the rest of the code is fine, this is the only possible area of error.
I am trying to generate uniform random points on the surface of a unit sphere for a Monte Carlo ray tracing program. When I say uniform I mean the points are uniformly distributed with respect to surface area. My current methodology is to calculate uniform random points on a hemisphere pointing in the positive z axis and base in the x-y plane.
The random point on the hemisphere represents the direction of emission of thermal radiation for a diffuse grey emitter.
I achieve the correct result when I use the following calculation :
Note : dsfmt* is will return a random number between 0 and 1.
azimuthal = 2*PI*dsfmt_genrand_close_open(&dsfmtt);
zenith = asin(sqrt(dsfmt_genrand_close_open(&dsfmtt)));
// Calculate the cartesian point
osRay.c._x = sin(zenith)*cos(azimuthal);
osRay.c._y = sin(zenith)*sin(azimuthal);
osRay.c._z = cos(zenith);
However this is quite slow and profiling suggests that it takes up a large proportion of run time. Therefore I sought out some alternative methods:
The Marsaglia 1972 rejection method
do {
x1 = 2.0*dsfmt_genrand_open_open(&dsfmtt)-1.0;
x2 = 2.0*dsfmt_genrand_open_open(&dsfmtt)-1.0;
S = x1*x1 + x2*x2;
} while(S > 1.0f);
osRay.c._x = 2.0*x1*sqrt(1.0-S);
osRay.c._y = 2.0*x2*sqrt(1.0-S);
osRay.c._z = abs(1.0-2.0*S);
Analytical cartesian coordinate calculation
azimuthal = 2*PI*dsfmt_genrand_close_open(&dsfmtt);
u = 2*dsfmt_genrand_close_open(&dsfmtt) -1;
w = sqrt(1-u*u);
osRay.c._x = w*cos(azimuthal);
osRay.c._y = w*sin(azimuthal);
osRay.c._z = abs(u);
While these last two methods run serval times faster than the first, when I use them I get results which indicate that they are not generating uniform random points on the surface of a sphere but rather are giving a distribution which favours the equator.
Additionally the last two methods give identical final results however I am certain that they are incorrect as I am comparing against an analytical solution.
Every reference I have found indicates that these methods do produce uniform distributions however I do not achieve the correct result.
Is there an error in my implementation or have I missed a fundamental idea in the second and third methods?
The simplest way to generate a uniform distribution on the unit sphere (whatever its dimension is) is to draw independent normal distributions and normalize the resulting vector.
Indeed, for example in dimension 3, e^(-x^2/2) e^(-y^2/2) e^(-z^2/2) = e^(-(x^2 + y^2 + z^2)/2) so the joint distribution is invariant by rotations.
This is fast if you use a fast normal distribution generator (either Ziggurat or Ratio-Of-Uniforms) and a fast normalization routine (google for "fast inverse square root). No transcendental function call is required.
Also, the Marsaglia is not uniform on the half sphere. You'll have more points near the equator since the correspondence point on the 2D disc <-> point on the half sphere is not isometric. The last one seems correct though (however I didn't make the calculation to ensure this).
If you take a horizontal slice of the unit sphere, of height h, its surface area is just 2 pi h. (This is how Archimedes calculated the surface area of a sphere.) So the z-coordinate is uniformly distributed in [0,1]:
azimuthal = 2*PI*dsfmt_genrand_close_open(&dsfmtt);
osRay.c._z = dsfmt_genrand_close_open(&dsfmtt);
xyproj = sqrt(1 - osRay.c._z*osRay.c._z);
osRay.c._x = xyproj*cos(azimuthal);
osRay.c._y = xyproj*sin(azimuthal);
Also you might be able to save some time by calculating cos(azimuthal) and sin(azimuthal) together -- see this stackoverflow question for discussion.
Edited to add: OK, I see now that this is just a slight tweak of your third method. But it cuts out a step.
This should be quick if you have a fast RNG:
// RNG::draw() returns a uniformly distributed number between -1 and 1.
void drawSphereSurface(RNG& rng, double& x1, double& x2, double& x3)
{
while (true) {
x1 = rng.draw();
x2 = rng.draw();
x3 = rng.draw();
const double radius = sqrt(x1*x1 + x2*x2 + x3*x3);
if (radius > 0 && radius < 1) {
x1 /= radius;
x2 /= radius;
x3 /= radius;
return;
}
}
}
To speed it up, you can move the sqrt call inside the if block.
Have you tried getting rid of asin?
azimuthal = 2*PI*dsfmt_genrand_close_open(&dsfmtt);
sin2_zenith = dsfmt_genrand_close_open(&dsfmtt);
sin_zenith = sqrt(sin2_zenith);
// Calculate the cartesian point
osRay.c._x = sin_zenith*cos(azimuthal);
osRay.c._y = sin_zenith*sin(azimuthal);
osRay.c._z = sqrt(1 - sin2_zenith);
I think the problem you are having with non-uniform results is because in polar coordinates, a random point on the circle is not uniformly distributed on the radial axis. If you look at the area on [theta, theta+dtheta]x[r,r+dr], for fixed theta and dtheta, the area will be different of different values of r. Intuitivly, there is "more area" further out from the center. Thus, you need to scale your random radius to account for this. I haven't got the proof lying around, but the scaling is r=R*sqrt(rand), with R being the radius of the circle and rand begin the random number.
The second and third methods do in fact produce uniformly distributed random points on the surface of a sphere with the second method (Marsaglia 1972) producing the fastest run times at around twice the speed on an Intel Xeon 2.8 GHz Quad-Core.
As noted by Alexandre C there is an additional method using the normal distribution which expands to n-spheres better than the methods I have presented.
This link will give you further information on selecting uniformly distributed random points on the surface of a sphere.
My initial method as pointed out by TonyK does not produce uniformly distributed points and rather bias's the poles when generating the random points. This is required by the problem I am trying to solve however I simply assumed it would generate uniformly random points. As suggested by Pablo this method can be optimised by removing the asin() call to reduce run time by around 20%.
1st try (wrong)
point=[rand(-1,1),rand(-1,1),rand(-1,1)];
len = length_of_vector(point);
EDITED:
What about?
while(1)
point=[rand(-1,1),rand(-1,1),rand(-1,1)];
len = length_of_vector(point);
if( len > 1 )
continue;
point = point / len
break
Acception is here approx 0.4. Than mean that you will reject 60% of solutions.
I asked this question yesterday: Simulating matlab's mrdivide with 2 square matrices
And thats got mrdivide working. However now I'm having problems with mldivide, which is currently implemented as follows:
cv::Mat mldivide(const cv::Mat& A, const cv::Mat& B )
{
//return b * A.inv();
cv::Mat a;
cv::Mat b;
A.convertTo( a, CV_64FC1 );
B.convertTo( b, CV_64FC1 );
cv::Mat ret;
cv::solve( a, b, ret, cv::DECOMP_NORMAL );
cv::Mat ret2;
ret.convertTo( ret2, A.type() );
return ret2;
}
By my understanding the fact that mrdivide is working should mean that mldivide is working but I can't get it to give me the same results as matlab. Again the results are nothing alike.
Its worth noting I am trying to do a [19x19] \ [19x200] so not square matrices this time.
Like I've previously mentioned in your other question, I am using MATLAB along with mexopencv, that way I can easily compare the output of both MATLAB and OpenCV.
That said, I can't reproduce your problem: I generated randomly matrices, and repeated the comparison N=100 times. I'm running MATLAB R2015a with mexopencv compiled against OpenCV 3.0.0:
N = 100;
r = zeros(N,2);
d = zeros(N,1);
for i=1:N
% double precision, i.e CV_64F
A = randn(19,19);
B = randn(19,200);
x1 = A\B;
x2 = cv.solve(A,B); % this a MEX function that calls cv::solve
r(i,:) = [norm(A*x1-B), norm(A*x2-B)];
d(i) = norm(x1-x2);
end
All results agreed and the errors were very small in the order of 1e-11:
>> mean(r)
ans =
1.0e-12 *
0.2282 0.2698
>> mean(d)
ans =
6.5457e-12
(btw I also tried x2 = cv.solve(A,B, 'IsNormal',true); which sets the cv::DECOMP_NORMAL flag, and the results were not that different either).
This leads me to believe that either your matrices happen to accentuate some edge case in the OpenCV solver, where it failed to give a proper solution, or more likely you have a bug somewhere else in your code.
I'd start by double checking how you load your data, and especially watch out for how the matrices are laid out (obviously MATLAB is column-major, while OpenCV is row-major)...
Also you never told us anything about your matrices; do they exhibit a certain characteristic, are there any symmetries, are they mostly zeros, their rank, etc..
In OpenCV, the default solver method is LU factorization, and you have to explicitly change it yourself if appropriate. MATLAB on the hand will automatically choose a method that best suits the matrix A, and LU is just one of the possible decompositions.
EDIT (response to comments)
When using SVD decompositition in MATLAB, the sign of the left and right eigenvectors U and V is arbitrary (this really comes from the DGESVD LAPACK routine). In order to get consistent results, one convention is to require that the first element of each eigenvector be a certain sign, and multiplying each vector by +1 or -1 to flip the sign as appropriate. I would also suggest checking out eigenshuffle.
One more time, here is a test I did to confirm that I get similar results for SVD in MATLAB and OpenCV:
N = 100;
r = zeros(N,2);
d = zeros(N,3);
for i=1:N
% double precision, i.e CV_64F
A = rand(19);
% compute SVD in MATLAB, and apply sign convention
[U1,S1,V1] = svd(A);
sn = sign(U1(1,:));
U1 = bsxfun(#times, sn, U1);
V1 = bsxfun(#times, sn, V1);
r(i,1) = norm(U1*S1*V1' - A);
% compute SVD in OpenCV, and apply sign convention
[S2,U2,V2] = cv.SVD.Compute(A);
S2 = diag(S2);
sn = sign(U2(1,:));
U2 = bsxfun(#times, sn, U2);
V2 = bsxfun(#times, sn', V2)'; % Note: V2 was transposed w.r.t V1
r(i,2) = norm(U2*S2*V2' - A);
% compare
d(i,:) = [norm(V1-V2), norm(U1-U2), norm(S1-S2)];
end
Again, all results were very similar and the errors close to machine epsilon and negligible:
>> mean(r)
ans =
1.0e-13 *
0.3381 0.1215
>> mean(d)
ans =
1.0e-13 *
0.3113 0.3009 0.0578
One thing I'm not sure about in OpenCV, but MATLAB's svd function returns the singular values sorted in decreasing order (unlike the eig function), with the columns of the eigenvectors in corresponding order.
Now if the singular values in OpenCV are not guaranteed to be sorted for some reason, you have to do it manually as well if you want to compare the results against MATLAB, as in:
% not needed in MATLAB
[U,S,V] = svd(A);
[S, ord] = sort(diag(S), 'descend');
S = diag(S);
U = U(:,ord)
V = V(:,ord);
I'm studying about the CSS algorithm and I don't get the hang of the concept of 'Arc Length Parameter'.
According to the literature, planar curve Gamma(u)=(x(u),y(u)) and they say this u is the arc length parameter and apparently, Gaussian Kernel g is also parameterized by this u here.
Stop me if I got something wrong but, aren't x and y location of the pixel? How is it represented by another parameter?
I had no idea when I first saw it on the literature so, I looked up the code. and apparently, I got puzzled even more.
here is the portion of the code
void getGaussianDerivs(double sigma, int M, vector<double>& gaussian,
vector<double>& dg, vector<double>& d2g) {
int L = (M - 1) / 2;
double sigma_sq = sigma * sigma;
double sigma_quad = sigma_sq*sigma_sq;
dg.resize(M); d2g.resize(M); gaussian.resize(M);
Mat_<double> g = getGaussianKernel(M, sigma, CV_64F);
for (double i = -L; i < L+1.0; i += 1.0) {
int idx = (int)(i+L);
gaussian[idx] = g(idx);
// from http://www.cedar.buffalo.edu/~srihari/CSE555/Normal2.pdf
dg[idx] = (-i/sigma_sq) * g(idx);
d2g[idx] = (-sigma_sq + i*i)/sigma_quad * g(idx);
}
}
so, it seems the code uses simple 1D Gaussian Kernel Aperture size of M and it is trying to compute its 1st and 2nd derivatives. As far as I know, 1D Gaussian kernel has parameter of x which is a horizontal coordinate and sigma which is scale. it seems like that 'arc length parameter u' is equivalent to the variable of x. That doesn't make any sense because later in the code, it directly convolutes the set of x and y on the contour.
what is this u?
PS. since I replied to the fellow who tried to answer my question, I think I should modify my question, so, here we go.
What I'm confusing is, how is this parameter 'u' implemented in codes? I think I understood the full code above -of course, I inserted only a portion of the code- but the problem is, I have no idea about what it would be for the 'improved' version of the algorithm. It says it's using 'affine length parameter' instead of this 'arc length parameter' and I'm not so sure how I implement the concept into the code.
According to the literature, the main difference between arc length parameter and affine length parameter is it's sampling interval and arc length parameter uses 1 for the vertical and horizontal direction and root of 2 for the diagonal direction. It makes sense since the portion of the code above is using for loop to compute 1st and 2nd derivatives of the 1d Gaussian and it directly inserts the value of interval 1 but, how is it gonna be with different interval with different variable? Is it possible that I'm not able to use 'for loop' for it?