Related
I have two shapes, a rectangle and a parallelogram that signify two gantry systems. The one gantry system has a camera on it and can detect the position of the other gantry system as it sits above. I cannot via a series of transforms (translate, rotate, shear x, shear y, translate) get it even remotely close to fitting to the system 1. Could I please get some pointers/insight as to what I am doing wrong?
I've tested each transform with a unit vector so I know the math works. I suspect either I am using the incorrect angles(using the same on the unit vectors though), there are linearity issues where it is not quite linear and therefore transforms wont work (this also seems unlikely due to the physical nature), or most likely my order of operations are incorrect.
from matplotlib import pyplot as plt
import numpy as np
from mpl_toolkits.axes_grid1.inset_locator import TransformedBbox, BboxPatch, BboxConnector
def get_angle(array, array_2, side=3):
if side == 0:
# Get start and end points from array
vector = array[1] - array[0]
# Get start and end points from array
vector_2 = array_2[1] - array_2[0]
elif side == 1:
# Get start and end points from array
vector = array[2] - array[1]
# Get start and end points from array
vector_2 = array_2[2] - array_2[1]
elif side == 2:
# Get start and end points from array
vector = array[2] - array[3]
# Get start and end points from array
vector_2 = array_2[2] - array_2[3]
elif side == 3:
# Get start and end points from array
vector = array[3] - array[0]
# Get start and end points from array
vector_2 = array_2[3] - array_2[0]
# Calculate unit vectors
dot = vector[0] * vector_2[0] + vector[1] * vector_2[1] # dot product between [x1, y1] and [x2, y2]
det = vector[0] * vector_2[1] - vector[1] * vector_2[0] # determinant
angle = np.arctan2(det, dot) # atan2(y, x) or atan2(sin, cos)
return angle
def shear_trans_x(coords, phi):
shear_x = np.array([[1, np.tan(phi), 0],
[0, 1, 0],
[0, 0, 1]])
coords = np.append(coords, np.ones((coords.shape[0], 1)), axis=1)
resultant = coords # shear_x.T
return resultant[:, 0:2]
def shear_trans_y(coords, psi):
shear_y = np.array([[1, 0, 0],
[np.tan(psi), 1, 0],
[0, 0, 1]])
coords = np.append(coords, np.ones((coords.shape[0], 1)), axis=1)
resultant = coords # shear_y.T
return resultant[:, 0:2]
def translate(coordinates, offset):
coordinates = np.append(coordinates, np.ones((coordinates.shape[0], 1)), axis=1)
a = np.array([[1, 0, offset[0]],
[0, 1, offset[1]],
[0, 0, 1 ]])
result = coordinates # a.T
return result[:, 0:2]
def rotate(coords, theta, origin=[0,0]):
cos = np.cos(theta)
sin = np.sin(theta)
a = np.array([[cos, -sin, 0],
[sin, cos, 0],
[0, 0, 1]])
if np.all(origin == [0, 0]):
coords = np.append(coords, np.ones((coords.shape[0], 1)), axis=1)
result = coords # a.T
return result[:, 0:2]
else:
coords = translate(coords, -origin)
coords = rotate(coords, theta, origin=[0, 0])
coords = translate(coords, origin)
return coords
def mark_inset(parent_axes, inset_axes, loc1a=1, loc1b=1, loc2a=2, loc2b=2, **kwargs):
'''
draw a bbox of the region of the inset axes in the parent axes and
connecting lines between the bbox and the inset axes area
loc1, loc2 : {1, 2, 3, 4}
'''
rect = TransformedBbox(inset_axes.viewLim, parent_axes.transData)
p1 = BboxConnector(inset_axes.bbox, rect, loc1=loc1a, loc2=loc1b, **kwargs)
inset_axes.add_patch(p1)
p1.set_clip_on(False)
p2 = BboxConnector(inset_axes.bbox, rect, loc1=loc2a, loc2=loc2b, **kwargs)
inset_axes.add_patch(p2)
p2.set_clip_on(False)
pp = BboxPatch(rect, fill=False, **kwargs)
parent_axes.add_patch(pp)
return pp, p1, p2
if __name__ == '__main__':
# calibration data
gantry_1_coords = np.array([[169.474, 74.4851], [629.474, 74.4851], [629.474, 334.4851], [169.474, 334.4851]])
gantry_2_coords_error = np.array([[-0.04, 0.04], [-0.04, -0.31], [0.76, -0.57], [1.03, 0.22]])
# gantry_2_coords_error = np.array([[0.13, 0.04], [-0.13, -0.75], [0.31, -0.93], [0.58, -0.31]])
# add error to gantry 1 coords
gantry_2_coords = gantry_1_coords + gantry_2_coords_error
# append first point to end for plotting to display a closed box
gantry_1_coords = np.append(gantry_1_coords, np.array([gantry_1_coords[0]]), axis=0)
gantry_2_coords = np.append(gantry_2_coords, np.array([gantry_2_coords[0]]), axis=0)
# get length of diagonal direction
magnitude = np.linalg.norm(gantry_1_coords[0] - gantry_1_coords[2])
magnitude_gantry_2 = np.linalg.norm(gantry_2_coords[0] - gantry_2_coords[2])
# translate to gantry_1 first position
translated_gantry_2 = translate(gantry_2_coords, (gantry_1_coords[0] - gantry_2_coords[0]))
print('translation_offset_1', ' = ', gantry_1_coords[0] - gantry_2_coords[0])
# rotate gantry_2 to gantry_1
theta = get_angle(translated_gantry_2, gantry_1_coords, side=0)
rotate_gantry_2_coords = rotate(translated_gantry_2, theta, translated_gantry_2[0])
print('rotation angle', ' = ', theta)
# un-shear x axis gantry_2
shear_phi = get_angle(rotate_gantry_2_coords, gantry_1_coords, side=3)
sheared_x_gantry_2 = shear_trans_x(rotate_gantry_2_coords, shear_phi)
print('shear x angle', ' = ', shear_phi)
# un-shear y axis gantry_2
shear_psi = get_angle(sheared_x_gantry_2, gantry_1_coords, side=2)
sheared_y_gantry_2 = shear_trans_y(sheared_x_gantry_2, shear_psi)
print('shear y angle', ' = ', shear_psi)
# translate to gantry_1 first position
final_gantry_2_coords = translate(sheared_y_gantry_2, (gantry_1_coords[0] - sheared_y_gantry_2[0]))
print('translation_offset_2', ' = ', gantry_1_coords[0] - sheared_y_gantry_2[0])
# create exaggerated errors for plotting
ex_gantry_2_coords = (gantry_2_coords - gantry_1_coords) * 50 + gantry_2_coords
ex_gantry_2_final_coords = (final_gantry_2_coords - gantry_1_coords) * 50 + final_gantry_2_coords
# separate out x & y components for plotting
gantry_1_x, gantry_1_y = gantry_1_coords.T
gantry_2_x, gantry_2_y = ex_gantry_2_coords.T
gantry_2_final_x, gantry_2_final_y = ex_gantry_2_final_coords.T
# plot results
fig, ax = plt.subplots()
ax.plot(gantry_1_x, gantry_1_y, color='black', linestyle='--', label='gantry_1')
ax.plot(gantry_2_x, gantry_2_y, color='blue', linestyle='--', label='gantry_2 original')
ax.plot(gantry_2_final_x, gantry_2_final_y, color='red', linestyle='--', label='gantry_2 transformed')
# get legend lines and labels from center graph
lines, labels = ax.get_legend_handles_labels()
fig.legend(lines, labels)
plt.show()
# print('gantry 1 positions: ', gantry_1_coords)
# print('transformed gantry 2 positions: ', final_gantry_2_coords)
Fixing existing code
In terms of the existing code, I applied the transformations one by one, and I think you're missing a negative sign here:
sheared_x_gantry_2 = shear_trans_x(rotate_gantry_2_coords, -shear_phi)
# ^--- here
After applying that, the graph looks better:
Least squares fit
However, I think this is the wrong general approach. For example, when you fix the shear, that's going to break the translation and rotation, at least a little bit. You can repeatedly apply the fixes, and converge on the right answer, but there's a better way.
Instead, I would suggest finding a least-squares fit for the transformation matrix, rather than building up a bunch of rotation and shear matrices. Numpy has a function that will do this.
def add_bias_term(matrix):
return np.append(np.ones((matrix.shape[0], 1)), matrix, axis=1)
x, _, _, _ = np.linalg.lstsq(add_bias_term(gantry_2_coords), gantry_1_coords, rcond=None)
final_gantry_2_coords = add_bias_term(gantry_2_coords) # x
This is both a heck of a lot shorter, and produces a better fit to boot:
And here is the matrix that it finds:
array([[ 0.19213806, -0.37107611],
[ 1.00028902, 0.00123954],
[-0.00359818, 1.00014869]])
(Note that the first row is the bias term.)
Although, the fit is not perfect. Here are the residuals:
array([[-0.06704727, -0.10997465], # point 0
[ 0.06716097, 0.11016114], # point 1
[-0.06720015, -0.1102254 ], # point 2
[ 0.06708645, 0.11003891]]) # point 3
Unfortunately, this remaining error is nonlinear, by definition. (If there were an affine matrix which reduced the error better, lstsq would have found it.)
Adding nonlinearity
Eyeballing the residuals, the error goes in one direction when both x and y are large, and in the other direction when only one of x or y are large. That suggests to me that you need an interaction term. In other words, you need to preprocess the input matrix so that it has a column with X, a column with Y, and a column with X*Y.
The code to do that looks like this:
def add_bias_term(matrix):
return np.append(np.ones((matrix.shape[0], 1)), matrix, axis=1)
def add_interaction(matrix):
inter = (matrix[:, 0] * matrix[:, 1]).reshape(matrix.shape[0], 1)
return np.append(inter, matrix, axis=1)
x, _, _, _ = np.linalg.lstsq(add_bias_term(add_interaction(gantry_2_coords)), gantry_1_coords, rcond=None)
final_gantry_2_coords = (add_bias_term(add_interaction(gantry_2_coords)) # x)
And the graph for that looks like this:
And that's close enough that the two graphs are right on top of each other.
i have this code:
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint
def rownanie(Y, t, l, q, a, u):
y1, y2, z1, z2 = Y
dydt = [y2, ((l*q)/a)*(1/y1)*(1-z2*u), z2, (a*y2*u)/y1]
return dydt
l = 100
q = 1
a = 10
u = 0.25
y0 = -1
z0 = 0
y0_prim, z0_prim = 0, 0
t = np.linspace(0, 100, 10001)
sol = odeint(rownanie, [y0, y0_prim, z0, z0_prim], t, args=(l,q,a,u))
print(sol)
plt.plot(sol[:, 0], sol[:, 2])
plt.xlabel('Y')
plt.ylabel('Z')
plt.grid()
So i have 4 columns of data, lets say [:, 0] till [:,0]. I have to focus only on two : [:, 0] , [:, 2]. When i make a graph of it - its a harmonic function. [:, 0] are values , [:, 2] are arguments. I need to find these arguments for which values are max. Or i need the difference, the distance beetween two arguments (two maxes) I tried with "if", but the values are approximations so they are not the same. Could you help me with this one?
You were right, you need to define a tolerance for the difference with respect to the maximum value. I marked the points for clarification. The idea here is to first get the difference from the maximum of values max(sol[:, 0]). Then you can use the NumPy array's indexing using a tolerance of 1e-4. [abs(diff) < 1e-4] returns your indices where this condition holds True. Now you have these maximum 5 points. You can do whatever processing you want with them. The choice of tolerance will depend also on your number of mesh points (10001 in this case). It requires some playing around. One can also write some function to check this smartly.
diff = sol[:, 0] - max(sol[:, 0])
plt.plot(sol[:, 0], sol[:, 2])
plt.plot(sol[:, 0][abs(diff) < 1e-4], sol[:, 2][abs(diff) < 1e-4], 'kx')
Graph
And i need to find this difference but every maximum is a little bit different
I have trajectory data, where each trajectory consists of a sequence of coordinates(x, y points) and each trajectory is identified by a unique ID.
These trajectories are in x - y plane, and I want to divide the whole plane into equal sized grid (square grid). This grid is obviously invisible but is used to divide trajectories into sub-segments. Whenever a trajectory intersects with a grid line, it is segmented there and becomes a new sub-trajectory with new_id.
I have included a simple handmade graph to make clear what I am expecting.
It can be seen how the trajectory is divided at the intersections of the grid lines, and each of these segments has new unique id.
I am working on Python, and seek some python implementation links, suggestions, algorithms, or even a pseudocode for the same.
Please let me know if anything is unclear.
UPDATE
In order to divide the plane into grid , cell indexing is done as following:
#finding cell id for each coordinate
#cellid = (coord / cellSize).astype(int)
cellid = (coord / 0.5).astype(int)
cellid
Out[] : array([[1, 1],
[3, 1],
[4, 2],
[4, 4],
[5, 5],
[6, 5]])
#Getting x-cell id and y-cell id separately
x_cellid = cellid[:,0]
y_cellid = cellid[:,1]
#finding total number of cells
xmax = df.xcoord.max()
xmin = df.xcoord.min()
ymax = df.ycoord.max()
ymin = df.ycoord.min()
no_of_xcells = math.floor((xmax-xmin)/ 0.5)
no_of_ycells = math.floor((ymax-ymin)/ 0.5)
total_cells = no_of_xcells * no_of_ycells
total_cells
Out[] : 25
Since the plane is now divided into 25 cells each with a cellid. In order to find intersections, maybe I could check the next coordinate in the trajectory, if the cellid remains the same, then that segment of the trajectory is in the same cell and has no intersection with grid. Say, if x_cellid[2] is greater than x_cellid[0], then segment intersects vertical grid lines. Though, I am still unsure how to find the intersections with the grid lines and segment the trajectory on intersections giving them new id.
This can be solved by shapely:
%matplotlib inline
import pylab as pl
from shapely.geometry import MultiLineString, LineString
import numpy as np
from matplotlib.collections import LineCollection
x0, y0, x1, y1 = -10, -10, 10, 10
n = 11
lines = []
for x in np.linspace(x0, x1, n):
lines.append(((x, y0), (x, y1)))
for y in np.linspace(y0, y1, n):
lines.append(((x0, y), (x1, y)))
grid = MultiLineString(lines)
x = np.linspace(-9, 9, 200)
y = np.sin(x)*x
line = LineString(np.c_[x, y])
fig, ax = pl.subplots()
for i, segment in enumerate(line.difference(grid)):
x, y = segment.xy
pl.plot(x, y)
pl.text(np.mean(x), np.mean(y), str(i))
lc = LineCollection(lines, color="gray", lw=1, alpha=0.5)
ax.add_collection(lc);
The result:
To not use shapely, and do it yourself:
import pylab as pl
import numpy as np
from matplotlib.collections import LineCollection
x0, y0, x1, y1 = -10, -10, 10, 10
n = 11
xgrid = np.linspace(x0, x1, n)
ygrid = np.linspace(y0, y1, n)
x = np.linspace(-9, 9, 200)
y = np.sin(x)*x
t = np.arange(len(x))
idx_grid, idx_t = np.where((xgrid[:, None] - x[None, :-1]) * (xgrid[:, None] - x[None, 1:]) <= 0)
tx = idx_t + (xgrid[idx_grid] - x[idx_t]) / (x[idx_t+1] - x[idx_t])
idx_grid, idx_t = np.where((ygrid[:, None] - y[None, :-1]) * (ygrid[:, None] - y[None, 1:]) <= 0)
ty = idx_t + (ygrid[idx_grid] - y[idx_t]) / (y[idx_t+1] - y[idx_t])
t2 = np.sort(np.r_[t, tx, tx, ty, ty])
x2 = np.interp(t2, t, x)
y2 = np.interp(t2, t, y)
loc = np.where(np.diff(t2) == 0)[0] + 1
xlist = np.split(x2, loc)
ylist = np.split(y2, loc)
fig, ax = pl.subplots()
for i, (xp, yp) in enumerate(zip(xlist, ylist)):
pl.plot(xp, yp)
pl.text(np.mean(xp), np.mean(yp), str(i))
lines = []
for x in np.linspace(x0, x1, n):
lines.append(((x, y0), (x, y1)))
for y in np.linspace(y0, y1, n):
lines.append(((x0, y), (x1, y)))
lc = LineCollection(lines, color="gray", lw=1, alpha=0.5)
ax.add_collection(lc);
You're asking a lot. You should attack most of the design and coding yourself, once you have a general approach. Algorithm identification is reasonable for Stack Overflow; asking for design and reference links is not.
I suggest that you put the point coordinates into a list. use the NumPy and SciKit capabilities to interpolate the grid intersections. You can store segments in a list (of whatever defines a segment in your data design). Consider making a dictionary that allows you to retrieve the segments by grid coordinates. For instance, if segments are denoted only by the endpoints, and points are a class of yours, you might have something like this, using the lower-left corner of each square as its defining point:
grid_seg = {
(0.5, 0.5): [p0, p1],
(1.0, 0.5): [p1, p2],
(1.0, 1.0): [p2, p3],
...
}
where p0, p1, etc. are the interpolated crossing points.
Each trajectory is composed of a series of straight line segments. You therefore need a routine to break each line segment into sections that lie completely within a grid cell. The basis for such a routine would be the Digital Differential Analyzer (DDA) algorithm, though you'll need to modify the basic algorithm since you need endpoints of the line within each cell, not just which cells are visited.
A couple of things you have to be careful of:
1) If you're working with floating point numbers, beware of rounding errors in the calculation of the step values, as these can cause the algorithm to fail. For this reason many people choose to convert to an integer grid, obviously with a loss of precision. This is a good discussion of the issues, with some working code (though not python).
2) You'll need to decide which of the 4 grid lines surrounding a cell belong to the cell. One convention would be to use the bottom and left edges. You can see the issue if you consider a horizontal line segment that falls on a grid line - does its segments belong to the cell above or the cell below?
Cheers
data = list of list of coordinates
For point_id, point_coord in enumerate(point_coord_list):
if current point & last point stayed in same cell:
append point's index to last list of data
else:
append a new empty list to data
interpolate the two points and add a new point
that is on the grid lines.
Data stores all trajectories. Each list within the data is a trajectory.
The cell index along x and y axes (x_cell_id, y_cell_id) can be found by dividing coordinate of point by dimension of cell, then round to integer. If the cell indices of current point are same as that of last points, then these two points are in the same cell. list is good for inserting new points but it is not as memory efficient as arrays.
Might be a good idea to create a class for trajectory. Or use a memory buffer and sparse data structure instead of list and list and an array for the x-y coordinates if the list of coordinates wastes too much memory.
Inserting new points into array is slow, so we can use another array for new points.
Warning: I haven't thought too much about the things below. It probably has bugs, and someone needs to fill in the gaps.
# coord n x 2 numpy array.
# columns 0, 1 are x and y coordinate.
# row n is for point n
# cell_size length of one side of the square cell.
# n_ycells number of cells along the y axis
import numpy as np
cell_id_2d = (coord / cell_size).astype(int)
x_cell_id = cell_id_2d[:,0]
y_cell_id = cell_id_2d[:,1]
cell_id_1d = x_cell_id + y_cell_id*n_x_cells
# if the trajectory exits a cell, its cell id changes
# and the delta_cell_id is not zero.
delta_cell_id = cell_id_1d[1:] - cell_id_1d[:-1]
# The nth trajectory should contains the points from
# the (crossing_id[n])th to the (crossing_id[n + 1] - 1)th
w = np.where(delta_cell_id != 0)[0]
crossing_ids = np.empty(w.size + 1)
crossing_ids[1:] = w
crossing_ids[0] = 0
# need to interpolate when the trajectory cross cell boundary.
# probably can replace this loop with numpy functions/indexing
new_points = np.empty((w.size, 2))
for i in range(1, n):
st = coord[crossing_ids[i]]
en = coord[crossing_ids[i+1]]
# 1. check which boundary of the cell is crossed
# 2. interpolate
# 3. put points into new_points
# Each trajectory contains some points from coord array and 2 points
# in the new_points array.
For retrieval, make a sparse array that contains the index of the starting point in the coord array.
Linear interpolation can look bad if the cell size is large.
Further explanation:
Description of the grid
For n_xcells = 4, n_ycells = 3, the grid is:
0 1 2 3 4
0 [ ][ ][ ][ ][ ]
1 [ ][ ][ ][* ][ ]
2 [ ][ ][ ][ ][ ]
[* ] has an x_index of 3 and a y_index of 1.
There are (n_x_cells * n_y_cells) cells in the grid.
Relationship between point and cell
The cell that contains the ith point of the trajectory has an x_index of x_cell_id[i] and a y_index of x_cell_id[i]. I get this by discretization through dividing the xy-coordinates of the points by the length of the cell and then truncate to integers.
The cell_id_1d of the cells are the number in [ ]
0 1 2 3 4
0 [0 ][1 ][2 ][3 ][4 ]
1 [5 ][6 ][7 ][8 ][9 ]
2 [10][11][12][13][14]
cell_id_1d[i] = x_cell_id[i] + y_cell_id[i]*n_x_cells
I converted the pair of cell indices (x_cell_id[i], y_cell_id[i]) for the ith point to a single index called cell_id_1d.
How to find if trajectory exit a cell at the ith point
Now, the ith and (i + 1)th points are in same cell, if and only if (x_cell_id[i], y_cell_id[i]) == (x_cell_id[i + 1], y_cell_id[i + 1]) and also cell_id_1d[i] == cell_id[i + 1], and cell_id[i + 1] - cell_id[i] == 0. delta_cell_ids[i] = cell_id_1d[i + 1] - cell_id[i], which is zero if and only the ith and (i + 1)th points are in the same cell.
I'm trying to get some code that will perform a perspective transformation (in this case a 3d rotation) on an image.
import os.path
import numpy as np
import cv
def rotation(angle, axis):
return np.eye(3) + np.sin(angle) * skew(axis) \
+ (1 - np.cos(angle)) * skew(axis).dot(skew(axis))
def skew(vec):
return np.array([[0, -vec[2], vec[1]],
[vec[2], 0, -vec[0]],
[-vec[1], vec[0], 0]])
def rotate_image(imgname_in, angle, axis, imgname_out=None):
if imgname_out is None:
base, ext = os.path.splitext(imgname_in)
imgname_out = base + '-out' + ext
img_in = cv.LoadImage(imgname_in)
img_size = cv.GetSize(img_in)
img_out = cv.CreateImage(img_size, img_in.depth, img_in.nChannels)
transform = rotation(angle, axis)
cv.WarpPerspective(img_in, img_out, cv.fromarray(transform))
cv.SaveImage(imgname_out, img_out)
When I rotate about the z-axis, everything works as expected, but rotating around the x or y axis seems completely off. I need to rotate by angles as small as pi/200 before I start getting results that seem at all reasonable. Any idea what could be wrong?
First, build the rotation matrix, of the form
[cos(theta) -sin(theta) 0]
R = [sin(theta) cos(theta) 0]
[0 0 1]
Applying this coordinate transform gives you a rotation around the origin.
If, instead, you want to rotate around the image center, you have to first shift the image center
to the origin, then apply the rotation, and then shift everything back. You can do so using a
translation matrix:
[1 0 -image_width/2]
T = [0 1 -image_height/2]
[0 0 1]
The transformation matrix for translation, rotation, and inverse translation then becomes:
H = inv(T) * R * T
I'll have to think a bit about how to relate the skew matrix to the 3D transformation. I expect the easiest route is to set up a 4D transformation matrix, and then to project that back to 2D homogeneous coordinates. But for now, the general form of the skew matrix:
[x_scale 0 0]
S = [0 y_scale 0]
[x_skew y_skew 1]
The x_skew and y_skew values are typically tiny (1e-3 or less).
Here's the code:
from skimage import data, transform
import numpy as np
import matplotlib.pyplot as plt
img = data.camera()
theta = np.deg2rad(10)
tx = 0
ty = 0
S, C = np.sin(theta), np.cos(theta)
# Rotation matrix, angle theta, translation tx, ty
H = np.array([[C, -S, tx],
[S, C, ty],
[0, 0, 1]])
# Translation matrix to shift the image center to the origin
r, c = img.shape
T = np.array([[1, 0, -c / 2.],
[0, 1, -r / 2.],
[0, 0, 1]])
# Skew, for perspective
S = np.array([[1, 0, 0],
[0, 1.3, 0],
[0, 1e-3, 1]])
img_rot = transform.homography(img, H)
img_rot_center_skew = transform.homography(img, S.dot(np.linalg.inv(T).dot(H).dot(T)))
f, (ax0, ax1, ax2) = plt.subplots(1, 3)
ax0.imshow(img, cmap=plt.cm.gray, interpolation='nearest')
ax1.imshow(img_rot, cmap=plt.cm.gray, interpolation='nearest')
ax2.imshow(img_rot_center_skew, cmap=plt.cm.gray, interpolation='nearest')
plt.show()
And the output:
I do not get the way you build your rotation matrix. It seems rather complicated to me. Usually, it would be built by constructing a zero matrix, putting 1 on unneeded axes, and the common sin, cos, -cos, sin into the two used dimensions. Then multiplying all these together.
Where did you get that np.eye(3) + np.sin(angle) * skew(axis) + (1 - np.cos(angle)) * skew(axis).dot(skew(axis)) construct from?
Try building the projection matrix from basic building blocks. Constructing a rotation matrix is fairly easy, and "rotationmatrix dot skewmatrix" should work.
You might need to pay attention to the rotation center though. Your image probably is placed at a virtual position of 1 on the z axis, so by rotating on x or y, it moves around a bit.
So you'd need to use a translation so z becomes 0, then rotate, then translate back. (Translation matrixes in affine coordinates are pretty simple, too. See wikipedia: https://en.wikipedia.org/wiki/Transformation_matrix )
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.],
]))