Hi I'm trying to implement IK to my skeleton system and was reading some articles online, and one of the ways to do so was using the pseudo inverse of the Jacobian matrix.
However, I've seen 2 forms of the pseudo inverse, and I would like to know what the differences of these two representations are.
First form is J+ = ((Jt*J).inverse()) * Jt
Second form is J+ = Jt * ((J*Jt).inverse())
To be honest, I'm not even sure how they got the second form, I can only derive the first form at the moment.
Any help would be appreciated!
Usually, the inverse of a matrix has the property that multiplying it with the original matrix on either side produces the identity matrix.
A * A^-1 = A^-1 * A = I
However, this is not true for the pseudo inverse anymore. For the pseudo inverse, order matters (alone from the fact that the matrix might not be square). Your first form is for left-multiplication, your second form is for right multiplication:
J+ * J = I <= J+ = (J^T * J)^-1 * J^T
J * J+ = I <= J+ = J^T * (J * A^T)^-1
If you use the pseudo inverse to solve a linear system, the first form is used when you have column vectors (i.e. J x = b as J+ J x = I x = x = J+ b), and the second form is used when you have row vectors (i.e. x J = b).
Related
All, I am trying to take the laplacian of the following function:
g(x,y) = 1/2cx^2+1/2dy2
The laplacian is c + d, which is a constant. Using FFT I should get the same ( in my FFT example I am padding the function to avoid edge effects).
Here is my code:
Define a 2D function
n = 30 # number of points
Lx = 30 # extension in x
Ly = 30 # extension in x
dx = n/Lx # Step in x
dy = n/Ly # Step in x
c=4
d=4
x=np.arange(-Lx/2,Lx/2)
y=np.arange(-Ly/2,Ly/2)
g = np.zeros((Lx,Ly))
lapg = np.zeros((Lx,Ly))
for j in range(Ly):
for i in range(Lx):
g[i,j] = (1/2)*c*x[i]**2 + (1/2)*d*y[j]**2
lapg[i,j] = c + d
kxpad = 2*np.pi*np.fft.fftfreq(2*Lx,d=dx)
#kxpad = (2*np.pi/(2*Lx))*np.arange(-2*Lx/2,2*Lx/2)
#kxpad = np.fft.fftshift(kxpad)
#kypad = (2*np.pi/(2*Ly))*np.arange(-2*Ly/2,2*Ly/2)
#kypad = np.fft.fftshift(kypad)
kypad = 2*np.pi*np.fft.fftfreq(2*Ly,d=dy)
kpad = np.zeros((2*Lx,2*Ly))
for j in range(2*Ly):
for i in range(2*Lx):
kpad[i,j] = math.sqrt(kxpad[i]**2+kypad[j]**2)
kpad = np.fft.fftshift(kpad)
gpad = np.zeros((2*Lx,2*Ly))
gpad[:Lx,:Ly] = g # Filling main part of g in gpad
gpad[:Lx,Ly:] = g[:,-1::-1] # Filling the last 3 columns of gpad with g flipped
gpad[Lx:,:Ly] = g[-1::-1,:]# Filling the last 3 lines of gpad with g flipped
gpad[Lx:,Ly:] = g[-1::-1, -1::-1]# Filling the last 3 lines and last 3 columns of gpad with g flipped in line and column
rdFFT2D = np.zeros((Lx,Ly))
gpadhat = np.fft.fft2(gpad)
dgpadhat = -(kpad**2)*gpadhat #taking the derivative iwFFT(f)
rdpadFFT2D = np.real(np.fft.ifft2(dgpadhat))
rdFFT2D = rdpadFFT2D[:Lx,:Ly]
[
First image is the plot of the original function g(x,y), 2nd image is the analytical laplacian of g and 3rd image is the sugar loaf in Rio de Janeiro( lol ), actually it is the laplacian using FFT. What Am I doing wrong here?
Edit : Commenting on ripple effect.
Cris you mean the ripple effect due to the set_zlimit in the image below?Just to remember you that the result should be 8.
Edit 2 : Using non-symmetrical x and y values, produce the two images.
The padding will not change the boundary condition: You are padding by replicating the function, mirrored, four times. The function is symmetric, so the mirroring doesn't change it. Thus, your padding simply repeats the function four times. The convolution through the DFT (which you're attempting to implement) uses a periodic boundary condition, and thus already sees the input function as periodic. Replicating the function will not improve the convolution results at the edges.
To improve the result at the edges, you would need to implement a different boundary condition, the most effective one (since the input is analytical anyway) is to simply extend your domain and then crop it after applying the convolution. This introduces a boundary extension where the image is padded by seeing more data outside the original domain. It is an ideal boundary extension suitable for an ideal case where we don't have to deal with real-world data.
This implements the Laplace though the DFT with greatly simplified code, where we ignore any boundary extension, as well as the sample spacing (basically setting dx=1 and dy=1):
import numpy as np
import matplotlib.pyplot as pp
n = 30 # number of points
c = 4
d = 4
x = np.arange(-n//2,n//2)
y = np.arange(-n//2,n//2)
g = (1/2)*c*x[None,:]**2 + (1/2)*d*y[:,None]**2
kx = 2 * np.pi * np.fft.fftfreq(n)
ky = 2 * np.pi * np.fft.fftfreq(n)
lapg = np.real(np.fft.ifft2(np.fft.fft2(g) * (-kx[None, :]**2 - ky[:, None]**2)))
fig = pp.figure()
ax = fig.add_subplot(121, projection='3d')
ax.plot_surface(x[None,:], y[:,None], g)
ax = fig.add_subplot(122, projection='3d')
ax.plot_surface(x[None,:], y[:,None], lapg)
pp.show()
Edit: Boundary extension would work as follows:
import numpy as np
import matplotlib.pyplot as pp
n_true = 30 # number of pixels we want to compute
n_boundary = 15 # number of pixels to extend the image in all directions
c = 4
d = 4
# First compute g and lapg including boundary extenstion
n = n_true + n_boundary * 2
x = np.arange(-n//2,n//2)
y = np.arange(-n//2,n//2)
g = (1/2)*c*x[None,:]**2 + (1/2)*d*y[:,None]**2
kx = 2 * np.pi * np.fft.fftfreq(n)
ky = 2 * np.pi * np.fft.fftfreq(n)
lapg = np.real(np.fft.ifft2(np.fft.fft2(g) * (-kx[None, :]**2 - ky[:, None]**2)))
# Now crop the two images to our desired size
x = x[n_boundary:-n_boundary]
y = y[n_boundary:-n_boundary]
g = g[n_boundary:-n_boundary, n_boundary:-n_boundary]
lapg = lapg[n_boundary:-n_boundary, n_boundary:-n_boundary]
# Display
fig = pp.figure()
ax = fig.add_subplot(121, projection='3d')
ax.plot_surface(x[None,:], y[:,None], g)
ax.set_zlim(0, 800)
ax = fig.add_subplot(122, projection='3d')
ax.plot_surface(x[None,:], y[:,None], lapg)
ax.set_zlim(0, 800)
pp.show()
Note that I'm scaling the z-axes of the two plots in the same way to not enhance the effects of the boundary too much. Fourier-domain filtering like this is typically much more sensitive to edge effects than spatial-domain (or temporal-domain) filtering because the filter has an infinitely-long impulse response. If you leave out the set_zlim command, you'll see a ripple effect in the otherwise flat lapg image. The ripples are very small, but no matter how small, they'll look huge on a completely flat function because they'll stretch from the bottom to the top of the plot. The equal set_zlim in the two plots just puts this noise in proportion.
I'm trying to implement a DCT10 accourding to this paper https://www.researchgate.net/publication/330405662_PittPack_Open-Source_FFT-Based_Poisson%27s_Equation_Solver_for_Computing_With_Accelerators (section "Neumann Boundary Condition").
However I have the problem that after performing the FFT and half-sample shifting, the result is not purely real (which i think it should be, right ?) Therefore when truncating the imaginary part, the mentioned reverse transform will not result in my original values.
Here is my Matlab code (DCT in first dimension):
function X_dct = dct_type2(x_sig)
N = size(x_sig);
% shuffle to prepare for FFT
x_hat = zeros(N);
for m = 1 : N(2)
for n = 1 : (N(1) / 2)
x_hat(n, m) = x_sig((2 * n) - 1, m);
x_hat(N(1) - n + 1, m) = x_sig(2 * n, m);
end
end
% perform FFT
X_hat_dft = fft(x_hat, N(1), 1);
% apply shifting by half-sample
X_dct = zeros(N);
for m = 1 : N(2)
for k = 1 : N(1)
X_dct(k, m) = 2 * exp(-1i * (pi * (k-1)) / (2 * N(1))) * X_hat_dft(k, m);
end
end
end
Can somebody explain what is the problem here ? Or is my assumption wrong that the result should be purely real ?
So it turns out that it is correct to drop the non-zero imaginary part using this technique, even though it intuitively appeared wrong to me.
That the reverse transform did't recorver the original values was merely a scaling issue of the frequency components.
I have a scilab function that looks something like this (very simplified code just to get the concept of how it works):
function [A, S, Q]=myfunc(a)
A = a^2;
S = a+a+a;
if S > A then
Q = "Bigger";
else
Q = "Lower";
end
endfunction
And I get the expected result if I run:
--> [A,S,Q]=myfunc(2)
Q =
Bigger
S =
6.
A =
4.
But if I put matrices into the function I expect to get equivalent matrices back as an answer with a result but instead I got this:
--> [A,S,Q]=myfunc([2 4 6 8])
Q =
Lower
S =
6. 12. 18. 24.
A =
4. 16. 36. 64.
Why isn't Q returning matrices of values like S and A? And how do I achieve that it will return "Bigger. Lower. Lower. Lower." as an answer? That is, I want to perform the operation on each element of the matrix.
Because in your program you wrote Q = "Bigger" and Q = "Lower". That means that Q will only have one value. If you want to store the comparisons for every value in A and S, you have to make Scilab do that.
You can achieve such behavior by using loops. This is how you can do it by using two for loops:
function [A, S, Q]=myfunc(a)
A = a^2;
S = a+a+a;
//Get the size of input a
[nrows, ncols] = size(a)
//Traverse all rows of the input
for i = 1 : nrows
//Traverse all columns of the input
for j = 1 : ncols
//Compare each element
if S(i,j) > A(i,j) then
//Store each result
Q(i,j) = "Bigger"
else
Q(i,j) = "Lower"
end
end
end
endfunction
Beware of A = a^2. It can break your function. It has different behaviors if input a is a vector (1-by-n or n-by-1 matrix), rectangle matrix (m-by-n matrix, m ≠ n ), or square matrix (n-by-n matrix):
Vector: it works like .^, i.e. it raises each element individually (see Scilab help).
Rectangle: it won't work because it has to follow the rule of matrix multiplication.
Square: it works and follows the rule of matrix multiplication.
I will add that in Scilab, the fewer the number of loop, the better : so #luispauloml answer may rewrite to
function [A, S, Q]=myfunc(a)
A = a.^2; // used element wise power, see luispauloml advice
S = a+a+a;
Q(S > A) = "Bigger"
Q(S <= A) = "Lower"
Q = matrix(Q,size(a,1),size(a,2)) // a-like shape
endfunction
I want to traverse all the elements in the set Q = [0, 2^16) in a non sequential manner. To do so I need a function f(x) Q --> Q which gives the order in which the set will be sorted. for example:
f(0) = 2345
f(1) = 4364
f(2) = 24
(...)
To recover the order I would need the inverse function f'(x) Q --> Q which would output:
f(2345) = 0
f(4364) = 1
f(24) = 2
(...)
The function must be bijective, for each element of Q the function uniquely maps to another element of Q.
How can I generate such a function or are there any know functions that do this?
EDIT: In the following answer, f(x) is "what comes after x", not "what goes in position x". For example, if your first number is 5, then f(5) is the next element, not f(1). In retrospect, you probably thought of f(x) as "what goes in position x". The function defined in this answer is much weaker if used as "what goes in position x".
Linear congruential generators fit your needs.
A linear congruential generator is defined by the equation
f(x) = a*x+c (mod m)
for some constants a, c, and m. In this case, m = 65536.
An LCG has full period (the property you want) if the following properties hold:
c and m are relatively prime.
a-1 is divisible by all prime factors of m.
If m is a multiple of 4, a-1 is a multiple of 4.
We'll go with a = 5, c = 1.
To invert an LCG, we solve for f(x) in terms of x:
x = (a^-1)*(f(x) - c) (mod m)
We can find the inverse of 5 mod 65536 by the extended Euclidean algorithm, or since we just need this one computation, we can plug it into Wolfram Alpha. The result is 52429.
Thus, we have
f(x) = (5*x + 1) % 65536
f^-1(x) = (52429 * (x - 1)) % 65536
There's many approaches to solving this.
Since your set size is small, the requirement for generating the function and its inverse can simply be done via memory lookup. So once you choose your permutation, you can store the forward and reverse directions in lookup tables.
One approach to creating a permutation is mapping out all elements in an array and then randomly swapping them "enough" times. C code:
int f[PERM_SIZE], inv_f[PERM_SIZE];
int i;
// start out with identity permutation
for (i=0; i < PERM_SIZE; ++i) {
f[i] = i;
inv_f[i] = i;
}
// seed your random number generator
srand(SEED);
// look "enough" times, where we choose "enough" = size of array
for (i=0; i < PERM_SIZE; ++i) {
int j, k;
j = rand()%PERM_SIZE;
k = rand()%PERM_SIZE;
swap( &f[i], &f[j] );
}
// create inverse of f
for (i=0; i < PERM_SIZE; ++i)
inv_f[f[i]] = i;
Enjoy
I have a question i know a line i just know its slope(m) and a point on it A(x,y) How can i calculate the points(actually two of them) on this line with a distance(d) from point A ???
I m asking this for finding intensity of pixels on a line that pass through A(x,y) with a distance .Distance in this case will be number of pixels.
I would suggest converting the line to a parametric format instead of point-slope. That is, a parametric function for the line returns points along that line for the value of some parameter t. You can represent the line as a reference point, and a vector representing the direction of the line going through that point. That way, you just travel d units forward and backward from point A to get your other points.
Since your line has slope m, its direction vector is <1, m>. Since it moves m pixels in y for every 1 pixel in x. You want to normalize that direction vector to be unit length so you divide by the magnitude of the vector.
magnitude = (1^2 + m^2)^(1/2)
N = <1, m> / magnitude = <1 / magnitude, m / magnitude>
The normalized direction vector is N. Now you are almost done. You just need to write the equation for your line in parameterized format:
f(t) = A + t*N
This uses vector math. Specifically, scalar vector multiplication (of your parameter t and the vector N) and vector addition (of A and t*N). The result of the function f is a point along the line. The 2 points you are looking for are f(d) and f(-d). Implement that in the language of your choosing.
The advantage to using this method, as opposed to all the other answers so far, is that you can easily extend this method to support a line with "infinite" slope. That is, a vertical line like x = 3. You don't really need the slope, all you need is the normalized direction vector. For a vertical line, it is <0, 1>. This is why graphics operations often use vector math, because the calculations are more straight-forward and less prone to singularities.
It may seem a little complicated at first, but once you get the hang of vector operations, a lot of computer graphics tasks get a lot easier.
Let me explain the answer in a simple way.
Start point - (x0, y0)
End point - (x1, y1)
We need to find a point (xt, yt) at a distance dt from start point towards end point.
The distance between Start and End point is given by d = sqrt((x1 - x0)^2 + (y1 - y0)^2)
Let the ratio of distances, t = dt / d
Then the point (xt, yt) = (((1 - t) * x0 + t * x1), ((1 - t) * y0 + t * y1))
When 0 < t < 1, the point is on the line.
When t < 0, the point is outside the line near to (x0, y0).
When t > 1, the point is outside the line near to (x1, y1).
Here's a Python implementation to find a point on a line segment at a given distance from the initial point:
import numpy as np
def get_point_on_vector(initial_pt, terminal_pt, distance):
v = np.array(initial_pt, dtype=float)
u = np.array(terminal_pt, dtype=float)
n = v - u
n /= np.linalg.norm(n, 2)
point = v - distance * n
return tuple(point)
Based on the excellent answer from #Theophile here on math stackexchange.
Let's call the point you are trying to find P, with coordinates px, py, and your starting point A's coordinates ax and ay. Slope m is just the ratio of the change in Y over the change in X, so if your point P is distance s from A, then its coordinates are px = ax + s, and py = ay + m * s. Now using Pythagoras, the distance d from A to P will be d = sqrt(s * s + (m * s) * (m * s)). To make P be a specific D units away from A, find s as s = D/sqrt(1 + m * m).
I thought this was an awesome and easy to understand solution:
http://www.physicsforums.com/showpost.php?s=f04d131386fbd83b7b5df27f8da84fa1&p=2822353&postcount=4