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.

Setting a list of (x,y) cooordinates into an array so polygons are drawn

I have some code that prints out a list of coordinates (stored in points
f=open('139cm_2000_frame27.json')
data=json.load(f)
shapes=data["shapes"]
for i in shapes:
print(i['label']) # prints the label first
for c in i['points']:
d=np.array(c)
print(d) # an array containing coordinates in the form (x,y)
d, the coordinates, are the points of n number of 10 sided polygons. So coordinates 0-9 are the coordinates of the first polygon, coordinates 10-19 are the second polygon...
There could be any number of polygons in the json file but they will each, always, have 10 coordinates.
I need to find a way of using those coordinates to 'draw'/'recreate' these polygons in a 128x128 array.
I have tried
from skimage.draw import polygon
img = np.zeros((128, 128), dtype=np.uint8)
r = np.array([#the x coordinates of d])
c = np.array([#the y coordinates of d])
rr, cc = polygon(r, c)
img[rr, cc] = 1 #unsure about the 1
img
but I do not know how to 1) get sets of 10 coordinates and 2) read the xs into r and the ys into c
Thank you so much!
An example of the input json:
{
"version": "4.6.0",
"flags": {},
"shapes": [
{
"label": "blob",
"points": [
[
61.42857142857143,
20.285714285714285
],
[
59.10047478151446,
18.879430437885873
],
[
58.04359793578868,
16.37330203102605
],
[
58.661631924538575,
13.724584936383643
],
[
60.71850877026435,
11.94499905752918
],
[
63.42857142857143,
11.714285714285715
],
[
65.75666807562841,
13.120569562114127
],
[
66.81354492135418,
15.62669796897395
],
[
66.19551093260428,
18.275415063616357
],
[
64.13863408687851,
20.05500094247082
]
],
"group_id": null,
"shape_type": "polygon",
"flags": {}
},
{
"label": "blob",
"points": [
[
88.71428571428572,
82.42857142857143
],
[
85.63470409582908,
81.33512050565437
......

From software engineering point of view, it is recomended to break your code into simple separate parts (i.e. make it modular).
First you will need a function for reading the input json and parsing it. I called it read_input in the code below.
The format of the parsed data depends on the application.
I chose to return a list of pairs of ndarrays. Each element in the list represents one polygon. Each polygon contains 2 ndarrays: 1 for the x coordinates, and 1 for the y coordinates. I chose this representation because it is convenient for drawing the polygons (see below).
Second you will need a function for drawing the polygons (draw_polygons). It will contain an iteration over the polygon list, and call a lower level function for drawing 1 polygon (draw_one_polygon), again for modular reasons.
See the code below:
import json
import numpy as np
from skimage.draw import polygon
def read_input(filename: str):
polygons = []
with open(filename) as f:
data = json.load(f)
shapes = data["shapes"]
for i in shapes:
cur_poly_points = i["points"]
tmp = list(zip(*cur_poly_points))
# NOTE: The following line assumes that the point coordinates are given as (x,y).
# Change the order of the indices if needed.
polygons.append((np.array(tmp[1]), np.array(tmp[0])))
return polygons
def draw_one_polygon(img, one_poly):
r = one_poly[0];
c = one_poly[1];
rr, cc = polygon(r, c)
img[rr,cc] = 1
def draw_polygons(img, polygons):
for poly in polygons:
draw_one_polygon(img, poly)
filename = '139cm_2000_frame27.json'
polygons = read_input(filename)
img = np.zeros((128, 128), dtype=np.uint8)
draw_polygons(img, polygons)
print(img)
Note: in your actual code you should verify that the coordinates do not exceed the image dimension.
Documentation and example: skimage.draw.polygon
If you are not familiar with this notation: *cur_poly_points, see here: How to unzip a list of tuples into individual lists?.

Converting indices in marching cubes to original x,y,z space - visualizing isosurface 3d skimage

I want to draw a volume in x1,x2,x3-space. The volume is an isocurve found by the marching cubes algorithm in skimage. The function generating the volume is pdf_grid = f(x1,x2,x3) and
I want to draw the volume where pdf = 60% max(pdf).
My issue is that the marching cubes algorithm generates vertices and faces, but how do I map those back to the x1, x2, x3-space?
My (rather limited) understanding of marching cubes is that "vertices" refer to the indices in the volume (pdf_grid in my case). If "vertices" contained only the exact indices in the grid this would have been easy, but "vertices" contains floats and not integers. It seems like marching cubes do some interpolation between grid points (according to https://www.cs.carleton.edu/cs_comps/0405/shape/marching_cubes.html), so the question is then how to recover exactly the values of x1,x2,x3?
import numpy as np
import scipy.stats
import matplotlib.pyplot as plt
#Make some random data
cov = np.array([[1, .2, -.5],
[.2, 1.2, .1],
[-.5, .1, .8]])
dist = scipy.stats.multivariate_normal(mean = [1., 3., 2], cov = cov)
N = 500
x_samples = dist.rvs(size=N).T
#Create the kernel density estimator - approximation of a pdf
kernel = scipy.stats.gaussian_kde(x_samples)
x_mean = x_samples.mean(axis=1)
#Find the mode
res = scipy.optimize.minimize(lambda x: -kernel.logpdf(x),
x_mean #x0, initial guess
)
x_mode = res["x"]
num_el = 50 #number of elements in the grid
x_min = np.min(x_samples, axis = 1)
x_max = np.max(x_samples, axis = 1)
x1g, x2g, x3g = np.mgrid[x_min[0]:x_max[0]:num_el*1j,
x_min[1]:x_max[1]:num_el*1j,
x_min[2]:x_max[2]:num_el*1j
]
pdf_grid = np.zeros(x1g.shape) #implicit function/grid for the marching cubes
for an in range(x1g.shape[0]):
for b in range(x1g.shape[1]):
for c in range(x1g.shape[2]):
pdf_grid[a,b,c] = kernel(np.array([x1g[a,b,c],
x2g[a,b,c],
x3g[a,b,c]]
))
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
from skimage import measure
iso_level = .6 #draw a volume which contains pdf_val(mode)*60%
verts, faces, normals, values = measure.marching_cubes(pdf_grid, kernel(x_mode)*iso_level)
#How to convert the figure back to x1,x2,x3 space? I just draw the output as it was done in the skimage example here https://scikit-image.org/docs/0.16.x/auto_examples/edges/plot_marching_cubes.html#sphx-glr-auto-examples-edges-plot-marching-cubes-py so you can see the volume
# Fancy indexing: `verts[faces]` to generate a collection of triangles
mesh = Poly3DCollection(verts[faces],
alpha = .5,
label = f"KDE = {iso_level}"+r"$x_{mode}$",
linewidth = .1)
mesh.set_edgecolor('k')
fig, ax = plt.subplots(subplot_kw=dict(projection='3d'))
c1 = ax.add_collection3d(mesh)
c1._facecolors2d=c1._facecolor3d
c1._edgecolors2d=c1._edgecolor3d
#Plot the samples. Marching cubes volume does not capture these samples
pdf_val = kernel(x_samples) #get density value for each point (for color-coding)
x1, x2, x3 = x_samples
scatter_plot = ax.scatter(x1, x2, x3, c=pdf_val, alpha = .2, label = r" samples")
ax.scatter(x_mode[0], x_mode[1], x_mode[2], c = "r", alpha = .2, label = r"$x_{mode}$")
ax.set_xlabel(r"$x_1$")
ax.set_ylabel(r"$x_2$")
ax.set_zlabel(r"$x_3$")
# ax.set_box_aspect([np.ptp(i) for me in x_samples]) # equal aspect ratio
cbar = fig.color bar(scatter_plot, ax=ax)
cbar.set_label(r"$KDE(w) \approx pdf(w)$")
ax.legend()
#Make the axis limit so that the volume and samples are shown.
ax.set_xlim(- 5, np.max(verts, axis=0)[0] + 3)
ax.set_ylim(- 5, np.max(verts, axis=0)[1] + 3)
ax.set_zlim(- 5, np.max(verts, axis=0)[2] + 3)

This is probably way too late of an answer to help OP, but in case anyone else comes across this post looking for a solution to this problem, the issue stems from the marching cubes algorithm outputting the relevant vertices in array space. This space is defined by the number of elements per dimension of the mesh grid and the marching cubes algorithm does indeed do some interpolation in this space (explaining the presence of floats).
Anyways, in order to transform the vertices back into x1,x2,x3 space you just need to scale and shift them by the appropriate quantities. These quantities are defined by the range, number of elements of the mesh grid, and the minimum value in each dimension respectively. So using the variables defined in the OP, the following will provide the actual location of the vertices:
verts_actual = verts*((x_max-x_min)/pdf_grid.shape) + x_min

Python: Coordinates Boxes around Polyline

I have the following problem. I have a numpy array of coordinates (entry 0 to 2) and want to define all the coordinates of small boxes between pairs of my coordiante list instead of creating a huge box around the minimum and maximum of all my coordinates in the list. The boxes should have a range of 5 around the coordinate pairs for example.
My list for example looks like:
[[ 24.313 294.679 1.5 1. 0. ]
[ 25.51 295.263 1.5 2. 0. ]
[ 26.743 294.526 1.5 3. 0. ]
...,
[ 30.362 307.242 10.779 95. 0. ]
[ 29.662 307.502 10.38 96. 0. ]
[ 29.947 308.99 11.147 97. 0. ]]
My first idea is to calculate the minumum and maximum of each pair and use itertools.product to create the coordinates for the small boxes. So i want to have a box around 24.313 294.679 1.5 and 25.51 295.263 1.5, next a box aorund 25.51 295.263 1.5 and 26.743 294.526 1.5 and so on. For better understanding, i want the coordinates like here, but in 3D of course:
And not like here:
Is there any easy numpy, scipy approach to do this?

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
# create some data; in 2D so we can plot stuff
x = np.linspace(0, 2*np.pi, 10)
y = np.sin(x)
data = np.c_[x,y]
# --------------------------------------------------
# core bit: get boxes
# bboxes = np.array([data[:-1], np.diff(data, axis=0)]).transpose([1,2,0]) # shorter but with negative widths, etc
data_pairs = np.array([data[:-1], data[1:]])
minima = data_pairs.min(axis=0)
maxima = data_pairs.max(axis=0)
widths = maxima-minima
bboxes = np.array([minima, widths]).transpose(1,2,0)
# --------------------------------------------------
# plot
fig, ax = plt.subplots(1,1)
ax.plot(data[:,0], data[:,1], 'ko')
for bbox in bboxes:
patch = Rectangle(xy=bbox[:,0], width=bbox[0,1], height=bbox[1,1], linewidth=0., alpha=0.5)
ax.add_artist(patch)
plt.show()
with pads:
# padded boxes:
pad = 0.1
N, D = data.shape
correction = pad*np.ones((N-1,D))
padded = bboxes.copy()
padded[:,:,0] -= correction
padded[:,:,1] += 2*correction
fig, ax = plt.subplots(1,1)
ax.plot(data[:,0], data[:,1], 'ko')
for bbox in padded:
patch = Rectangle(xy=bbox[:,0], width=bbox[0,1], height=bbox[1,1], linewidth=0., alpha=0.5, facecolor='red')
ax.add_artist(patch)
ax.set_xlim(0-pad, 2*np.pi+pad)
ax.set_ylim(-1-pad, 1+pad)
plt.show()

Improving a numpy implementation of a simple spring network

I wanted a very simple spring system written in numpy. The system would be defined as a simple network of knots, linked by links. I'm not interested in evaluating the system over time, but instead I want to go from an initial state, change a variable (usually move a knot to a new position) and solve the system until it reaches a stable state (last applied force is below a given threshold). The knots have no mass, there's no gravity, the forces are all derived from each link's current lengths/init lengths. And the only "special" variable is that each knot can bet set as "anchored" (doesn't move).
So I wrote this simple solver below, and included a simple example. Jump to the very end for my question.
import numpy as np
from numpy.core.umath_tests import inner1d
np.set_printoptions(precision=4)
np.set_printoptions(suppress=True)
np.set_printoptions(linewidth =150)
np.set_printoptions(threshold=10)
def solver(kPos, kAnchor, link0, link1, w0, cycles=1000, precision=0.001, dampening=0.1, debug=False):
"""
kPos : vector array - knot position
kAnchor : float array - knot's anchor state, 0 = moves freely, 1 = anchored (not moving)
link0 : int array - array of links connecting each knot. each index corresponds to a knot
link1 : int array - array of links connecting each knot. each index corresponds to a knot
w0 : float array - initial link length
cycles : int - eval stops when n cycles reached
precision : float - eval stops when highest applied force is below this value
dampening : float - keeps system stable during each iteration
"""
kPos = np.asarray(kPos)
pos = np.array(kPos) # copy of kPos
kAnchor = 1-np.clip(np.asarray(kAnchor).astype(float),0,1)[:,None]
link0 = np.asarray(link0).astype(int)
link1 = np.asarray(link1).astype(int)
w0 = np.asarray(w0).astype(float)
F = np.zeros(pos.shape)
i = 0
for i in xrange(cycles):
# Init force applied per knot
F = np.zeros(pos.shape)
# Calculate forces
AB = pos[link1] - pos[link0] # get link vectors between knots
w1 = np.sqrt(inner1d(AB,AB)) # get link lengths
AB/=w1[:,None] # normalize link vectors
f = (w1 - w0) # calculate force vectors
f = f[:,None] * AB
# Apply force vectors on each knot
np.add.at(F, link0, f)
np.subtract.at(F, link1, f)
# Update point positions
pos += F * dampening * kAnchor
# If the maximum force applied is below our precision criteria, exit
if np.amax(F) < precision:
break
# Debug info
if debug:
print 'Iterations: %s'%i
print 'Max Force: %s'%np.amax(F)
return pos
Here's some test data to show how it works. In this case i'm using a grid, but in reality this can be any type of network, like a string with many knots, or a mess of polygons...:
import cProfile
# Create a 5x5 3D knot grid
z = np.linspace(-0.5, 0.5, 5)
x = np.linspace(-0.5, 0.5, 5)[::-1]
x,z = np.meshgrid(x,z)
kPos = np.array([np.array(thing) for thing in zip(x.flatten(), z.flatten())])
kPos = np.insert(kPos, 1, 0, axis=1)
'''
array([[-0.5 , 0. , 0.5 ],
[-0.25, 0. , 0.5 ],
[ 0. , 0. , 0.5 ],
...,
[ 0. , 0. , -0.5 ],
[ 0.25, 0. , -0.5 ],
[ 0.5 , 0. , -0.5 ]])
'''
# Define the links connecting each knots
link0 = [0,1,2,3,5,6,7,8,10,11,12,13,15,16,17,18,20,21,22,23,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19]
link1 = [1,2,3,4,6,7,8,9,11,12,13,14,16,17,18,19,21,22,23,24,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24]
AB = kPos[link0]-kPos[link1]
w0 = np.sqrt(inner1d(AB,AB)) # this is a square grid, each link's initial length will be 0.25
# Set the anchor states
kAnchor = np.zeros(len(kPos)) # All knots will be free floating
kAnchor[12] = 1 # Middle knot will be anchored
This is what the grid looks like:
If we run my code using this data, nothing will happen since the links aren't pushing or stretching:
print np.allclose(kPos,solver(kPos, kAnchor, link0, link1, w0, debug=True))
# Returns True
# Iterations: 0
# Max Force: 0.0
Now lets move that middle anchored knot up a bit and solve the system:
# Move the center knot up a little
kPos[12] = np.array([0,0.3,0])
# eval the system
new = solver(kPos, kAnchor, link0, link1, w0, debug=True) # positions will have moved
#Iterations: 102
#Max Force: 0.000976603249133
# Rerun with cProfile to see how fast it runs
cProfile.run('solver(kPos, kAnchor, link0, link1, w0)')
# 520 function calls in 0.008 seconds
And here's what the grid looks like after being pulled by that single anchored knot:
Question:
My actual use cases are a little more complex than this example and solve a little too slow for my taste: (100-200 knots with a network anywhere between 200-300 links, solves in a few seconds).
How can i make my solver function run faster? I'd consider Cython but i have zero experience with C. Any help would be greatly appreciated.

Your method, at a cursory glance, appears to be an explicit under-relaxation type of method. Calculate the residual force at each knot, apply a factor of that force as a displacement, repeat until convergence. It's the repeating until convergence that takes the time. The more points you have, the longer each iteration takes, but you also need more iterations for the constraints at one end of the mesh to propagate to the other.
Have you considered an implicit method? Write the equation for the residual force at each non-constrained node, assemble them into a large matrix, and solve in one step. Information now propagates across the entire problem in a single step. As an additional benefit, the matrix you construct should be sparse, which scipy has a module for.
Wikipedia: explicit and implicit methods
EDIT Example of an implicit method matching (roughly) your problem. This solution is linear, so it doesn't take into account the effect of the calculated displacement on the force. You would need to iterate (or use non-linear techniques) to calculate this. Hope it helps.
#!/usr/bin/python3
import matplotlib.pyplot as pp
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import scipy as sp
import scipy.sparse
import scipy.sparse.linalg
#------------------------------------------------------------------------------#
# Generate a grid of knots
nX = 10
nY = 10
x = np.linspace(-0.5, 0.5, nX)
y = np.linspace(-0.5, 0.5, nY)
x, y = np.meshgrid(x, y)
knots = list(zip(x.flatten(), y.flatten()))
# Create links between the knots
links = []
# Horizontal links
for i in range(0, nY):
for j in range(0, nX - 1):
links.append((i*nX + j, i*nX + j + 1))
# Vertical links
for i in range(0, nY - 1):
for j in range(0, nX):
links.append((i*nX + j, (i + 1)*nX + j))
# Create constraints. This dict takes a knot index as a key and returns the
# fixed z-displacement associated with that knot.
constraints = {
0 : 0.0,
nX - 1 : 0.0,
nX*(nY - 1): 0.0,
nX*nY - 1 : 1.0,
2*nX + 4 : 1.0,
}
#------------------------------------------------------------------------------#
# Matrix i-coordinate, j-coordinate and value
Ai = []
Aj = []
Ax = []
# Right hand side array
B = np.zeros(len(knots))
# Loop over the links
for link in links:
# Link geometry
displacement = np.array([ knots[1][i] - knots[0][i] for i in range(2) ])
distance = np.sqrt(displacement.dot(displacement))
# For each node
for i in range(2):
# If it is not a constraint, add the force associated with the link to
# the equation of the knot
if link[i] not in constraints:
Ai.append(link[i])
Aj.append(link[i])
Ax.append(-1/distance)
Ai.append(link[i])
Aj.append(link[not i])
Ax.append(+1/distance)
# If it is a constraint add a diagonal and a value
else:
Ai.append(link[i])
Aj.append(link[i])
Ax.append(+1.0)
B[link[i]] += constraints[link[i]]
# Create the matrix and solve
A = sp.sparse.coo_matrix((Ax, (Ai, Aj))).tocsr()
X = sp.sparse.linalg.lsqr(A, B)[0]
#------------------------------------------------------------------------------#
# Plot the links
fg = pp.figure()
ax = fg.add_subplot(111, projection='3d')
for link in links:
x = [ knots[i][0] for i in link ]
y = [ knots[i][1] for i in link ]
z = [ X[i] for i in link ]
ax.plot(x, y, z)
pp.show()