Related
I display a gyroid structure (TPMS) in a cartesian system using Pyvista. I try now to display the structure in cylindrical coordinates. Pyvista displays something cylindrical indeed but it seems that the unit cell length is not uniform (while there is no reason to change this my parameter "a" being steady. This change seems to appear especially along z but I don't understand why (see image).
I have this:
Here is a part of my code.
Thank you for your help.
import pyvista as pv
import numpy as np
from numpy import cos, sin, pi
from random import uniform
lattice_par = 1.0 # Unit cell length
a = (2*pi)/lattice_par
res = 200j
r, theta, z = np.mgrid[0:2:res, 0:2*pi:res, 0:4:res]
# consider using non-equidistant r for uniformity
def GyroidCyl(r, theta, z, b=0.8):
return (sin(a*(r*cos(theta) - 1))*cos(a*(r*sin(theta) - 1))
+ sin(a*(r*sin(theta) - 1))*cos(a*(z - 1))
+ sin(a*(z - 1))*cos(a*(r*cos(theta) - 1))
- b)
vol3 = GyroidCyl(r, theta, z)
# compute Cartesian coordinates for grid points
x = r * cos(theta)
y = r * sin(theta)
grid = pv.StructuredGrid(x, y, z)
grid["vol3"] = vol3.flatten()
contours3 = grid.contour([0]) # Isosurface = 0
pv.set_plot_theme('document')
p = pv.Plotter()
p.add_mesh(contours3, scalars=contours3.points[:, 2], show_scalar_bar=False, interpolate_before_map=True,
show_edges=False, smooth_shading=False, render=True)
p.show_axes_all()
p.add_floor()
p.show_grid()
p.add_title('Gyroid in cylindrical coordinates')
p.add_text('Volume Fraction Parameter = ' + str(b))
p.show(window_size=[2040, 1500])
So you've noted in comments that you're trying to replicate something like the strategy explained in this paper. What they do is take a regular gyroid unit cell, and then transform it to build a cylindrical shell. If igloos were cylindrical, then a gyroid cell would be a single piece of snow brick. Put them next to one another and stack them in a column, and you get a cylinder.
Since I can't use figures from the paper we'll have to recreate some ourselves. So you have to start from a regular gyroid defined by the implicit function
cos(x) sin(y) + cos(y) sin(z) + cos(z) sin(x) = 0
(or some variation thereof). Here's how a single unit cell looks:
import pyvista as pv
import numpy as np
res = 100j
a = 2*np.pi
x, y, z = np.mgrid[0:a:res, 0:a:res, 0:a:res]
def Gyroid(x, y, z):
return np.cos(x)*np.sin(y) + np.cos(y)*np.sin(z) + np.cos(z)*np.sin(x)
# compute implicit function
fun_values = Gyroid(x, y, z)
# create grid for contouring
grid = pv.StructuredGrid(x, y, z)
grid["vol3"] = fun_values.ravel('F')
contours3 = grid.contour([0]) # isosurface for 0
# plot the contour, i.e. the gyroid
pv.set_plot_theme('document')
plotter = pv.Plotter()
plotter.add_mesh(contours3, scalars=contours3.points[:, -1],
show_scalar_bar=False)
plotter.add_bounding_box()
plotter.enable_terrain_style()
plotter.show_axes()
plotter.show()
Using the "unit cell" term implies there's an underlying infinite lattice, which can be built by stacking these (rectangular) unit cells neatly next to one another. With some imagination we can convince ourselves that this is true. Or we can look at the formula and note that due to the trigonometric functions the function is periodic in x, y and z, with period 2*pi. This also tells us that we can transform the unit cell to have arbitrary rectangular dimensions by introducing lattice parameters a, b and c:
cos(kx x) sin(ky y) + cos(ky y) sin(kz z) + cos(kz z) sin(kx x) = 0, where
kx = 2 pi/a
ky = 2 pi/b
kz = 2 pi/c
(These kx, ky and kz quantities are called wave vectors in solid state physics.)
The corresponding change only affects the header:
res = 100j
a, b, c = lattice_params = 1, 2, 3
kx, ky, kz = [2*np.pi/lattice_param for lattice_param in lattice_params]
x, y, z = np.mgrid[0:a:res, 0:b:res, 0:c:res]
def Gyroid(x, y, z):
return ( np.cos(kx*x)*np.sin(ky*y)
+ np.cos(ky*y)*np.sin(kz*z)
+ np.cos(kz*z)*np.sin(kx*x))
This is where we start. What we have to do is take this unit cell, bend it so that it corresponds to a 30-degree circular arc on a cylinder, and stack the cylinder using this unit. According to the paper, they used 12 unit cells to create a circle in a plane (hence the 30-degree magic number), and stacked three such circular bands on top of each other to build the cylinder.
The actual mapping is also fairly clearly explained in the paper. Whereas your original x, y and z parameters of the function essentially interpolated between [0, a], [0, b] and [0, c], respectively, in the new setup x interpolates in the radius range [r1, r2], y interpolates in the angular range [0, pi/6] and z is just z. (In the paper x and y seem to be reversed with respect to this convention, but this shouldn't matter. If it matters, that's left as an exercise to the reader.)
So what we need to do is more or less keep the current grid points, but transform the corresponding x, y and z grid points so that they lie on a cylinder instead. Here's one take:
import pyvista as pv
import numpy as np
res = 100j
a, b, c = lattice_params = 1, 1, 1
kx, ky, kz = [2*np.pi/lattice_param for lattice_param in lattice_params]
r_aux, phi, z = np.mgrid[0:a:res, 0:b:res, 0:3*c:res]
# convert r_aux range to actual radii
r1, r2 = 1.5, 2
r = r2/a*r_aux + r1/a*(1 - r_aux)
def Gyroid(x, y, z):
return ( np.cos(kx*x)*np.sin(ky*y)
+ np.cos(ky*y)*np.sin(kz*z)
+ np.cos(kz*z)*np.sin(kx*x))
# compute data for cylindrical gyroid
# r_aux is x, phi / 12 is y and z is z
fun_values = Gyroid(r_aux, phi * 12, z)
# compute Cartesian coordinates for grid points
x = r * np.cos(phi*ky)
y = r * np.sin(phi*ky)
grid = pv.StructuredGrid(x, y, z)
grid["vol3"] = fun_values.ravel('F')
contours3 = grid.contour([0])
# plot cylindrical gyroid
pv.set_plot_theme('document')
plotter = pv.Plotter()
plotter.add_mesh(contours3, scalars=contours3.points[:, -1],
show_scalar_bar=False)
plotter.add_bounding_box()
plotter.show_axes()
plotter.enable_terrain_style()
plotter.show()
If you want to look at a single transformed unit cell in the cylindrical setting, use a single domain of phi and z for the function and only convert to 1/12 a full circle for the grid points:
fun_values = Gyroid(r_aux, phi, z/3)
# compute Cartesian coordinates for grid points
x = r * np.cos(phi*ky/12)
y = r * np.sin(phi*ky/12)
grid = pv.StructuredGrid(x, y, z/3)
But it's not easy to see the curvature in the (no longer a) unit cell:
I have written code that calculates the angle between two vectors. However the way in which is does this is to start with two vectors, rotate each according to some euler angles calculated in a separate program, then calculate the angle between the vectors.
Up until now I have been working with a use case that means both starting vectors are (0,0,1) that makes life super easy. I could just take one set of euler angles away from the other and then calculate the angle between 0,0,1 and the vector that had been rotated by the difference. It meant I could plot nice distribution plots and vector diagrams because everything was normalised to 0,0,1. (I have 1000s of these vectors for the record).
No I am trying to write in a function that would allow for a use case where the two starting vectors are not on 0,0,1. I figured the easiest way to do this would be to calculate direction of the vector relative to 0,0,1 and after calculating the position of the vector just rotate by the precalculated offsets. (this might be a stupid way to do it, if it is please tell me).
MY current code works for a case where a vector is 0,1,0 but then breaks down if i start entering random numbers.
import numpy as np
import math
def RotationMatrix(axis, rotang):
"""
This uses Euler-Rodrigues formula.
"""
#Input taken in degrees, here we change it to radians
theta = rotang * 0.0174532925
axis = np.asarray(axis)
#Ensure axis is a unit vector
axis = axis/math.sqrt(np.dot(axis, axis))
#calclating a, b, c and d according to euler-rodrigues forumla requirments
a = math.cos(theta/2)
b, c, d = axis*math.sin(theta/2)
a2, b2, c2, d2 = a*a, b*b, c*c, d*d
bc, ad, ac, ab, bd, cd = b*c, a*d, a*c, a*b, b*d, c*d
#Return the rotation matrix
return np.array([[a2+b2-c2-d2, 2*(bc-ad), 2*(bd+ac)],
[2*(bc+ad), a2+c2-b2-d2, 2*(cd-ab)],
[2*(bd-ac), 2*(cd+ab), a2+d2-b2-c2]])
def ApplyRotationMatrix(vector, rotationmatrix):
"""
This function take the output from the RotationMatrix function and
uses that to apply the rotation to an input vector
"""
a1 = (vector[0] * rotationmatrix[0, 0]) + (vector[1] * rotationmatrix[0, 1]) + (vector[2] * rotationmatrix[0, 2])
b1 = (vector[0] * rotationmatrix[1, 0]) + (vector[1] * rotationmatrix[1, 1]) + (vector[2] * rotationmatrix[1, 2])
c1 = (vector[0] * rotationmatrix[2, 0]) + (vector[1] * rotationmatrix[2, 1]) + (vector[2] * rotationmatrix[2, 2])
return np.array((a1, b1, c1)
'''
Functions for Calculating the angles of 3D vectors relative to one another
'''
def CalculateAngleBetweenVector(vector, vector2):
"""
Does what it says on the tin, outputs an angle in degrees between two input vectors.
"""
dp = np.dot(vector, vector2)
maga = math.sqrt((vector[0] ** 2) + (vector[1] ** 2) + (vector[2] ** 2))
magb = math.sqrt((vector2[0] ** 2) + (vector2[1] ** 2) + (vector2[2] ** 2))
magc = maga * magb
dpmag = dp / magc
#These if statements deal with rounding errors of floating point operations
if dpmag > 1:
error = dpmag - 1
print('error = {}, do not worry if this number is very small'.format(error))
dpmag = 1
elif dpmag < -1:
error = 1 + dpmag
print('error = {}, do not worry if this number is very small'.format(error))
dpmag = -1
angleindeg = ((math.acos(dpmag)) * 180) / math.pi
return angleindeg
def CalculateAngleAroundZ(Vector):
X,Y,Z = Vector[0], Vector[1], Vector[2]
AngleAroundZ = math.atan2(Y, X)
AngleAroundZdeg = (AngleAroundZ*180)/math.pi
return AngleAroundZdeg
def CalculateAngleAroundX(Vector):
X,Y,Z = Vector[0], Vector[1], Vector[2]
AngleAroundZ = math.atan2(Y, Z)
AngleAroundZdeg = (AngleAroundZ*180)/math.pi
return AngleAroundZdeg
def CalculateAngleAroundY(Vector):
X,Y,Z = Vector[0], Vector[1], Vector[2]
AngleAroundZ = math.atan2(X, Z)
AngleAroundZdeg = (AngleAroundZ*180)/math.pi
return AngleAroundZdeg
V1 = (0,0,1)
V2 = (3,5,4)
Xoffset = (CalculateAngleAroundX(V2))
Yoffset = (CalculateAngleAroundY(V2))
Zoffset = (CalculateAngleAroundZ(V2))
XRM = RotationMatrix((1,0,0), (Xoffset * 1))
YRM = RotationMatrix((0,1,0), (Yoffset * 1))
ZRM = RotationMatrix((0,0,1), (Zoffset * 1))
V2 = V2 / np.linalg.norm(V2)
V2X = ApplyRotationMatrix(V2, XRM)
V2XY = ApplyRotationMatrix(V2X, YRM)
V2XYZ = ApplyRotationMatrix(V2XY, ZRM)
print(V2XYZ)
print(CalculateAngleBetweenVector(V1, V2XYZ))
Any advice to fix this problem will be much appreciated.
I'm not sure to fully understand what you need but if it is to compute the angle between two vectors in space you can use the formula:
where a.b is the scalar product and theta is the angle between vectors.
thus your function CalculateAngleBetweenVector becomes:
def CalculateAngleBetweenVector(vector, vector2):
return math.acos(np.dot(vector,vector2)/(np.linalg.norm(vector)* np.linalg.norm(vector2))) * 180 /math.pi
You can also simplify your ApplyRotationMatrix function:
def ApplyRotationMatrix(vector, rotationmatrix):
"""
This function take the output from the RotationMatrix function and
uses that to apply the rotation to an input vector
"""
return rotationmatrix # vector
the # symbol is the matrix product
Hope this will help you. Feel free to precise your request if this is not helpfull.
Im an idiot I just needed to do the cross product and the dot product and rotate by the dot product *-1 around the cross product.
I'm looking for a way to analyze two cubic splines and find the point where they come the closest to each other. I've seen a lot of solutions and posts but I've been unable to implement the methods suggested. I know that the closest point will be one of the end-points of the two curves or a point where the first derivative of both curves is equal. Checking the end points is easy. Finding the points where the first derivatives match is hard.
Given:
Curve 0 is B(t) (red)
Curve 1 is C(s) (blue)
A candidate for closest point is where:
B'(t) = C'(s)
The first derivative of each curve takes the following form:
Where the a, b, c coefficients are formed from the control points of the curves:
a=P1-P0
b=P2-P1
c=P3-P2
Taking the 4 control points for each cubic spline I can get each curve's parametric sections into a matrix form that can be expressed with Numpy with the following Python code:
def test_closest_points():
# Control Points for the two qubic splines.
spline_0 = [(1,28), (58,93), (113,95), (239,32)]
spline_1 = [(58, 241), (26,76), (225,83), (211,205)]
first_derivative_matrix = np.array([[3, -6, 3], [-6, 6, 0], [3, 0, 0]])
spline_0_x_A = spline_0[1][0] - spline_0[0][0]
spline_0_x_B = spline_0[2][0] - spline_0[1][0]
spline_0_x_C = spline_0[3][0] - spline_0[2][0]
spline_0_y_A = spline_0[1][1] - spline_0[0][1]
spline_0_y_B = spline_0[2][1] - spline_0[1][1]
spline_0_y_C = spline_0[3][1] - spline_0[2][1]
spline_1_x_A = spline_1[1][0] - spline_1[0][0]
spline_1_x_B = spline_1[2][0] - spline_1[1][0]
spline_1_x_C = spline_1[3][0] - spline_1[2][0]
spline_1_y_A = spline_1[1][1] - spline_1[0][1]
spline_1_y_B = spline_1[2][1] - spline_1[1][1]
spline_1_y_C = spline_1[3][1] - spline_1[2][1]
spline_0_first_derivative_x_coefficients = np.array([[spline_0_x_A], [spline_0_x_B], [spline_0_x_C]])
spline_0_first_derivative_y_coefficients = np.array([[spline_0_y_A], [spline_0_y_B], [spline_0_y_C]])
spline_1_first_derivative_x_coefficients = np.array([[spline_1_x_A], [spline_1_x_B], [spline_1_x_C]])
spline_1_first_derivative_y_coefficients = np.array([[spline_1_y_A], [spline_1_y_B], [spline_1_y_C]])
# Show All te matrix values
print 'first_derivative_matrix:'
print first_derivative_matrix
print
print 'spline_0_first_derivative_x_coefficients:'
print spline_0_first_derivative_x_coefficients
print
print 'spline_0_first_derivative_y_coefficients:'
print spline_0_first_derivative_y_coefficients
print
print 'spline_1_first_derivative_x_coefficients:'
print spline_1_first_derivative_x_coefficients
print
print 'spline_1_first_derivative_y_coefficients:'
print spline_1_first_derivative_y_coefficients
print
# Now taking B(t) as spline_0 and C(s) as spline_1, I need to find the values of t and s where B'(t) = C'(s)
This post has some good high-level advice but I'm unsure how to implement a solution in python that can find the correct values for t and s that have matching first derivatives (slopes). The B'(t) - C'(s) = 0 problem seems like a matter of finding roots. Any advice on how to do it with python and Numpy would be greatly appreciated.
Using Numpy assumes that the problem should be solved numerically. Without loss of generality we can treat that 0<s<=1 and 0<t<=1. You can use SciPy package to solve the problem numerically, e.g.
from scipy.optimize import minimize
import numpy as np
def B(t):
"""Assumed for simplicity: 0 < t <= 1
"""
return np.sin(6.28 * t), np.cos(6.28 * t)
def C(s):
"""0 < s <= 1
"""
return 10 + np.sin(3.14 * s), 10 + np.cos(3.14 * s)
def Q(x):
"""Distance function to be minimized
"""
b = B(x[0])
c = C(x[1])
return (b[0] - c[0]) ** 2 + (b[1] - c[1]) ** 2
res = minimize(Q, (0.5, 0.5))
print("B-Point: ", B(res.x[0]))
print("C-Point: ", C(res.x[1]))
B-Point: (0.7071067518175205, 0.7071068105555733)
C-Point: (9.292893243165555, 9.29289319446135)
This is example for two circles (one circle and arc). This will likely work with splines.
Your assumption of B'(t) = C'(s) is too strong.
Derivatives have direction and magnitude. Directions must coincide in the candidate points, but magnitudes might differ.
To find points with the same derivative slopes and the closest distance you can solve equation system (of course, high power :( )
yb'(t) * xc'(u) - yc'(t) * xb'(u) = 0 //vector product of (anti)collinear vectors is zero
((xb(t) - xc(u))^2 + (xb(t) - xc(u))^2)' = 0 //distance derivative
You can use the function fmin also:
import numpy as np
import matplotlib.pylab as plt
from scipy.optimize import fmin
def BCubic(t, P0, P1, P2, P3):
a=P1-P0
b=P2-P1
c=P3-P2
return a*3*(1-t)**2 + b*6*(1-t)*t + c*3*t**2
def B(t):
return BCubic(t,4,2,3,1)
def C(t):
return BCubic(t,1,4,3,4)
def f(t):
# L1 or manhattan distance
return abs(B(t) - C(t))
init = 0 # 2
tmin = fmin(f,np.array([init]))
#Optimization terminated successfully.
#Current function value: 2.750000
# Iterations: 23
# Function evaluations: 46
print(tmin)
# [0.5833125]
tmin = tmin[0]
t = np.linspace(0, 2, 100)
plt.plot(t, B(t), label='B')
plt.plot(t, C(t), label='C')
plt.plot(t, abs(B(t)-C(t)), label='|B-C|')
plt.plot(tmin, B(tmin), 'r.', markersize=12, label='min')
plt.axvline(x=tmin, linestyle='--', color='k')
plt.legend()
plt.show()
I have 4 points, which are very near to be at the one plane - it is the 1,4-Dihydropyridine cycle.
I need to calculate distance from C3 and N1 to the plane, which is made of C1-C2-C4-C5.
Calculating distance is OK, but fitting plane is quite difficult to me.
1,4-DHP cycle:
1,4-DHP cycle, another view:
from array import *
from numpy import *
from scipy import *
# coordinates (XYZ) of C1, C2, C4 and C5
x = [0.274791784, -1.001679346, -1.851320839, 0.365840754]
y = [-1.155674199, -1.215133985, 0.053119249, 1.162878076]
z = [1.216239624, 0.764265677, 0.956099579, 1.198231236]
# plane equation Ax + By + Cz = D
# non-fitted plane
abcd = [0.506645455682, -0.185724560275, -1.43998120646, 1.37626378129]
# creating distance variable
distance = zeros(4, float)
# calculating distance from point to plane
for i in range(4):
distance[i] = (x[i]*abcd[0]+y[i]*abcd[1]+z[i]*abcd[2]+abcd[3])/sqrt(abcd[0]**2 + abcd[1]**2 + abcd[2]**2)
print distance
# calculating squares
squares = distance**2
print squares
How to make sum(squares) minimized? I have tried least squares, but it is too hard for me.
That sounds about right, but you should replace the nonlinear optimization with an SVD. The following creates the moment of inertia tensor, M, and then SVD's it to get the normal to the plane. This should be a close approximation to the least-squares fit and be much faster and more predictable. It returns the point-cloud center and the normal.
def planeFit(points):
"""
p, n = planeFit(points)
Given an array, points, of shape (d,...)
representing points in d-dimensional space,
fit an d-dimensional plane to the points.
Return a point, p, on the plane (the point-cloud centroid),
and the normal, n.
"""
import numpy as np
from numpy.linalg import svd
points = np.reshape(points, (np.shape(points)[0], -1)) # Collapse trialing dimensions
assert points.shape[0] <= points.shape[1], "There are only {} points in {} dimensions.".format(points.shape[1], points.shape[0])
ctr = points.mean(axis=1)
x = points - ctr[:,np.newaxis]
M = np.dot(x, x.T) # Could also use np.cov(x) here.
return ctr, svd(M)[0][:,-1]
For example: Construct a 2D cloud at (10, 100) that is thin in the x direction and 100 times bigger in the y direction:
>>> pts = np.diag((.1, 10)).dot(randn(2,1000)) + np.reshape((10, 100),(2,-1))
The fit plane is very nearly at (10, 100) with a normal very nearly along the x axis.
>>> planeFit(pts)
(array([ 10.00382471, 99.48404676]),
array([ 9.99999881e-01, 4.88824145e-04]))
Least squares should fit a plane easily. The equation for a plane is: ax + by + c = z. So set up matrices like this with all your data:
x_0 y_0 1
A = x_1 y_1 1
...
x_n y_n 1
And
a
x = b
c
And
z_0
B = z_1
...
z_n
In other words: Ax = B. Now solve for x which are your coefficients. But since you have more than 3 points, the system is over-determined so you need to use the left pseudo inverse. So the answer is:
a
b = (A^T A)^-1 A^T B
c
And here is some simple Python code with an example:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
N_POINTS = 10
TARGET_X_SLOPE = 2
TARGET_y_SLOPE = 3
TARGET_OFFSET = 5
EXTENTS = 5
NOISE = 5
# create random data
xs = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
ys = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
zs = []
for i in range(N_POINTS):
zs.append(xs[i]*TARGET_X_SLOPE + \
ys[i]*TARGET_y_SLOPE + \
TARGET_OFFSET + np.random.normal(scale=NOISE))
# plot raw data
plt.figure()
ax = plt.subplot(111, projection='3d')
ax.scatter(xs, ys, zs, color='b')
# do fit
tmp_A = []
tmp_b = []
for i in range(len(xs)):
tmp_A.append([xs[i], ys[i], 1])
tmp_b.append(zs[i])
b = np.matrix(tmp_b).T
A = np.matrix(tmp_A)
fit = (A.T * A).I * A.T * b
errors = b - A * fit
residual = np.linalg.norm(errors)
print("solution: %f x + %f y + %f = z" % (fit[0], fit[1], fit[2]))
print("errors:")
print(errors)
print("residual: {}".format(residual))
# plot plane
xlim = ax.get_xlim()
ylim = ax.get_ylim()
X,Y = np.meshgrid(np.arange(xlim[0], xlim[1]),
np.arange(ylim[0], ylim[1]))
Z = np.zeros(X.shape)
for r in range(X.shape[0]):
for c in range(X.shape[1]):
Z[r,c] = fit[0] * X[r,c] + fit[1] * Y[r,c] + fit[2]
ax.plot_wireframe(X,Y,Z, color='k')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
The solution for your points:
0.143509 x + 0.057196 y + 1.129595 = z
The fact that you are fitting to a plane is only slightly relevant here. What you are trying to do is minimize a particular function starting from a guess. For that use scipy.optimize. Note that there is no guarantee that this is the globally optimal solution, only locally optimal. A different initial condition may converge to a different result, this works well if you start close to the local minima you are seeking.
I've taken the liberty to clean up your code by taking advantage of numpy's broadcasting:
import numpy as np
# coordinates (XYZ) of C1, C2, C4 and C5
XYZ = np.array([
[0.274791784, -1.001679346, -1.851320839, 0.365840754],
[-1.155674199, -1.215133985, 0.053119249, 1.162878076],
[1.216239624, 0.764265677, 0.956099579, 1.198231236]])
# Inital guess of the plane
p0 = [0.506645455682, -0.185724560275, -1.43998120646, 1.37626378129]
def f_min(X,p):
plane_xyz = p[0:3]
distance = (plane_xyz*X.T).sum(axis=1) + p[3]
return distance / np.linalg.norm(plane_xyz)
def residuals(params, signal, X):
return f_min(X, params)
from scipy.optimize import leastsq
sol = leastsq(residuals, p0, args=(None, XYZ))[0]
print("Solution: ", sol)
print("Old Error: ", (f_min(XYZ, p0)**2).sum())
print("New Error: ", (f_min(XYZ, sol)**2).sum())
This gives:
Solution: [ 14.74286241 5.84070802 -101.4155017 114.6745077 ]
Old Error: 0.441513295404
New Error: 0.0453564286112
This returns the 3D plane coefficients along with the RMSE of the fit.
The plane is provided in a homogeneous coordinate representation, meaning its dot product with the homogeneous coordinates of a point produces the distance between the two.
def fit_plane(points):
assert points.shape[1] == 3
centroid = points.mean(axis=0)
x = points - centroid[None, :]
U, S, Vt = np.linalg.svd(x.T # x)
normal = U[:, -1]
origin_distance = normal # centroid
rmse = np.sqrt(S[-1] / len(points))
return np.hstack([normal, -origin_distance]), rmse
Minor note: the SVD can also be directly applied to the points instead of the outer product matrix, but I found it to be slower with NumPy's SVD implementation.
U, S, Vt = np.linalg.svd(x.T, full_matrices=False)
rmse = S[-1] / np.sqrt(len(points))
Another way aside from svd to quickly reach a solution while dealing with outliers ( when you have a large data set ) is ransac :
def fit_plane(voxels, iterations=50, inlier_thresh=10): # voxels : x,y,z
inliers, planes = [], []
xy1 = np.concatenate([voxels[:, :-1], np.ones((voxels.shape[0], 1))], axis=1)
z = voxels[:, -1].reshape(-1, 1)
for _ in range(iterations):
random_pts = voxels[np.random.choice(voxels.shape[0], voxels.shape[1] * 10, replace=False), :]
plane_transformation, residual = fit_pts_to_plane(random_pts)
inliers.append(((z - np.matmul(xy1, plane_transformation)) <= inlier_thresh).sum())
planes.append(plane_transformation)
return planes[np.array(inliers).argmax()]
def fit_pts_to_plane(voxels): # x y z (m x 3)
# https://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points
xy1 = np.concatenate([voxels[:, :-1], np.ones((voxels.shape[0], 1))], axis=1)
z = voxels[:, -1].reshape(-1, 1)
fit = np.matmul(np.matmul(np.linalg.inv(np.matmul(xy1.T, xy1)), xy1.T), z)
errors = z - np.matmul(xy1, fit)
residual = np.linalg.norm(errors)
return fit, residual
Here's one way. If your points are P[1]..P[n] then compute the mean M of these and subtract it from each, getting points p[1]..p[n]. Then compute C = Sum{ p[i]*p[i]'} (the "covariance" matrix of the points). Next diagonalise C, that is find orthogonal U and diagonal E so that C = U*E*U'. If your points are indeed on a plane then one of the eigenvalues (ie the diagonal entries of E) will be very small (with perfect arithmetic it would be 0). In any case if the j'th one of these is the smallest, then let the j'th column of U be (A,B,C) and compute D = -M'*N. These parameters define the "best" plane, the one such that the sum of the squares of the distances from the P[] to the plane is least.
I have two vectors as Python lists and an angle. E.g.:
v = [3,5,0]
axis = [4,4,1]
theta = 1.2 #radian
What is the best/easiest way to get the resulting vector when rotating the v vector around the axis?
The rotation should appear to be counter clockwise for an observer to whom the axis vector is pointing. This is called the right hand rule
Using the Euler-Rodrigues formula:
import numpy as np
import math
def rotation_matrix(axis, theta):
"""
Return the rotation matrix associated with counterclockwise rotation about
the given axis by theta radians.
"""
axis = np.asarray(axis)
axis = axis / math.sqrt(np.dot(axis, axis))
a = math.cos(theta / 2.0)
b, c, d = -axis * math.sin(theta / 2.0)
aa, bb, cc, dd = a * a, b * b, c * c, d * d
bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
return np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)],
[2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)],
[2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]])
v = [3, 5, 0]
axis = [4, 4, 1]
theta = 1.2
print(np.dot(rotation_matrix(axis, theta), v))
# [ 2.74911638 4.77180932 1.91629719]
A one-liner, with numpy/scipy functions.
We use the following:
let a be the unit vector along axis, i.e. a = axis/norm(axis)
and A = I × a be the skew-symmetric matrix associated to a, i.e. the cross product of the identity matrix with a
then M = exp(θ A) is the rotation matrix.
from numpy import cross, eye, dot
from scipy.linalg import expm, norm
def M(axis, theta):
return expm(cross(eye(3), axis/norm(axis)*theta))
v, axis, theta = [3,5,0], [4,4,1], 1.2
M0 = M(axis, theta)
print(dot(M0,v))
# [ 2.74911638 4.77180932 1.91629719]
expm (code here) computes the taylor series of the exponential:
\sum_{k=0}^{20} \frac{1}{k!} (θ A)^k
, so it's time expensive, but readable and secure.
It can be a good way if you have few rotations to do but a lot of vectors.
I just wanted to mention that if speed is required, wrapping unutbu's code in scipy's weave.inline and passing an already existing matrix as a parameter yields a 20-fold decrease in the running time.
The code (in rotation_matrix_test.py):
import numpy as np
import timeit
from math import cos, sin, sqrt
import numpy.random as nr
from scipy import weave
def rotation_matrix_weave(axis, theta, mat = None):
if mat == None:
mat = np.eye(3,3)
support = "#include <math.h>"
code = """
double x = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
double a = cos(theta / 2.0);
double b = -(axis[0] / x) * sin(theta / 2.0);
double c = -(axis[1] / x) * sin(theta / 2.0);
double d = -(axis[2] / x) * sin(theta / 2.0);
mat[0] = a*a + b*b - c*c - d*d;
mat[1] = 2 * (b*c - a*d);
mat[2] = 2 * (b*d + a*c);
mat[3*1 + 0] = 2*(b*c+a*d);
mat[3*1 + 1] = a*a+c*c-b*b-d*d;
mat[3*1 + 2] = 2*(c*d-a*b);
mat[3*2 + 0] = 2*(b*d-a*c);
mat[3*2 + 1] = 2*(c*d+a*b);
mat[3*2 + 2] = a*a+d*d-b*b-c*c;
"""
weave.inline(code, ['axis', 'theta', 'mat'], support_code = support, libraries = ['m'])
return mat
def rotation_matrix_numpy(axis, theta):
mat = np.eye(3,3)
axis = axis/sqrt(np.dot(axis, axis))
a = cos(theta/2.)
b, c, d = -axis*sin(theta/2.)
return np.array([[a*a+b*b-c*c-d*d, 2*(b*c-a*d), 2*(b*d+a*c)],
[2*(b*c+a*d), a*a+c*c-b*b-d*d, 2*(c*d-a*b)],
[2*(b*d-a*c), 2*(c*d+a*b), a*a+d*d-b*b-c*c]])
The timing:
>>> import timeit
>>>
>>> setup = """
... import numpy as np
... import numpy.random as nr
...
... from rotation_matrix_test import rotation_matrix_weave
... from rotation_matrix_test import rotation_matrix_numpy
...
... mat1 = np.eye(3,3)
... theta = nr.random()
... axis = nr.random(3)
... """
>>>
>>> timeit.repeat("rotation_matrix_weave(axis, theta, mat1)", setup=setup, number=100000)
[0.36641597747802734, 0.34883809089660645, 0.3459300994873047]
>>> timeit.repeat("rotation_matrix_numpy(axis, theta)", setup=setup, number=100000)
[7.180983066558838, 7.172032117843628, 7.180462837219238]
Here is an elegant method using quaternions that are blazingly fast; I can calculate 10 million rotations per second with appropriately vectorised numpy arrays. It relies on the quaternion extension to numpy found here.
Quaternion Theory:
A quaternion is a number with one real and 3 imaginary dimensions usually written as q = w + xi + yj + zk where 'i', 'j', 'k' are imaginary dimensions. Just as a unit complex number 'c' can represent all 2d rotations by c=exp(i * theta), a unit quaternion 'q' can represent all 3d rotations by q=exp(p), where 'p' is a pure imaginary quaternion set by your axis and angle.
We start by converting your axis and angle to a quaternion whose imaginary dimensions are given by your axis of rotation, and whose magnitude is given by half the angle of rotation in radians. The 4 element vectors (w, x, y, z) are constructed as follows:
import numpy as np
import quaternion as quat
v = [3,5,0]
axis = [4,4,1]
theta = 1.2 #radian
vector = np.array([0.] + v)
rot_axis = np.array([0.] + axis)
axis_angle = (theta*0.5) * rot_axis/np.linalg.norm(rot_axis)
First, a numpy array of 4 elements is constructed with the real component w=0 for both the vector to be rotated vector and the rotation axis rot_axis. The axis angle representation is then constructed by normalizing then multiplying by half the desired angle theta. See here for why half the angle is required.
Now create the quaternions v and qlog using the library, and get the unit rotation quaternion q by taking the exponential.
vec = quat.quaternion(*v)
qlog = quat.quaternion(*axis_angle)
q = np.exp(qlog)
Finally, the rotation of the vector is calculated by the following operation.
v_prime = q * vec * np.conjugate(q)
print(v_prime) # quaternion(0.0, 2.7491163, 4.7718093, 1.9162971)
Now just discard the real element and you have your rotated vector!
v_prime_vec = v_prime.imag # [2.74911638 4.77180932 1.91629719] as a numpy array
Note that this method is particularly efficient if you have to rotate a vector through many sequential rotations, as the quaternion product can just be calculated as q = q1 * q2 * q3 * q4 * ... * qn and then the vector is only rotated by 'q' at the very end using v' = q * v * conj(q).
This method gives you a seamless transformation between axis angle <---> 3d rotation operator simply by exp and log functions (yes log(q) just returns the axis-angle representation!). For further clarification of how quaternion multiplication etc. work, see here
Take a look at http://vpython.org/contents/docs/visual/VisualIntro.html.
It provides a vector class which has a method A.rotate(theta,B). It also provides a helper function rotate(A,theta,B) if you don't want to call the method on A.
http://vpython.org/contents/docs/visual/vector.html
Use scipy's Rotation.from_rotvec(). The argument is the rotation vector (a unit vector) multiplied by the rotation angle in rads.
from scipy.spatial.transform import Rotation
from numpy.linalg import norm
v = [3, 5, 0]
axis = [4, 4, 1]
theta = 1.2
axis = axis / norm(axis) # normalize the rotation vector first
rot = Rotation.from_rotvec(theta * axis)
new_v = rot.apply(v)
print(new_v) # results in [2.74911638 4.77180932 1.91629719]
There are several more ways to use Rotation based on what data you have about the rotation:
from_quat Initialized from quaternions.
from_dcm Initialized from direction cosine matrices.
from_euler Initialized from Euler angles.
Off-topic note: One line code is not necessarily better code as implied by some users.
I made a fairly complete library of 3D mathematics for Python{2,3}. It still does not use Cython, but relies heavily on the efficiency of numpy. You can find it here with pip:
python[3] -m pip install math3d
Or have a look at my gitweb http://git.automatics.dyndns.dk/?p=pymath3d.git and now also on github: https://github.com/mortlind/pymath3d .
Once installed, in python you may create the orientation object which can rotate vectors, or be part of transform objects. E.g. the following code snippet composes an orientation that represents a rotation of 1 rad around the axis [1,2,3], applies it to the vector [4,5,6], and prints the result:
import math3d as m3d
r = m3d.Orientation.new_axis_angle([1,2,3], 1)
v = m3d.Vector(4,5,6)
print(r * v)
The output would be
<Vector: (2.53727, 6.15234, 5.71935)>
This is more efficient, by a factor of approximately four, as far as I can time it, than the oneliner using scipy posted by B. M. above. However, it requires installation of my math3d package.
It can also be solved using quaternion theory:
def angle_axis_quat(theta, axis):
"""
Given an angle and an axis, it returns a quaternion.
"""
axis = np.array(axis) / np.linalg.norm(axis)
return np.append([np.cos(theta/2)],np.sin(theta/2) * axis)
def mult_quat(q1, q2):
"""
Quaternion multiplication.
"""
q3 = np.copy(q1)
q3[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3]
q3[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2]
q3[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1]
q3[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0]
return q3
def rotate_quat(quat, vect):
"""
Rotate a vector with the rotation defined by a quaternion.
"""
# Transfrom vect into an quaternion
vect = np.append([0],vect)
# Normalize it
norm_vect = np.linalg.norm(vect)
vect = vect/norm_vect
# Computes the conjugate of quat
quat_ = np.append(quat[0],-quat[1:])
# The result is given by: quat * vect * quat_
res = mult_quat(quat, mult_quat(vect,quat_)) * norm_vect
return res[1:]
v = [3, 5, 0]
axis = [4, 4, 1]
theta = 1.2
print(rotate_quat(angle_axis_quat(theta, axis), v))
# [2.74911638 4.77180932 1.91629719]
Disclaimer: I am the author of this package
While special classes for rotations can be convenient, in some cases one needs rotation matrices (e.g. for working with other libraries like the affine_transform functions in scipy). To avoid everyone implementing their own little matrix generating functions, there exists a tiny pure python package which does nothing more than providing convenient rotation matrix generating functions. The package is on github (mgen) and can be installed via pip:
pip install mgen
Example usage copied from the readme:
import numpy as np
np.set_printoptions(suppress=True)
from mgen import rotation_around_axis
from mgen import rotation_from_angles
from mgen import rotation_around_x
matrix = rotation_from_angles([np.pi/2, 0, 0], 'XYX')
matrix.dot([0, 1, 0])
# array([0., 0., 1.])
matrix = rotation_around_axis([1, 0, 0], np.pi/2)
matrix.dot([0, 1, 0])
# array([0., 0., 1.])
matrix = rotation_around_x(np.pi/2)
matrix.dot([0, 1, 0])
# array([0., 0., 1.])
Note that the matrices are just regular numpy arrays, so no new data-structures are introduced when using this package.
Using pyquaternion is extremely simple; to install it (while still in python), run in your console:
import pip;
pip.main(['install','pyquaternion'])
Once installed:
from pyquaternion import Quaternion
v = [3,5,0]
axis = [4,4,1]
theta = 1.2 #radian
rotated_v = Quaternion(axis=axis,angle=theta).rotate(v)
I needed to rotate a 3D model around one of the three axes {x, y, z} in which that model was embedded and this was the top result for a search of how to do this in numpy. I used the following simple function:
def rotate(X, theta, axis='x'):
'''Rotate multidimensional array `X` `theta` degrees around axis `axis`'''
c, s = np.cos(theta), np.sin(theta)
if axis == 'x': return np.dot(X, np.array([
[1., 0, 0],
[0 , c, -s],
[0 , s, c]
]))
elif axis == 'y': return np.dot(X, np.array([
[c, 0, -s],
[0, 1, 0],
[s, 0, c]
]))
elif axis == 'z': return np.dot(X, np.array([
[c, -s, 0 ],
[s, c, 0 ],
[0, 0, 1.],
]))