I have points in a 3D plane that I have converted to a 2D projection using the following method:
import numpy as np
# Calculate axes for 2D projection
# Create random vector to cross
rv = np.add(self.plane.normal, [-1.0, 0.0, 1.0])
rv = np.divide(rv, np.linalg.norm(rv))
horizontal = np.cross(self.plane.normal, rv)
vertical = np.cross(self.plane.normal, horizontal)
diff2 = np.zeros((len(point23D), 3), dtype=np.float32)
diff2[:, 0] = np.subtract(point23D[:, 0], self.plane.origin[0])
diff2[:, 1] = np.subtract(point23D[:, 1], self.plane.origin[1])
diff2[:, 2] = np.subtract(point23D[:, 2], self.plane.origin[2])
x2 = np.add(np.add(np.multiply(diff2[:, 0], horizontal[0]), np.multiply(diff2[:, 1], horizontal[1])), np.multiply(diff2[:, 2], horizontal[2]))
y2 = np.add(np.add(np.multiply(diff2[:, 0], vertical[0]), np.multiply(diff2[:, 1], vertical[1])), np.multiply(diff2[:, 2], vertical[2]))
twodpoints2 = np.zeros((len(point23D), 3), dtype=np.float32)
twodpoints2[:, 0] = x2
twodpoints2[:, 1] = y2
I then do some calculations on these points in 2D space. After that I need to get the points back in 3D space on the same relative position. I have written the following code for that:
# Transform back to 3D
rotation_matrix = np.array([[horizontal[0], vertical[0], -self.plane.normal[0]],
[horizontal[1], vertical[1], -self.plane.normal[1]],
[horizontal[2], vertical[2], -self.plane.normal[2]]])
transformed_vertices = np.matmul(twodpoints, rotation_matrix)
transformed_vertices = np.add(transformed_vertices, self.plane.origin)
But this doesn't seem to do the projection correctly, the points projected back in 3D do not lie on the original 3D plane at all. Does anyone know why this is wrong or does anyone have a suggestion that would work better?
In this example I just projected the same points back into 3D to see if it works correctly, which it doesn't. In reality I'll have different points that need to be projected back, but they still need to be in the same plane in 3D space.
# You have a plane perpendicular to a vector
# N = np.array([x_N, y_N, z_N])
# and passing through a point
# Q = np.array([x_Q, y_Q, z_Q])
U = np.zeros((3,3))
U[2,:] = N / np.linalg.norm(N)
e = np.array([0,0,0])
e[np.argmin(np.abs(U[0,:]))] = 1
U[0, :] = np.cross(e, U[2,:])
U[0, :] = U[0, :] / np.linalg.norm(U[0, :])
U[1, :] = np.cross(U[2, :], U[0, :])
point2D = (point23D - Q).dot(U)
result_point2D = some_calcs(point2D)
result_point23D = res_point2D.dot(U.transpose())
Related
I'm trying to project 3D body keypoints to 2D keypoints,
My 3D points are:
points = np.array([[-7.55801499e-02, -3.69511306e-01, -2.63576955e-01],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
[ 3.08661222e-01, -2.93346141e-02, 3.72593999e-02],
[ 5.96781611e-01, -2.82074720e-01, 4.71359938e-01],
[ 5.38534284e-01, -8.05779934e-01, 4.68694866e-01],
[-3.67936224e-01, -1.09069087e-01, 9.90774706e-02],
[-5.24732828e-01, -2.87176669e-01, 6.09635711e-01],
[-4.37022656e-01, -7.87327409e-01, 4.43706572e-01],
[ 1.33009470e-09, -5.10657072e-09, 1.00000000e+00],
[ 1.13241628e-01, 3.25177647e-02, 1.24026799e+00],
[ 3.43442023e-01, -2.51034945e-01, 1.90472209e+00],
[ 2.57550180e-01, -2.86886752e-01, 2.75528717e+00],
[-1.37361348e-01, -2.60521360e-02, 1.19951272e+00],
[-3.26779515e-01, -5.59706092e-01, 1.75905156e+00],
[-4.65996087e-01, -7.69565761e-01, 2.56634569e+00],
[-1.89841837e-02, -3.19088846e-01, -3.69913191e-01],
[-1.61812544e-01, -3.10732543e-01, -3.47061515e-01],
[ 7.68100023e-02, -1.19293019e-01, -3.72248143e-01],
[-2.24317372e-01, -1.02143347e-01, -3.32051814e-01],
[-3.77829641e-01, -1.19915462e+00, 2.56900430e+00],
[-5.45104921e-01, -1.13393784e+00, 2.57149625e+00],
[-5.66698492e-01, -6.89325571e-01, 2.67840290e+00],
[ 4.65222150e-01, -6.44857705e-01, 2.83186650e+00],
[ 5.27995050e-01, -4.69421804e-01, 2.87518311e+00],
[ 1.77749291e-01, -1.74753308e-01, 2.88810611e+00]])
I plotted them using:
fig = plt.figure()
ax = plt.axes(projection='3d')
ax.set_xlim3d(1, -1)
ax.set_ylim3d(1, -1)
ax.set_zlim3d(1, -1)
ax.scatter3D(points[:, 0], points[:, 1],
points[:, 2], cmap='Greens')
The result is:
I want an array of 2D points with the same camera view, so my desired result a 2D array:
I have tried so far:
import cv2
ans = []
for k in range(25):
tmp = np.array(s[0, k, :]).reshape(1,3)
revc = np.array([0, 0, 0], np.float) # rotation vector
tvec = np.array([0, 0, 0], np.float) # translation vector
fx = fy = 1.0
cx = cy = 0.0
cameraMatrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
result = cv2.projectPoints(tmp, revc, tvec, cameraMatrix, None)
ans.append(result[0])
ans = np.array(ans).squeeze()
But the result I'm getting is:
plt.scatter(ans[:,0], ans[:, 1])
I can't figure out why the information is lost during projection, kindly help me in this. Also its not necessary for me to use OpenCV so you can suggest other methods like using numpy too.
Thanks
Here's a way to do this from "scratch". I have the following import statements:
import numpy as np
import matplotlib.pyplot as plt
from numpy import sin,cos,pi
from scipy.linalg import norm
After your 3d plotting code, I added the following:
azim = ax.azim*pi/180
elev = ax.elev*pi/180
elev *= 1.2 # this seems to improve the outcome
a_vec = np.array([cos(azim),sin(azim),0])
normal = cos(elev)*a_vec + np.array([0,0,sin(elev)])
z_vec = np.array([0,0,1])
y_comp = z_vec - (z_vec#normal)*normal
y_comp = y_comp/norm(y_comp)
x_comp = np.cross(y_comp,normal)
proj_mat = np.vstack([x_comp,y_comp]) # build projection matrix
proj_mat = -proj_mat # account for flipped axes
points_2D = points # proj_mat.T # apply projection
plt.figure()
plt.scatter(*points_2D.T)
plt.gca().set_aspect('equal', adjustable='box')
plt.axis('off')
plt.show()
The resulting points:
I'm trying to apply the Expectation Maximization Algorithm (EM) to a Gaussian Mixture Model (GMM) using Python and NumPy. The PDF document I am basing my implementation on can be found here.
Below are the equations:
When applying the algorithm I get the mean of the first and second cluster equal to:
array([[2.50832195],
[2.51546208]])
When the actual vector means for the first and second cluster are, respectively:
array([[0],
[0]])
and:
array([[5],
[5]])
The same thing happens when getting the values of the covariance matrices I get:
array([[7.05168736, 6.17098629],
[6.17098629, 7.23009494]])
When it should be:
array([[1, 0],
[0, 1]])
for both clusters.
Here is the code:
np.random.seed(1)
# first cluster
X_11 = np.random.normal(0, 1, 1000)
X_21 = np.random.normal(0, 1, 1000)
# second cluster
X_12 = np.random.normal(5, 1, 1000)
X_22 = np.random.normal(5, 1, 1000)
X_1 = np.concatenate((X_11,X_12), axis=None)
X_2 = np.concatenate((X_21,X_22), axis=None)
# data matrix of k x n dimensions (2 x 2000 dimensions)
X = np.concatenate((np.array([X_1]),np.array([X_2])), axis=0)
# multivariate normal distribution function gives n x 1 vector (2000 x 1 vector)
def normal_distribution(x, mu, sigma):
mvnd = []
for i in range(np.shape(x)[1]):
gd = (2*np.pi)**(-2/2) * np.linalg.det(sigma)**(-1/2) * np.exp((-1/2) * np.dot(np.dot((x[:,i:i+1]-mu).T, np.linalg.inv(sigma)), (x[:,i:i+1]-mu)))
mvnd.append(gd)
return np.reshape(np.array(mvnd), (np.shape(x)[1], 1))
# Initialized parameters
sigma_1 = np.array([[10, 0],
[0, 10]])
sigma_2 = np.array([[10, 0],
[0, 10]])
mu_1 = np.array([[10],
[10]])
mu_2 = np.array([[10],
[10]])
pi_1 = 0.5
pi_2 = 0.5
Sigma_1 = np.empty([2000, 2, 2])
Sigma_2 = np.empty([2000, 2, 2])
for i in range(10):
# E-step:
w_i1 = (pi_1*normal_distribution(X, mu_1, sigma_1))/(pi_1*normal_distribution(X, mu_1, sigma_1) + pi_2*normal_distribution(X, mu_2, sigma_2))
w_i2 = (pi_2*normal_distribution(X, mu_2, sigma_2))/(pi_1*normal_distribution(X, mu_1, sigma_1) + pi_2*normal_distribution(X, mu_2, sigma_2))
# M-step:
pi_1 = np.sum(w_i1)/2000
pi_2 = np.sum(w_i2)/2000
mu_1 = np.array([(1/(np.sum(w_i1)))*np.sum(w_i1.T*X, axis=1)]).T
mu_2 = np.array([(1/(np.sum(w_i2)))*np.sum(w_i2.T*X, axis=1)]).T
for i in range(2000):
Sigma_1[i:i+1, :, :] = w_i1[i:i+1,:]*np.dot((X[:,i:i+1]-mu_1), (X[:,i:i+1]-mu_1).T)
Sigma_2[i:i+1, :, :] = w_i2[i:i+1,:]*np.dot((X[:,i:i+1]-mu_2), (X[:,i:i+1]-mu_2).T)
sigma_1 = (1/(np.sum(w_i1)))*np.sum(Sigma_1, axis=0)
sigma_2 = (1/(np.sum(w_i2)))*np.sum(Sigma_2, axis=0)
Would really appreciate if someone could point out the mistake in my code or in my misunderstanding of the algorithm..
I have a problem showing data in a graph. The graph frame appears, but no graph is to be seen. Can you please help ?
I made sure that the dimension of the x axis and the data is the same ... I simply cannot find out why I do not get a graph in return.
Thank you very much in advance.
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
n = 1000
theta = 0.8
d = np.sqrt(1-theta**2)
def p(x,y):
"Stochastic kernel for the TAR model"
return norm().pdf((y-theta*np.abs(x))/d)/d
Z = norm().rvs(n)
X = np.empty(n)
for t in range(n-1):
X[t+1] = theta*np.abs(X[t])+d*Z[t+1]
n = len(X)
X = X.reshape((n, 1))
ys = np.linspace(-3,3,200)
k = len(ys)
ys = ys.reshape((1,k))
v = p(X,ys)
kernel = np.mean(v, axis=0)
h = len(kernel)
kernel = kernel.reshape((1,h))
fig, ax = plt.subplots(figsize=(10,7))
ax.plot(ys,kernel, 'b-', lw=2,alpha=0.6, label='look ahead estimate')
plt.show()
The problem is, that through reshaping the two 1-dimensional arrays ys and kernel to a 1xk or 1xh array respectively you get 2-dimensional arrays, where the first dimension is 1. The plot function apparently only iterates through the first dimension, which is why the plot doesn't show anything.
I can think of two easy options to fix that:
Do not reshape the variables kernel and ys:
# ... continuing your code ...
ys = np.linspace(-3,3,200)
k = len(ys)
#ys = ys.reshape((1,k))
v = p(X,ys)
kernel = np.mean(v, axis=0)
h = len(kernel)
#kernel = kernel.reshape((1,h))
fig, ax = plt.subplots(figsize=(10,7))
ax.plot(ys,kernel, 'b-', lw=2,alpha=0.6, label='look ahead estimate')
plt.show()
Call your plot function like this:
ax.plot(ys[0],kernel[0], 'b-', lw=2, alpha=0.6, label='look ahead estimate')
I hope this solves your problem.
To understand why you still have to reshape X:
Let's first understand your function p(x,y) in terms of dimensions:
def p(x,y):
"Stochastic kernel for the TAR model"
"""If x is not reshaped, you substract two one-dimensional arrays from each other,
which have not the same dimensions (dim(x) == 1000, dim(y) == 200 in your case).
This throws an error.
If you reshape X before passing to this function, the y array is substracted
element-wise by each of the values of X, which gives you a matrix with dimension
dim(x) x dim(y).
"""
return norm().pdf((y-theta*np.abs(x))/d)/d
For illustration what happens here dimension-wise:
>>> X = np.array([[1], [2], [3], [4]])
>>> Y = np.array([1, 2, 3])
>>> Y-X
array([[ 0, 1, 2],
[-1, 0, 1],
[-2, -1, 0],
[-3, -2, -1]])
Now we take a look what happens with the matrix returned by p(x,y):
The calculation of the kernel with np.mean(v, axis=0), where v is the returned matrix from p(X,ys), works such, that np.mean iterates over the lines of the matrix v and calculates the mean value of each "line vector" in the matrix. This gives you an one dimensional array (dimension of ys) which you can plot over ys.
My goal is to transform an image in such a way that three source points are mapped to three target points in an empty array. I have solved the finding of the correct affine matrix, however I cannot apply an affine transformation on a color image.
More specifically, I am struggling with the correct use of the scipy.ndimage.interpolation.affine_transform method. As this question and it's anwers point out, the affine_transform-method can be somewhat unintuitive (especially regarding offset calculation), however, user timday shows how apply a rotation and a shearing on an image and position it in another array, while user geodata gives more background information.
My problem is to generalize the approach shown there (1) to color images and (2) to an arbitrary transformation which I calculated myself.
This is my code (which should run as is on your computer):
import numpy as np
from scipy import ndimage
import matplotlib.pyplot as plt
def calcAffineMatrix(sourcePoints, targetPoints):
# For three source- and three target points, find the affine transformation
# Function works correctly, not part of the question
A = []
b = []
for sp, trg in zip(sourcePoints, targetPoints):
A.append([sp[0], 0, sp[1], 0, 1, 0])
A.append([0, sp[0], 0, sp[1], 0, 1])
b.append(trg[0])
b.append(trg[1])
result, resids, rank, s = np.linalg.lstsq(np.array(A), np.array(b))
a0, a1, a2, a3, a4, a5 = result
# Ignoring offset here, later use timday's suggested offset calculation
affineTrafo = np.array([[a0, a1, 0], [a2, a3, 0], [0, 0, 1]], 'd')
# Testing the correctness of transformation matrix
for i, _ in enumerate(sourcePoints):
src = sourcePoints[i]
src.append(1.)
trg = targetPoints[i]
trg.append(1.)
at = affineTrafo.copy()
at[2, 0:2] = [a4, a5]
assert(np.array_equal(np.round(np.array(src).dot(at)), np.array(trg)))
return affineTrafo
# Prepare source image
sourcePoints = [[162., 112.], [130., 112.], [162., 240.]]
targetPoints = [[180., 102.], [101., 101.], [190., 200.]]
image = np.empty((300, 300, 3), dtype='uint8')
image[:] = 255
# Mark border for better visibility
image[0:2, :] = 0
image[-3:-1, :] = 0
image[:, 0:2] = 0
image[:, -3:-1] = 0
# Mark source points in red
for sp in sourcePoints:
sp = [int(u) for u in sp]
image[sp[1] - 5:sp[1] + 5, sp[0] - 5:sp[0] + 5, :] = np.array([255, 0, 0])
# Show image
plt.subplot(3, 1, 1)
plt.imshow(image)
# Prepare array in which the image is placed
array = np.empty((400, 300, 3), dtype='uint8')
array[:] = 255
a2 = array.copy()
# Mark target points in blue
for tp in targetPoints:
tp = [int(u) for u in tp]
a2[tp[1] - 2:tp[1] + 2, tp[0] - 2:tp[0] + 2] = [0, 0, 255]
# Show array
plt.subplot(3, 1, 2)
plt.imshow(a2)
# Next 5 program lines are actually relevant for question:
# Calculate affine matrix
affineTrafo = calcAffineMatrix(sourcePoints, targetPoints)
# This follows the c_in-c_out method proposed in linked stackoverflow issue
# extended for color channel (no translation here)
c_in = np.array([sourcePoints[0][0], sourcePoints[0][1], 0])
c_out = np.array([targetPoints[0][0], targetPoints[0][1], 0])
offset = (c_in - np.dot(c_out, affineTrafo))
# Affine transform!
ndimage.interpolation.affine_transform(image, affineTrafo, order=2, offset=offset,
output=array, output_shape=array.shape,
cval=255)
# Mark blue target points in array, expected to be above red source points
for tp in targetPoints:
tp = [int(u) for u in tp]
array[tp[1] - 2:tp[1] + 2, tp[0] - 2:tp[0] + 2] = [0, 0, 255]
plt.subplot(3, 1, 3)
plt.imshow(array)
plt.show()
Other approaches I tried include working with the inverse, transpose or both of affineTrafo:
affineTrafo = np.linalg.inv(affineTrafo)
affineTrafo = affineTrafo.T
affineTrafo = np.linalg.inv(affineTrafo.T)
affineTrafo = np.linalg.inv(affineTrafo).T
In his answer, geodata shows how to calculate the matrix that affine_trafo needs to do a scaling and rotation:
If one wants a scaling S first and then a rotation R it holds that T=R*S and therefore T.inv=S.inv*R.inv (note the reversed order).
Which I tried to copy using matrix decomposition (decomposing my affine transformation into a rotation, a shearing and another rotation):
u, s, v = np.linalg.svd(affineTrafo[:2,:2])
uInv = np.linalg.inv(u)
sInv = np.linalg.inv(np.diag((s)))
vInv = np.linalg.inv(v)
affineTrafo[:2, :2] = uInv.dot(sInv).dot(vInv)
Again, without success.
For all of my results, it's not (only) an offset problem. It is clearly visible from the pictures that the relative positions of source and target points do not correspond.
I searched the web and stackoverflow and did not find an answer for my problem. Please help me! :)
I finally got it working thanks to AlexanderReynolds hint to use another library. This is of course a workaround; I could not get it working using scipy's affine_transform, so I used OpenCVs cv2.warpAffine instead. In case this is helpful to anyone else, this is my code:
import numpy as np
import matplotlib.pyplot as plt
import cv2
# Prepare source image
sourcePoints = [[162., 112.], [130., 112.], [162., 240.]]
targetPoints = [[180., 102.], [101., 101.], [190., 200.]]
image = np.empty((300, 300, 3), dtype='uint8')
image[:] = 255
# Mark border for better visibility
image[0:2, :] = 0
image[-3:-1, :] = 0
image[:, 0:2] = 0
image[:, -3:-1] = 0
# Mark source points in red
for sp in sourcePoints:
sp = [int(u) for u in sp]
image[sp[1] - 5:sp[1] + 5, sp[0] - 5:sp[0] + 5, :] = np.array([255, 0, 0])
# Show image
plt.subplot(3, 1, 1)
plt.imshow(image)
# Prepare array in which the image is placed
array = np.empty((400, 300, 3), dtype='uint8')
array[:] = 255
a2 = array.copy()
# Mark target points in blue
for tp in targetPoints:
tp = [int(u) for u in tp]
a2[tp[1] - 2:tp[1] + 2, tp[0] - 2:tp[0] + 2] = [0, 0, 255]
# Show array
plt.subplot(3, 1, 2)
plt.imshow(a2)
# Calculate affine matrix and transform image
M = cv2.getAffineTransform(np.float32(sourcePoints), np.float32(targetPoints))
array = cv2.warpAffine(image, M, array.shape[:2], borderValue=[255, 255, 255])
# Mark blue target points in array, expected to be above red source points
for tp in targetPoints:
tp = [int(u) for u in tp]
array[tp[1] - 2:tp[1] + 2, tp[0] - 2:tp[0] + 2] = [0, 0, 255]
plt.subplot(3, 1, 3)
plt.imshow(array)
plt.show()
Comments:
Interesting how it worked almost immediately after changing the library. After having spent more than a day trying to get it work with scipy, this is a lesson for myself to change libraries faster.
In case someone wants to find an (least squares) approximation for an affine transformation based on more than three points, this is how you get the matrix that works with cv2.warpAffine:
Code:
def calcAffineMatrix(sourcePoints, targetPoints):
# For three or more source and target points, find the affine transformation
A = []
b = []
for sp, trg in zip(sourcePoints, targetPoints):
A.append([sp[0], 0, sp[1], 0, 1, 0])
A.append([0, sp[0], 0, sp[1], 0, 1])
b.append(trg[0])
b.append(trg[1])
result, resids, rank, s = np.linalg.lstsq(np.array(A), np.array(b))
a0, a1, a2, a3, a4, a5 = result
affineTrafo = np.float32([[a0, a2, a4], [a1, a3, a5]])
return affineTrafo
I want to solve for the extrinsics by using direct linear transformation on corresponding 3D LIDAR points and 2D camera points. I already have the intrinsics.
Problem is, points behind the camera gets re-projected as well (see picture below).
So I constrain to only points "in front of the camera", i.e z > 0. The problem is, on different trials where different sets of points are used, the produced extrinsic matrix produces differing axes. Sometimes, constraining z > 0 gives the right results (centre part of image), whereas other times I need z < 0, which I believe to be the z-axis going into the camera. So the question is, how do I constrain the Z axes of the camera to be sticking out of the camera?
def with_intrinsic(points2d, points3d, intrinsic):
cam1_K_inverse = np.linalg.inv(intrinsic)
#direct linear transformation calibration, assumes no intrinsic matrix
assert points2d.shape[0] >= 3
assert points3d.shape[0] == points2d.shape[0]
A = []
points2d_homo = []
for u,v in points2d:
points2d_homo.append([u, v, 1])
points2d_homo = np.array(points2d_homo).T #columns to be data points
points2d_inv = np.dot(cam1_K_inverse, points2d_homo).T
assert points2d_inv.shape == (points2d.shape[0], 3)
assert points2d_inv[0, 2] == 1
for idx in range(points2d.shape[0]):
x3d, y3d, z3d = points3d[idx]
u, v, _ = points2d_inv[idx]
A.append([x3d, y3d, z3d, 1, 0, 0, 0, 0, -u * x3d, -u * y3d, -u * z3d, -u])
A.append([0, 0, 0, 0, x3d, y3d, z3d, 1, -v * x3d, -v * y3d, -v * z3d, -v])
A = np.array(A)
U, D, VT = np.linalg.svd(A)
M = VT.T[:, -1].reshape((3, 4))
error = get_reprojection_error(points2d, points3d, intrinsic, M)
logging.debug("error with_intrinsic: %s", error)
return M
update: I tried to check if re-projecting 1 of the "training" points will yield me z < 0. If so, I do a np.dot(R, extrinsic) to rotate the point about PI radians around 1 of the axis. I've tried all 3 axes but that still don't seem to yield the correct result.
R1 = np.array([
[1, 0, 0],
[0, np.cos(pi), -np.sin(pi)],
[0, np.sin(pi), np.cos(pi)],
])
R2 = np.array([
[np.cos(pi), 0, np.sin(pi)],
[0, 1, 0],
[-np.sin(pi), 0, np.cos(pi)],
])
R3 = np.array([
[np.cos(pi), -np.sin(pi), 0],
[np.sin(pi), np.cos(pi), 0],
[0, 0, 1],
])