(Rewritten, compressed and partly corrected version)
I try some trivial rotation of a triangle in 3d to point its normal straight up without any distortion. I'm following this post calculate-rotation-matrix-to-align-vector, but for some reasons, my result is undesirable and I can't see the error in my code or my error in reasoning. My Python code as followed:
# defining the triangle with three points in 3d space (xyz)
v1 = [1,1,1]
v2 = [2,2,2]
v3 = [2,1,3]
# calculating the normal and converting to unit-vector
n = np.cross( np.subtract(v2,v1), np.subtract(v3,v1) )
a = n / np.sqrt( n[0]**2 + n[1]**2 + n[2]**2 )
b = [0,1,0]
v = np.cross(a,b)
s = np.sqrt(v.dot(v))
c = np.dot(a,b)
Vx = np.array([
[ 0 , -v[2], v[1] ],
[ v[2], 0 , -v[0]],
[-v[1], v[0], 0 ]
])
Vx2 = Vx.dot(Vx)
I = np.identity(3)
iv = (1/(1+c))
# single rotation matrix
R = np.add(I, np.add(Vx, np.multiply(Vx2,iv)))
Then calculating the centroid, the rotation matrix as described and also as separated matrices as explained here Rotating a Vector in 3D Space
cx = (v1[0]+v2[0]+v3[0]) / 3.0
cy = (v1[1]+v2[1]+v3[1]) / 3.0
cz = (v1[2]+v2[2]+v3[2]) / 3.0
v1t = [v1[0]-cx,v1[1]-cy,v1[2]-cz]
v2t = [v2[0]-cx,v2[1]-cy,v2[2]-cz]
v3t = [v3[0]-cx,v3[1]-cy,v3[2]-cz]
As well as the single rotation matrices:
rz = np.array([
[c,-s, 0],
[s, c, 0],
[0, 0, 1]
])
ry = np.array([
[ c, 0, s],
[ 0, 1, 0],
[-s, 0, c]
])
rx = np.array([
[1, 0, 0],
[0, c,-s],
[0, s, c]
])
rxyz = rx.dot(ry).dot(rz)
Now I apply the rotation, with R and rxyz:
p1 = R.dot(v1t)
p2 = R.dot(v2t)
p3 = R.dot(v3t)
p4 = rxyz.dot(v1t)
p5 = rxyz.dot(v2t)
p6 = rxyz.dot(v3t)
The result is now better than the first approach and the Y of p1 and p3 are the correctly same, but p2 is still wrong. And there still seems to be a distortion.
P1-3 (red)
[-0.20673470096224283, 1.1102230246251565e-16, -1.2299659828522118]
[-0.5865305980755144, -3.885780586188048e-16, 0.45993196570442385]
[0.7932652990377571, 1.1102230246251565e-16, 0.770034017147788]
P4-6 (blue)
[-1.1482080390363765, 0.30059830961195716, -0.38316381732390803]
[0.3040075530555323, -0.6336677067539932, -0.2481938771563051]
[0.8442004859808443, 0.3330693971420361, 0.6313576944802128]
The light gray triangle is the original translated to 0,0,0 and red and blue are the rotated ones.
Related
As far as I know, after image resize, the corresponding intrinsic parameter K also changes proportionally, but why the coordinates of the 3D reconstruction of the same point are not the same?
The following python program is a simple experiment, the original image size is , after resize it becomes , the intrinsic parameter K1 corresponds to the original image, the intrinsic parameter K2 corresponds to the resize, RT1, RT2 are the extrinsic projection matrix of the camera (should remain unchanged?,[R,T], size), without considering the effects of camera skew factor and distortions,why is there a difference in the reconstructed 3D points?
import cv2
import numpy as np
fx = 1040
fy = 1040
cx = 1920 / 2
cy = 1080 / 2
K1 = np.array([[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]])
RT1 = np.array([[1, 0, 0, 4],
[0, 1, 0, 5],
[0, 0, 1, 6]]) # just random set
theta = np.pi / 6
RT2 = np.array([[np.cos(theta), -np.sin(theta), 0, 40],
[np.sin(theta), np.cos(theta), 0, 50],
[0, 0, 1, 60]]) # just random set
p1 = np.matmul(K1, RT1) # extrinsic projection matrix
p2 = np.matmul(K1, RT2) # extrinsic projection matrix
pt1 = np.array([100.0, 200.0])
pt2 = np.array([300.0, 400.0])
point3d1 = cv2.triangulatePoints(p1, p2, pt1, pt2)
# Remember to divide out the 4th row. Make it homogeneous
point3d1 = point3d1 / point3d1[3]
print(point3d1)
[[-260.07160113]
[ -27.39546108]
[ 273.95189881]
[ 1. ]]
then resize image to test recontruct 3D point, see if it is numerical equal.
rx = 640.0 / 1920.0
ry = 480.0 / 1080.0
fx = fx * rx
fy = fy * ry
cx = cx * rx
cy = cy * ry
K2 = np.array([[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]])
p1 = np.matmul(K2, RT1)
p2 = np.matmul(K2, RT2)
pt1 = np.array([pt1[0] * rx, pt1[1] * ry])
pt2 = np.array([pt2[0] * rx, pt2[1] * ry])
point3d2 = cv2.triangulatePoints(p1, p2, pt1, pt2)
# Remember to divide out the 4th row. Make it homogeneous
point3d2 = point3d2 / point3d2[3]
print(point3d2)
[[-193.03965985]
[ -26.72133393]
[ 189.12512305]
[ 1. ]]
you see, point3d1 and point3d2 is not same,why?
After careful consideration, I was lucky to get a more plausible explanation, which I now state as follows to help others.
In a short conclusion:
Image scaling must specify a uniform (fx=fy) scaling factor in order to derive the correct intrinsic parameter K, otherwise inconsistencies in the x,y axis focal lengths with respect to the original image directly lead to deviations in the calculated 3D points!
Returning to the problem at the beginning, the given image size is 1080×1920, and its focal length is 1040 pixels, i.e. fx=fy=1040, because by definition fx=f/dx,fy=f/dy, where dx, dy are the number of pixels per unit length, and f is the actual physical size of the focal length; thus the a priori dx=dy can be introduced, which is constant This "convention" should also be followed for later image scaling.
Imagine if the scaled image fx,fy were obtained in different proportions, dx,dy would not be the same, causing distortion of the image, and in addition, according to the external projection matrix P = K*[R,t], fx,fy in K would vary disproportionately leading to a deviation in the calculated P!
BTW, Similarly, I put the reference answer to the experiment done by matlab at this link.
I'm trying to calculate 3D point from 2 Images. However I'm not sure whether my Implementation/thought process is correct, because I don't really know which of my calculated 3D points/Rotation & Translation matrix is the correct one.
Things which I can provide or have done:
got the intrinsic parameters of both cameras k1 and k2
calculated the fundamental matrix with cv2.findFundamentalMat (fairly sure that it is correct, because i checked it with the equation x'Fx = 0)
k1 = np.array([[512, 0, 512.],
[ 0, 512, 384],
[ 0, 0, 1]]).reshape(3,3)
k2 =np.array([[512, 0, 512.],
[ 0, 512, 384],
[ 0, 0, 1]]).reshape(3,3)
fMatrix = np.array([[ 2.13503670e-06, -1.09093289e-05, 1.34791051e-03],
[ 1.15140270e-05, -8.93292052e-07, -8.31221024e-03],
[-4.24200928e-03, 7.99815714e-03, 1.00000000e+00]]).reshape(3,3)
p1 = np.array([752, 573, 1])
p2 = np.array([701, 542, 1])
print(p2.T.dot(fMatrix).dot(p2))
-0.015196065125415714
calculated the essential matrix E = K2.T * F *K1 (Not sure if this is correct)
extracted the rotation and translation Matrix R, R2 and Sb, S2b.
constructed the 4 possible extrinsic matricies M1,M2, M3, M4, with [R|t]
essentialMatrix = k2.T.dot(fMatrix).dot(k1)
U, S, V = np.linalg.svd(essentialMatrix)
newEssentialMatrix = U.dot(np.diag([1,1,0])).dot(V.T)
U, S, V = np.linalg.svd(newEssentialMatrix)
W = np.array([[0, -1, 0], [1, 0, 0],[0, 0, 1]]).reshape(3,3)
Z = np.array([[0,1,0],[-1,0,0],[0,0,0]]).reshape(3,3)
Sb = U.dot(Z).dot(U.T) # [t]x
R = U.dot(W).dot(V.T) # R
S2b = U.dot(Z.T).dot(U.T) # [t]x
R2 = U.dot(W.T).dot(V.T) # R
t1 = np.array([[Sb[2][1]],[Sb[0][2]],[Sb[1][0]]])
t2 = np.array([[S2b[2][1]],[S2b[0][2]],[S2b[1][0]]])
# M = [U W.T V.T | t] = # M = [U W.T V.T | t] = [R | t]
M1 = np.append(R, t1, axis=1)
M2 = np.append(R, t2, axis=1)
M3 = np.append(R2, t1, axis=1)
M4 = np.append(R2, t2, axis=1)
trying to verify which extrinic/projection Matrix I need to use. In other words I'm creating for camera1 the projection matrix P1 = k1*[I|0].
And for camera2 4 different projection Matricies P2 = k2*M1, P2 = k2*M2, P2 = k2*M3, P2 = k2*M4
Testing\calculating with a the corresponding points p1 and p2 and cv2.triangulatePoints()
origin = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0]]).reshape(3,4)
projectionM1 = k1.dot(origin)
projectionM2 = k2.dot(M1)
p3D = cv2.triangulatePoints(projectionM1,projectionM2,p1[:2],p2[:2])
print("M1")
print(p3D/p3D[3])
print("-----")
projectionM2 = k2.dot(M2)
p3D = cv2.triangulatePoints(projectionM1,projectionM2,p1[:2],p2[:2])
print("M2")
print(p3D/p3D[3])
print("-----")
projectionM2 = k2.dot(M3)
p3D = cv2.triangulatePoints(projectionM1,projectionM2,p1[:2],p2[:2])
print("M3")
print(p3D/p3D[3])
print("-----")
projectionM2 = k2.dot(M4)
p3D = cv2.triangulatePoints(projectionM1,projectionM2,p1[:2],p2[:2])
print("M4")
print(p3D/p3D[3])
So here are my results:
M1
[[-0.19115383]
[-0.12595232]
[-0.51133412]
[ 1. ]]
-----
M2
[[0.19115383]
[0.12595232]
[0.51133412]
[1. ]]
-----
M3
[[-0.6098932 ]
[-0.25755958]
[-1.29386456]
[ 1. ]]
-----
M4
[[0.6098932 ]
[0.25755958]
[1.29386456]
[1. ]]
So my question is which one is the correct one?
My guess it's either M2 or M4 since they only contain positive Z values, but then again, I have no idea how the coordinate system of camera1 is defined, maybe M1 or M3 might be correct.
In this picture you can see the perspective of camera1 with the point which should be calculated.
If anything else is missing or wrong pls don't hesitate to tell me. Thank you very much for your help.
I have two copies of the same molecule as .xyz file. This means that each atom is has X, Y and Z coordinates. However, you can rotate the molecule and obtain different coordinates for each atom, although the relative positions are the same and the molecule remains the same. I want to align the two molecules using three atoms as reference points. However, I am struggling to completely align the two molecules.
Firstly, I align both molecules by translation for a single atom. Then, I am doing two subsequent rotation using rotation matrices as explained elsewhere. For some reason, I need to take the negative of the cross product of both vectors and use a sinus instead of a cosinus to get both structures to be perfectly aligned (I discovered this after a lot of trial and error).
For the second rotation, I project both vectors I want to align on a plane defined by the rotation vector. This is necessary because I don't want to rotate along the cross product of the two vectors to align, since that would disalign the rest of the molecule. Instead, I rotate along the two already aligned vectors. The project allows me to find the angle in the plane between the two vectors, and thus the rotation necessary.
However, this code does not properly align the two molecules.
"group1[0]" contains the XYZ coordinates of the three atoms to align in a list. Likewise for "group2[0]" and the structure 2.
#Point 1: align the functional groups to the origin
O1 = np.array(coords1[group1[0][0]])
O2 = np.array(coords2[group2[0][0]])
mat_2 = np.zeros((len(atoms2), 3))
for ind, c in enumerate(coords1):
coords1[ind] = np.array(c) - O1
for ind, c in enumerate(coords2):
coords2[ind] = np.array(c) - O2
mat_2[ind] = coords2[ind]
#Point 2: align according to a first vector
v1 = np.array(coords1[group1[0][1]])#Since atom 1 is the origin, the coordinates is the vector already
v2 = np.array(coords2[group2[0][1]])#Since atom 1 is the origin, the coordinates is the vector already
v1 = v1/np.linalg.norm(v1)
v2 = v2/np.linalg.norm(v2)
#Let v be the axis of rotation
v = -np.cross(v1, v2)#why do I need a minus here?
if np.linalg.norm(v) != 0:
a = np.arccos(np.dot(v1, v2)/(np.linalg.norm(v1)*np.linalg.norm(v2)))
#c = np.dot(v1, v2)*np.cos(a)
c = np.dot(v1, v2)*np.sin(a)#The internet says cos, but this works perfectly
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
rot_mat = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) + vx + vx.dot(vx)*(1-c)/(1-c**2)
mat_2 = np.array(mat_2)
R_mat_rot = np.matmul(rot_mat, mat_2.T).T
else:
exit(0)
coords3 = R_mat_rot.copy()
#I get exactly what I want up until here
#Point 3: Rotate along atom2-atom1 (v1) to align the third atom
v = -v1.copy()
v2 = np.array(coords3[group2[0][2]]) - np.array(coords3[group2[0][0]]) #Since atom 1 is the origin, the coordinates is the vector already
v2 = v2/np.linalg.norm(v2)
v1 = np.array(coords1[group1[0][2]]) - np.array(coords1[group1[0][0]]) #Since atom 1 is the origin, the coordinates is the vector already
v1 = v1/np.linalg.norm(v1)
if np.linalg.norm(v) != 0:
#consider v to be the vector normal to a plane
#we want the projection of v1 and v2 unto that plane
vp1 = np.cross(v, np.cross(v1, v)) - np.array(coords1[group1[0][0]])
vp1 = vp1/np.linalg.norm(vp1)
vp2 = np.cross(v, np.cross(v2, v)) - np.array(coords3[group2[0][0]])
vp2 = vp2/np.linalg.norm(vp2)
#we find the angle between those vectors on the plane
a = np.arccos(np.dot(vp1, vp2))/(np.linalg.norm(vp1)*np.linalg.norm(vp2))
#rotation of that amount
c = np.dot(v1, v2)*np.cos(a)
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
rot_mat = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) + vx + np.dot(vx, vx)*(1-c)/(1-c**2)
R_mat_rot = np.matmul(rot_mat, coords3.T).T
coords4 = R_mat_rot.copy()#Final coordinates
I have come across this matrix multiplication problem where M is some non-singular 3x3 matrix with known values (i.e M = sympy.Matrix([[1, 0, 0],[0, 2, 0],[0, 0, 3]])) C is a 3x3 matrix to be determined and N is of the following form:
1. The 1st and 3rdrow of N are the same as C (e.g N.row(0)[i] = C.row(0)[i] for 0<=i<=2)
2. The elements in the 2nd row of N are the sum of the corresponding column in M (e.g N.row(1)[1] = sum(M.col(1)))
After searching the web for a way to express this problem as a system of equations I've found nothing. I've been trying to solve this using symbolic matrices and by or by solving three different systems of the form Ax=b each one made of a row from C multiplied by M with b as a column from N such that A = M.T, x = (C.row(i)).T and b = N.
Solving it symbolically resulted in a ridiculous expression that cannot be even be comprehended and I was unable to get a numeric solution from it.
My latest attempt follow:
import sympy as sp
def func(mat=matrix([[1, 1, 1], [0, 2, 2], [1, 4, 5]])):
c11, c12, c13, c21, c22, c23, c31, c32, c33 = sp.symbols('c11, c12, c13, c21, c22, c23, c31, c32, c33')
M = mat.T
b1 = sp.Matrix([[x, y, z]]).T
b2 = sp.Matrix([[sum(M.col(0)), sum(M.col(1)), sum(M.col(2))]]).T
b3 = sp.Matrix([[a, b, c]]).T
M1 = M.col_insert(3, b1)
M2 = M.col_insert(3, b2)
M3 = M.col_insert(3, b3)
C1 = sp.linsolve(M1, (x, y, z))
C2 = sp.linsolve(M2, (x, y, z))
C3 = sp.linsolve(M3, (a, b, c))
return C1, C2, C3
Calling this yields the following:
>>> func()
({(x + y - z, -x/2 + 2*y - 3*z/2, -y + z)}, {(-3, -17/2, 6)}, {(a + b - c, -a/2 + 2*b - 3*c/2, -b + c)})
I won't claim I understand your code, but the solution is actually easy to guess: The first and third rows of C and N must either be left eigenvectors of M with eigenvalue 1 which will in the general case not exist or must be zero. The middle row requirement is solved by C being all ones and since M is nonsingular that's the only solution.
Let's use good old numpy to numerically check this:
import numpy as np
M = np.random.random((3, 3))
M
# array([[ 0.39632944, 0.82429087, 0.88705214],
# [ 0.39092656, 0.63228762, 0.54931835],
# [ 0.76935833, 0.40833527, 0.46202912]])
C = np.outer((0,1,0),(1,1,1))
C
# array([[0, 0, 0]
# [1, 1, 1],
# [0, 0, 0]])
N = np.outer((0,1,0),M.sum(0))
N
# array([[ 0. , 0. , 0. ],
# [ 1.55661432, 1.86491377, 1.89839961],
# [ 0. , 0. , 0. ]])
np.allclose(C # M , N)
# True
What is the easiest/fastest way to take a weighted sum of values in a numpy array?
Example: Solving the heat equation with the Euler method
length_l=10
time_l=10
u=zeros((length_l,length_l))# (x,y)
u[:, 0]=1
u[:,-1]=1
print(u)
def dStep(ALPHA=0.1):
for position,value in ndenumerate(u):
D2u= (u[position+(1,0)]-2*value+u[position+(-1, 0)])/(1**2) \
+(u[position+(0,1)]-2*value+u[position+( 0,-1)])/(1**2)
value+=ALPHA*D2u()
while True:
dStep()
print(u)
D2u should be the second central difference in two dimensions. This would work if I could add indexes like (1,4)+(1,3)=(2,7). Unfortunately, python adds them as (1,4)+(1,3)=(1,4,1,3).
Note that computing D2u is equivalent to taking a dot product with this kernel centered around the current position:
0, 1, 0
1,-4, 1
0, 1, 0
Can this be vectorised as a dot product?
I think you want something like:
import numpy as np
from scipy.ndimage import convolve
length_l = 10
time_l = 10
u = np.zeros((length_l, length_l))# (x,y)
u[:, 0] = 1
u[:, -1] = 1
alpha = .1
weights = np.array([[ 0, 1, 0],
[ 1, -4, 1],
[ 0, 1, 0]])
for i in range(5):
u += alpha * convolve(u, weights)
print(u)
You could reduce down a bit by doing:
weights = alpha * weights
weights[1, 1] = weights[1, 1] + 1
for i in range(5):
u = convolve(u, weights)
print(u)