Develop Reference

How to implement rigid body rotation math in python

I have two sets of three 3d points of the same rigid body that's shape is a (non-perfect) right triangle.
The sets are the initial and final states. The points in each of the sets are corresponding. Both have its first/front points at the origin. The initial state represents when the figures orientation is at (0,0,0) in terms of pitch, roll, yaw. I am trying to find the orientation of the final state in the same format or similar with a different order. I dont want R matrix or quaternions.
I decieded to implement this method of finding the R matrix from two sets of three points. Specifically its the bottom part that starts with "More information, easier computation". I replaced the P's and Q's with I's and F's. In this method he assumes that the "distances between the Pi's and Qi's are the same". I believe I can assert this by doing:
assert round(math.sqrt(sum(a**2 for a in initial_state[1])), 3) == round(math.sqrt(sum(a**2 for a in final_state[1])), 3)
assert round(math.sqrt(sum(a**2 for a in initial_state[2])), 3) == round(math.sqrt(sum(a**2 for a in final_state[2])), 3)
After finding the R matrix I convert it to angles. I'm not sure whats wrong with the implementation but its giving some wacky angles. It may be due to the rotationMatrixToEulerAngles() method as well.
import matplotlib.pyplot as plt
import numpy as np
import math
def rotationMatrixToEulerAngles(R):
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
singular = sy < 1e-6
if not singular :
x = math.atan2(R[2,1] , R[2,2])
y = math.atan2(-R[2,0], sy)
z = math.atan2(R[1,0], R[0,0])
else :
x = math.atan2(-R[1,2], R[1,1])
y = math.atan2(-R[2,0], sy)
z = 0
result = np.array([math.degrees(x)*-1, math.degrees(y)*-1, math.degrees(z)*-1])
# make sure not to return -0
for i, _ in enumerate(result):
if result[i] == -0.0:
result[i] = 0
return result
fig = plt.figure(figsize=(7,7))
ax = fig.add_subplot(111, projection='3d')
initial_state = np.array(
[[ 0.0, 0.0, 0.0 ],
[-15.79080794, 0.08799, 0.0 ],
[ 0.0, -12.49253637, 0.0 ]])
final_state = np.array(
[[ 0.0, 0.0, 0.0 ],
[-15.76038, 0.39484, 0.901 ],
[ -0.28012, -12.47225, -0.6542 ]])
# ------ Implementation of Strategy ------
i4 = np.add(np.cross(np.subtract(initial_state[1], initial_state[0]), np.subtract(initial_state[2], initial_state[0])), initial_state[0])
f4 = np.add(np.cross(np.subtract(final_state[1], final_state[0]), np.subtract(final_state[2], final_state[0])), final_state[0])
I = np.array([np.subtract(initial_state[1], initial_state[0]),
np.subtract(initial_state[2], initial_state[0]),
np.subtract(i4, initial_state[0])])
F = np.array([np.subtract(final_state[1], final_state[0]),
np.subtract(final_state[2], final_state[0]),
np.subtract(f4, final_state[0])])
r_matrix = F # np.linalg.inv(I)
angles = rotationMatrixToEulerAngles(r_matrix)
print(angles[0])
print(angles[1])
print(angles[2])
# plot inital vs final state
for o in initial_state:
ax.scatter(o[0], o[1], o[2])
for o in final_state:
ax.scatter(o[0], o[1], o[2])
plt.xlabel("X")
plt.ylabel("Y")
plt.legend(["1", "2", "3", "a", "b", "c"])
ax.set_ylim3d(-18, 2)
ax.set_xlim3d(-18, 2)
ax.set_zlim3d(-18, 2)
plt.show()
Output angles:
-40.135994612369494
-34.858079104181336
-1.0182498153168882
Theres no way its turned 40 and 35 degrees.

Sort vertices of (multiple) triangles based on distance to (multiple) reference points

Consider a point (ptA) on the surface of a triangle (with normal nf). To sort the the vertices of a triangle (sx,sy,sz) based on their distance to the reference point (ptA), I do the following:
import numpy as np
ptA = np.array([1.013, 0.013, 0.013])
nf = np.array([1., 1., 1.]) # 'nf' is the face normal
sv = np.eye(3) * nf * 1.039 # Each row of 'sv' is a vertex of the triangle
diff = sv-ptA[:,None]
srtOrdr = np.einsum('ij,ij->j',diff,diff).argsort()
sn = sv[:,srtOrdr] # Sorted vertices
How does one scale this problem and solve it efficiently (without for loop) using python/numpy i.e., multiple triangles each with a reference point, sort the vertices based on the corresponding distances. What I have tried considering a small example (3triangles & 3 reference points):
# The reference points
ptA = np.array([[1.013, 0.013, 0.013],
[0.013, 1.013, 0.013],
[0.013, 0.013, 1.013]])
# The surface normals
nf = np.array([[1.,1.,1.],
[1.,1.,1.],
[1.,1.,1.]])
# The 3 vertices of all the traingles
sx = np.zeros((3, 3))
sx[:, 0] = nf[:, 0] * 1.039
sy = np.zeros((3, 3))
sy[:, 1] = nf[:, 1] * 1.039
sz = np.zeros((3, 3))
sz[:, 2] = nf[:, 2] * 1.039
diffx = sx-ptA
diffy = sy-ptA
diffz = sz-ptA
I am confused on how to go ahead from here. Any help is greatly appreciated.
Please note: I have used (1,1,1) as the surface normal for all test cases just for example sake.

Opencv Homography matrix H and Inverse H to transform a point is not getting expected results

I am using Opencv python interface and got the homography matrix H. It seems to work properly as I can use warp perspective to get warped image from the source image. I now tried to use H and Inverse H to transform a point (not image) back and forth between the two coordinates and is not getting the expected results.
To get the matrix, I did this:
pts1 = np.float32(corners)
pts2 = np.float32([[0,0], [400,0], [400,400], [0,400]])
self.transform_matrix = cv2.getPerspectiveTransform(pts1, pts2)
Given this matrix, I use the following to do a forward and inverse transform:
def transformPoints(self, x, y, reverse=False, integer=True):
if reverse == False:
H = self.transform_matrix
else:
val, H = cv2.invert(self.transform_matrix)
# get the elements in the transform matrix
h0 = H[0,0]
h1 = H[0,1]
h2 = H[0,2]
h3 = H[1,0]
h4 = H[1,1]
h5 = H[1,2]
h6 = H[2,0]
h7 = H[2,1]
h8 = H[2,2]
tx = (h0*x + h1*y + h2)
ty = (h3*x + h4*x + h5)
tz = (h6*x + h7*y + h8)
if integer==True:
px = int(tx/tz)
py = int(ty/tz)
Z = int(1/tz)
else:
px = tx/tz
py = ty/tz
Z = 1/tz
return (px, py)
Now, if I do this:
s, t = 100,200
print "s=%d, t=%d" % (s,t)
a, b = pt.transformPoints(s,t)
print "a=%d, b=%d" % (a,b)
c, d = pt.transformPoints(a, b, True)
print "c=%d, d=%d" % (c,d)
This is what it prints:
a=395, b=169
c=91, d=226
I was expecting c=100 and d=200, or at least something close.
This is the matrix and it's inverse.
H matrix
[[ -1.01486350e-01 -1.99156329e+01 8.44058060e+02]
[ 1.82486862e+00 3.62765073e-01 -1.49259809e+03]
[ -4.43678849e-03 -4.28012674e-02 1.00000000e+00]]
Inverse:
[[ 4.13378829e-01 1.05495739e-01 -1.91452995e+02]
[ -3.12201095e-02 -2.37099792e-02 -9.03788455e+00]
[ 4.97814178e-04 -5.46754880e-04 -2.36269358e-01]]
I tried to do a dot product, and it seems to generate an identity matrix ok:
[[ 1.00000000e+00 1.77635684e-15 -5.68434189e-14]
[ -6.93889390e-18 1.00000000e+00 5.32907052e-15]
[ -2.16840434e-19 1.73472348e-18 1.00000000e+00]]
Any help is appreciated.

You have a typo in second line
tx = (h0*x + h1*y + h2)
ty = (h3*x + h4*x + h5)
tz = (h6*x + h7*y + h8)
h4 should be multiplied with y coordinate

Python scipy getting the results of Delaunay triangulation

I am using scipy.spatial.Delaunay to triangulate a cloud of 3D points.
Each point is imported in an array with the structure R, G, B, X, Y, Z, only the values X, Y, Z are used to triangulate.
I calculate the barycentric coordinates of a list of "target" points.
For each target, I'd like to get:
the R, G, B of each point of the simplex in which target is located.
the barycentric coordinates of target
I cannot figure out how to get these data with my code.
print bcoords works fine (it generates a series of 4 values for each target):
[[ 2.89657287e-01 3.98169955e-01 1.24220635e-01 1.87952122e-01]
[ 3.24695465e-01 3.99228351e-01 8.91849061e-02 1.86891278e-01]
[ 2.89657287e-01 3.98169955e-01 1.24220635e-01 1.87952122e-01]
...,
[ -1.13763739e+03 1.32600196e+03 2.61787735e+02 -4.49152304e+02]
[ -1.13764457e+03 1.32600118e+03 2.61796224e+02 -4.49152837e+02]
[ -1.13765132e+03 1.32600045e+03 2.61804205e+02 -4.49153338e+02]]
However, print tetrahedra generates a list of numbers:
[915 915 915 ..., -1 -1 -1]
And this list is a list of indices
How can I replace it with a list of vertices?
code:
import csv
import numpy as np
import scipy.spatial
points = np.array([(int(R), int(G), int(B), float(X), float(Y), float(Z))
for R, G, B, X, Y, Z in csv.reader(open('XYZcolorlist_D65.csv'))])
# load X,Y,Z coordinates of 'points' in a np.array
# alternative points = pd.read_csv('XYZcolorlist_D65.csv')
tri = scipy.spatial.Delaunay(points[:,[3,4,5]])
# do the triangulation
indices = tri.simplices
# indices of vertices
vertices = points[indices]
# the vertices for each tetrahedron
targets = np.array([(float(X), float(Y), float(Z))
for name, X, Y, Z, crap in csv.reader(open('targets.csv'))])
# load the XYZ target values in a np.array
tetrahedra = tri.find_simplex(targets)
# find which tetrahedron each point belong to
X = tri.transform[tetrahedra,:3]
Y = targets - tri.transform[tetrahedra,3]
b = np.einsum('ijk,ik->ij', X, Y)
bcoords = np.c_[b, 1 - b.sum(axis=1)]
# find the barycentric coordinates of each point
#np.savetxt('coords.out', bcoords, fmt="%f")
print points.shape
print indices.shape
print vertices.shape
print tetrahedra.shape
print bcoords.shape
print bcoords
print tetrahedra
print indices
print vertices

How can an almost arbitrary plane in a 3D dataset be plotted by matplotlib?

There is an array containing 3D data of shape e.g. (64,64,64), how do you plot a plane given by a point and a normal (similar to hkl planes in crystallography), through this dataset?
Similar to what can be done in MayaVi by rotating a plane through the data.
The resulting plot will contain non-square planes in most cases.
Can those be done with matplotlib (some sort of non-rectangular patch)?
Edit: I almost solved this myself (see below) but still wonder how non-rectangular patches can be plotted in matplotlib...?
Edit: Due to discussions below I restated the question.

This is funny, a similar question I replied to just today. The way to go is: interpolation. You can use griddata from scipy.interpolate:
Griddata
This page features a very nice example, and the signature of the function is really close to your data.
You still have to somehow define the points on you plane for which you want to interpolate the data. I will have a look at this, my linear algebra lessons where a couple of years ago

I have the penultimate solution for this problem. Partially solved by using the second answer to Plot a plane based on a normal vector and a point in Matlab or matplotlib :
# coding: utf-8
import numpy as np
from matplotlib.pyplot import imshow,show
A=np.empty((64,64,64)) #This is the data array
def f(x,y):
return np.sin(x/(2*np.pi))+np.cos(y/(2*np.pi))
xx,yy= np.meshgrid(range(64), range(64))
for x in range(64):
A[:,:,x]=f(xx,yy)*np.cos(x/np.pi)
N=np.zeros((64,64))
"""This is the plane we cut from A.
It should be larger than 64, due to diagonal planes being larger.
Will be fixed."""
normal=np.array([-1,-1,1]) #Define cut plane here. Normal vector components restricted to integers
point=np.array([0,0,0])
d = -np.sum(point*normal)
def plane(x,y): # Get plane's z values
return (-normal[0]*x-normal[1]*y-d)/normal[2]
def getZZ(x,y): #Get z for all values x,y. If z>64 it's out of range
for i in x:
for j in y:
if plane(i,j)<64:
N[i,j]=A[i,j,plane(i,j)]
getZZ(range(64),range(64))
imshow(N, interpolation="Nearest")
show()
It's not the ultimate solution since the plot is not restricted to points having a z value, planes larger than 64 * 64 are not accounted for and the planes have to be defined at (0,0,0).

For the reduced requirements, I prepared a simple example
import numpy as np
import pylab as plt
data = np.arange((64**3))
data.resize((64,64,64))
def get_slice(volume, orientation, index):
orientation2slicefunc = {
"x" : lambda ar:ar[index,:,:],
"y" : lambda ar:ar[:,index,:],
"z" : lambda ar:ar[:,:,index]
}
return orientation2slicefunc[orientation](volume)
plt.subplot(221)
plt.imshow(get_slice(data, "x", 10), vmin=0, vmax=64**3)
plt.subplot(222)
plt.imshow(get_slice(data, "x", 39), vmin=0, vmax=64**3)
plt.subplot(223)
plt.imshow(get_slice(data, "y", 15), vmin=0, vmax=64**3)
plt.subplot(224)
plt.imshow(get_slice(data, "z", 25), vmin=0, vmax=64**3)
plt.show()
This leads to the following plot:
The main trick is dictionary mapping orienations to lambda-methods, which saves us from writing annoying if-then-else-blocks. Of course you can decide to give different names,
e.g., numbers, for the orientations.
Maybe this helps you.
Thorsten
P.S.: I didn't care about "IndexOutOfRange", for me it's o.k. to let this exception pop out since it is perfectly understandable in this context.

I had to do something similar for a MRI data enhancement:
Probably the code can be optimized but it works as it is.
My data is 3 dimension numpy array representing an MRI scanner. It has size [128,128,128] but the code can be modified to accept any dimensions. Also when the plane is outside the cube boundary you have to give the default values to the variable fill in the main function, in my case I choose: data_cube[0:5,0:5,0:5].mean()
def create_normal_vector(x, y,z):
normal = np.asarray([x,y,z])
normal = normal/np.sqrt(sum(normal**2))
return normal
def get_plane_equation_parameters(normal,point):
a,b,c = normal
d = np.dot(normal,point)
return a,b,c,d #ax+by+cz=d
def get_point_plane_proximity(plane,point):
#just aproximation
return np.dot(plane[0:-1],point) - plane[-1]
def get_corner_interesections(plane, cube_dim = 128): #to reduce the search space
#dimension is 128,128,128
corners_list = []
only_x = np.zeros(4)
min_prox_x = 9999
min_prox_y = 9999
min_prox_z = 9999
min_prox_yz = 9999
for i in range(cube_dim):
temp_min_prox_x=abs(get_point_plane_proximity(plane,np.asarray([i,0,0])))
# print("pseudo distance x: {0}, point: [{1},0,0]".format(temp_min_prox_x,i))
if temp_min_prox_x < min_prox_x:
min_prox_x = temp_min_prox_x
corner_intersection_x = np.asarray([i,0,0])
only_x[0]= i
temp_min_prox_y=abs(get_point_plane_proximity(plane,np.asarray([i,cube_dim,0])))
# print("pseudo distance y: {0}, point: [{1},{2},0]".format(temp_min_prox_y,i,cube_dim))
if temp_min_prox_y < min_prox_y:
min_prox_y = temp_min_prox_y
corner_intersection_y = np.asarray([i,cube_dim,0])
only_x[1]= i
temp_min_prox_z=abs(get_point_plane_proximity(plane,np.asarray([i,0,cube_dim])))
#print("pseudo distance z: {0}, point: [{1},0,{2}]".format(temp_min_prox_z,i,cube_dim))
if temp_min_prox_z < min_prox_z:
min_prox_z = temp_min_prox_z
corner_intersection_z = np.asarray([i,0,cube_dim])
only_x[2]= i
temp_min_prox_yz=abs(get_point_plane_proximity(plane,np.asarray([i,cube_dim,cube_dim])))
#print("pseudo distance z: {0}, point: [{1},{2},{2}]".format(temp_min_prox_yz,i,cube_dim))
if temp_min_prox_yz < min_prox_yz:
min_prox_yz = temp_min_prox_yz
corner_intersection_yz = np.asarray([i,cube_dim,cube_dim])
only_x[3]= i
corners_list.append(corner_intersection_x)
corners_list.append(corner_intersection_y)
corners_list.append(corner_intersection_z)
corners_list.append(corner_intersection_yz)
corners_list.append(only_x.min())
corners_list.append(only_x.max())
return corners_list
def get_points_intersection(plane,min_x,max_x,data_cube,shape=128):
fill = data_cube[0:5,0:5,0:5].mean() #this can be a parameter
extended_data_cube = np.ones([shape+2,shape,shape])*fill
extended_data_cube[1:shape+1,:,:] = data_cube
diag_image = np.zeros([shape,shape])
min_x_value = 999999
for i in range(shape):
for j in range(shape):
for k in range(int(min_x),int(max_x)+1):
current_value = abs(get_point_plane_proximity(plane,np.asarray([k,i,j])))
#print("current_value:{0}, val: [{1},{2},{3}]".format(current_value,k,i,j))
if current_value < min_x_value:
diag_image[i,j] = extended_data_cube[k,i,j]
min_x_value = current_value
min_x_value = 999999
return diag_image
The way it works is the following:
you create a normal vector:
for example [5,0,3]
normal1=create_normal_vector(5, 0,3) #this is only to normalize
then you create a point:
(my cube data shape is [128,128,128])
point = [64,64,64]
You calculate the plane equation parameters, [a,b,c,d] where ax+by+cz=d
plane1=get_plane_equation_parameters(normal1,point)
then to reduce the search space you can calculate the intersection of the plane with the cube:
corners1 = get_corner_interesections(plane1,128)
where corners1 = [intersection [x,0,0],intersection [x,128,0],intersection [x,0,128],intersection [x,128,128], min intersection [x,y,z], max intersection [x,y,z]]
With all these you can calculate the intersection between the cube and the plane:
image1 = get_points_intersection(plane1,corners1[-2],corners1[-1],data_cube)
Some examples:
normal is [1,0,0] point is [64,64,64]
normal is [5,1,0],[5,1,1],[5,0,1] point is [64,64,64]:
normal is [5,3,0],[5,3,3],[5,0,3] point is [64,64,64]:
normal is [5,-5,0],[5,-5,-5],[5,0,-5] point is [64,64,64]:
Thank you.

The other answers here do not appear to be very efficient with explicit loops over pixels or using scipy.interpolate.griddata, which is designed for unstructured input data. Here is an efficient (vectorized) and generic solution.
There is a pure numpy implementation (for nearest-neighbor "interpolation") and one for linear interpolation, which delegates the interpolation to scipy.ndimage.map_coordinates. (The latter function probably didn't exist in 2013, when this question was asked.)
import numpy as np
from scipy.ndimage import map_coordinates
def slice_datacube(cube, center, eXY, mXY, fill=np.nan, interp=True):
"""Get a 2D slice from a 3-D array.
Copyright: Han-Kwang Nienhuys, 2020.
License: any of CC-BY-SA, CC-BY, BSD, GPL, LGPL
Reference: https://stackoverflow.com/a/62733930/6228891
Parameters:
- cube: 3D array, assumed shape (nx, ny, nz).
- center: shape (3,) with coordinates of center.
can be float.
- eXY: unit vectors, shape (2, 3) - for X and Y axes of the slice.
(unit vectors must be orthogonal; normalization is optional).
- mXY: size tuple of output array (mX, mY) - int.
- fill: value to use for out-of-range points.
- interp: whether to interpolate (rather than using 'nearest')
Return:
- slice: array, shape (mX, mY).
"""
center = np.array(center, dtype=float)
assert center.shape == (3,)
eXY = np.array(eXY)/np.linalg.norm(eXY, axis=1)[:, np.newaxis]
if not np.isclose(eXY[0] # eXY[1], 0, atol=1e-6):
raise ValueError(f'eX and eY not orthogonal.')
# R: rotation matrix: data_coords = center + R # slice_coords
eZ = np.cross(eXY[0], eXY[1])
R = np.array([eXY[0], eXY[1], eZ], dtype=np.float32).T
# setup slice points P with coordinates (X, Y, 0)
mX, mY = int(mXY[0]), int(mXY[1])
Xs = np.arange(0.5-mX/2, 0.5+mX/2)
Ys = np.arange(0.5-mY/2, 0.5+mY/2)
PP = np.zeros((3, mX, mY), dtype=np.float32)
PP[0, :, :] = Xs.reshape(mX, 1)
PP[1, :, :] = Ys.reshape(1, mY)
# Transform to data coordinates (x, y, z) - idx.shape == (3, mX, mY)
if interp:
idx = np.einsum('il,ljk->ijk', R, PP) + center.reshape(3, 1, 1)
slice = map_coordinates(cube, idx, order=1, mode='constant', cval=fill)
else:
idx = np.einsum('il,ljk->ijk', R, PP) + (0.5 + center.reshape(3, 1, 1))
idx = idx.astype(np.int16)
# Find out which coordinates are out of range - shape (mX, mY)
badpoints = np.any([
idx[0, :, :] < 0,
idx[0, :, :] >= cube.shape[0],
idx[1, :, :] < 0,
idx[1, :, :] >= cube.shape[1],
idx[2, :, :] < 0,
idx[2, :, :] >= cube.shape[2],
], axis=0)
idx[:, badpoints] = 0
slice = cube[idx[0], idx[1], idx[2]]
slice[badpoints] = fill
return slice
# Demonstration
nx, ny, nz = 50, 70, 100
cube = np.full((nx, ny, nz), np.float32(1))
cube[nx//4:nx*3//4, :, :] += 1
cube[:, ny//2:ny*3//4, :] += 3
cube[:, :, nz//4:nz//2] += 7
cube[nx//3-2:nx//3+2, ny//2-2:ny//2+2, :] = 0 # black dot
Rz, Rx = np.pi/6, np.pi/4 # rotation angles around z and x
cz, sz = np.cos(Rz), np.sin(Rz)
cx, sx = np.cos(Rx), np.sin(Rx)
Rmz = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]])
Rmx = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]])
eXY = (Rmx # Rmz).T[:2]
slice = slice_datacube(
cube,
center=[nx/3, ny/2, nz*0.7],
eXY=eXY,
mXY=[80, 90],
fill=np.nan,
interp=False
)
import matplotlib.pyplot as plt
plt.close('all')
plt.imshow(slice.T) # imshow expects shape (mY, mX)
plt.colorbar()
Output (for interp=False):
For this test case (50x70x100 datacube, 80x90 slice size) the run time is 376 µs (interp=False) and 550 µs (interp=True) on my laptop.