Related
I have a set of 3D points and the correspondend point in 2D from a diffrent position.
The 2D points are on a 360° panorama. So i can convert them to polar -> (r,theta , phi ) with no information about r.
But r is just the distance of the transformed 3D Point:
[R|t]*xyz = xyz'
r = sqrt(xyz')
Then with the 3D point also in spherical coordinates, i can now search for R and t with this linear equation system:
x' = sin(theta) * cos(phi) * r
y' = sin(theta) * cos(phi) * r
z' = sin(theta) * cos(phi) * r
I get good results for tests with t=[0,0,0.5] and without any rotation. But if there is a rotation the results are bad.
Is this the correct approach for my problem?
How can I use solvepnp() without a camera Matrix (it is a panorama without distortion)?
I am using opt.least_squares to calculate R and t.
I solved it with two diffrent methods.
One is for small rotations and solves for R and t (12 parameter), the other method can compute even big rotations with Euler and t (6 parameter).
I am calling the opt.least_squares() two times with diffrent initial values and use the method with an better reprojection error.
The f.eul2rot is just a conversion between euler angles and the rotation matrix.
def sphere_eq(p):
xyz_points = xyz
uv_points = uv
#r11,r12,r13,r21,r22,r23,r31,r32,r33,tx,ty,tz = p
if len(p) == 12:
r11, r12, r13, r21, r22, r23, r31, r32, r33, tx, ty, tz = p
R = np.array([[r11, r12, r13],
[r21, r22, r23],
[r31, r32, r33]])
else:
gamma, beta, alpha,tx,ty,tz = p
E = [gamma, beta, alpha]
R = f.eul2rot(E)
pi = np.pi
eq_grad = ()
for i in range(len(xyz_points)):
# Point with Orgin: LASER in Cartesian and Spherical coordinates
xyz_laser = np.array([xyz_points[i,0],xyz_points[i,1],xyz_points[i,2]])
# Transformation - ROTATION MATRIX and Translation Vector
t = np.array([[tx, ty, tz]])
# Point with Orgin: CAMERA in Cartesian and Spherical coordinates
uv_camera = np.array(uv_points[i])
long_camera = ((uv_camera[0]) / w) * 2 * pi
lat_camera = ((uv_camera[1]) / h) * pi
xyz_camera = (R.dot(xyz_laser) + t)[0]
r = np.linalg.norm(xyz_laser + t)
x_eq = (xyz_camera[0] - (np.sin(lat_camera) * np.cos(long_camera) * r),)
y_eq = (xyz_camera[1] - (np.sin(lat_camera) * np.sin(long_camera) * r),)
z_eq = (xyz_camera[2] - (np.cos(lat_camera) * r),)
eq_grad = eq_grad + x_eq + y_eq + z_eq
return eq_grad
x = np.zeros(12)
x[0], x[4], x[8] = 1, 1, 1
initial_guess = [x,np.zeros(6)]
for p, x0 in enumerate(initial_guess):
x = opt.least_squares(sphere_eq, x0, '3-point', method='trf')
if len(x0) == 6:
E = np.resize(x.x[:4], 3)
R = f.eul2rot(E)
t = np.resize(x.x[4:], (3, 1))
else:
R = np.resize(x.x[:8], (3, 3))
E = f.rot2eul(R)
t = np.resize(x.x[9:], (3, 1))
I have the following data points: There are 5 sublists in this list of data. What I am trying to do is find the points where there is a maximum amount of curvature.
for i in range(len(smallest_5)):
x = [x for x,y in smallest_5[i]]
y = [y for x,y in smallest_5[i]]
plt.scatter(x,y)
plt.savefig('bend'+str(count)+'.png')
plt.show()
I've used this code to plot the points.
sub_curvature = []
for i in range(len(smallest_5)):
a = np.array(smallest_5[i])
dx_dt = np.gradient(a[:,0])
dy_dt = np.gradient(a[:,1])
velocity = np.array([ [dx_dt[i], dy_dt[i]] for i in range(dx_dt.size)])
ds_dt = np.sqrt(dx_dt * dx_dt + dy_dt * dy_dt)
tangent = np.array([1/ds_dt] * 2).transpose() * velocity
tangent_x = tangent[:, 0]
tangent_y = tangent[:, 1]
deriv_tangent_x = np.gradient(tangent_x)
deriv_tangent_y = np.gradient(tangent_y)
dT_dt = np.array([ [deriv_tangent_x[i], deriv_tangent_y[i]] for i in range(deriv_tangent_x.size)])
length_dT_dt = np.sqrt(deriv_tangent_x * deriv_tangent_x + deriv_tangent_y * deriv_tangent_y)
normal = np.array([1/length_dT_dt] * 2).transpose() * dT_dt
d2s_dt2 = np.gradient(ds_dt)
d2x_dt2 = np.gradient(dx_dt)
d2y_dt2 = np.gradient(dy_dt)
curvature = np.abs(d2x_dt2 * dy_dt - dx_dt * d2y_dt2) / (dx_dt * dx_dt + dy_dt * dy_dt)**1.5
t_component = np.array([d2s_dt2] * 2).transpose()
n_component = np.array([curvature * ds_dt * ds_dt] * 2).transpose()
acceleration = t_component * tangent + n_component * normal
sub_curvature.append(curvature)
I used the code above to calculate the curvature of individual points on the data.
Above are some of the graphs I created using the data. As you can see, the first one has no real bend but the last two have a point where there is a large bend. How could I go about identifying this region? Is it correct to calculate the curvature for individual points or should I look at the curvature over a sliding window of points? Thank you!
If we assume "curvature" to mean circular curvature, then you'll need a sliding window over 3 points (since 3 points determine a circle).
For any three points (a,b,c) the curvature is 2 * |(a-b) x (b-c)| / (|a-b| * |b-c| * |c-b|).
We can get a-b and b-c from
ab = smallest_5[1:] - smallest_5[:-1]
and a-c from:
ac = smallest_5[2:] - smallest_5[:-2]
Then the squared curvature is:
curv_sq = 4 * (np.cross(ab[1:], ab[:-1])**2).sum() / ((ab[1:]**2).sum() * (ab[:-1]**2).sum() * (ac**2).sum())
Since we're just looking for a maximum curvature, we don't actually have to take the square root of that. We can find the index of the point with maximum curvature with
max_curv_index = np.argmax(curv_sq)
As an idea, you can find the minimum y which is not the first or the last value in the y-dimension of the array. For example:
s4 = np.array(smallest_5[4]).T # exctract a sub-array
min_y = np.agrmin(s4[1]) # gives 13
min_y == (0 or len(s4[1]-1) # gives False, so the minimum is in the middle of the curve
s0 = np.array(smallest_5[0]).T # exctract a sub-array
min_y = np.agrmin(s0[1]) # gives 16
min_y == (0 or len(s0[1]-1) # gives True, so the minimum is not in the middle of the curve
I am new in Python and I have a sphere of radius (R) and centred at (x0,y0,z0). Now, I need to find those points which are either on the surface of the sphere or inside the sphere e.g. points (x1,y1,z1) which satisfy ((x1-x0)**2+(y1-y0)**2+(z1-x0)*82)**1/2 <= R. I would like to print only those point's coordinates in a form of numpy array. Output would be something like this-[[x11,y11,z11],[x12,y12,z12],...]. I have the following code so far-
import numpy as np
import math
def create_points_around_atom(number,atom_coordinates):
n= number
x0 = atom_coordinates[0]
y0 = atom_coordinates[1]
z0 = atom_coordinates[2]
R = 1.2
for i in range(n):
phi = np.random.uniform(0,2*np.pi,size=(n,))
costheta = np.random.uniform(-1,1,size=(n,))
u = np.random.uniform(0,1,size=(n,))
theta = np.arccos(costheta)
r = R * np.cbrt(u)
x1 = r*np.sin(theta)*np.cos(phi)
y1 = r*np.sin(theta)*np.sin(phi)
z1 = r*np.cos(theta)
dist = np.sqrt((x1-x0)**2+(y1-y0)**2+(z1-z0)**2)
distance = list(dist)
point_on_inside_sphere = []
for j in distance:
if j <= R:
point_on_inside_sphere.append(j)
print('j:',j,'\tR:',R)
print('The list is:', point_on_inside_sphere)
print(len(point_on_inside_sphere))
kk =0
for kk in range(len(point_on_inside_sphere)):
for jj in point_on_inside_sphere:
xx = np.sqrt(jj**2-y1**2-z1**2)
yy = np.sqrt(jj**2-x1**2-z1**2)
zz = np.sqrt(jj**2-y1**2-x1**2)
print("x:", xx, "y:", yy,"z:", zz)
kk +=1
And I am running it-
create_points_around_atom(n=2,structure[1].coords)
where, structure[1].coords is a numpy array of three coordinates.
To sum up what has been discussed in the comments, and some other points:
There is no need to filter the points because u <= 1, which means np.cbrt(u) <= 1 and hence r = R * np.cbrt(u) <= R, i.e. all points will already be inside or on the surface of the sphere.
Calling np.random.uniform with size=(n,) creates an array of n elements, so there's no need to do this n times in a loop.
You are filtering distances from the atom_coordinate, but the points you are generating are centered on [0, 0, 0], because you are not adding this offset.
Passing R as an argument seems more sensible than hard-coding it.
There's no need to "pre-load" arguments in Python like one would sometimes do in C.
Since sin(theta) is non-negative over the sphere, you can directly calculate it from the costheta array using the identity cos²(x) + sin²(x) = 1.
Sample implementation:
# pass radius as an argument
def create_points_around_atom(number, center, radius):
# generate the random quantities
phi = np.random.uniform( 0, 2*np.pi, size=(number,))
theta_cos = np.random.uniform(-1, 1, size=(number,))
u = np.random.uniform( 0, 1, size=(number,))
# calculate sin(theta) from cos(theta)
theta_sin = np.sqrt(1 - theta_cos**2)
r = radius * np.cbrt(u)
# use list comprehension to generate the coordinate array without a loop
# don't forget to offset by the atom's position (center)
return np.array([
np.array([
center[0] + r[i] * theta_sin[i] * np.cos(phi[i]),
center[1] + r[i] * theta_sin[i] * np.sin(phi[i]),
center[2] + r[i] * theta_cos[i]
]) for i in range(number)
])
I'd like to place 4 points around a point on a sphere (cartesian coordinates: x y z), it doesn't matter how far these 4 points are from the center point (straight line distance or spherical distance) but I'd like these 4 points to be the same distance D from the center point (ideally the 5 points should have a + or x shape, so one north, one south, one east and one south).
I could do it by changing one variable (x, y or z) then keeping another the same and calculating the last variable based on the formula x * x + y * y + z * z = radius * radius but that didn't give good results. I could also maybe use the pythagorean theorem to get the distance between each of the 4 points and the center but I think there is a better formula that I don't know (and couldn't find by doing my research).
Thank you.
Some math
AFAIU your problem is that you have a sphere and a point on the sphere and you want to add 4 more points on the same sphere that would form a kind of a cross on the surface of the sphere around the target point.
I think it is easier to think about this problem in terms of vectors. You have a vector from the center of the sphere to your target point V of size R. All the point lying on the distance d from the target point form another sphere. The crossing of two sphere is a circle. Obviously this circle lies in a plane that is orthogonal to V. Solving a simple system of equations you can find that the distance from the target point to that plane is d^2/(2*R). So the vector from the center of the original sphere to the center of the circle:
Vc = V * (1 - d^2/(2*R^2))
and the radius of that circle is
Rc = sqrt(d^2 - (d^2/(2*R))**2)
So now to select 4 points, you need to select two orthogonal unit vectors lying in that plane D1 and D2. Then 4 points would be Vc + Rc*D1, Vc - Rc*D1, Vc + Rc*D2, and Vc - Rc*D2. To do this you may first select D1 fixing z =0 and switch x and y in Vc
D1 = (Vy/sqrt(Vx^2+Vy^2), -Vx/sqrt(Vx^2+Vy^2), 0)
and then find D2 as a result of cross-product of V and D1. This will work unless unless Vx = Vy = 0 (i.e. V goes along the z-axis) but in that case you can select
D1 = (1,0,0)
D2 = (0,1,0)
Some code
And here is some Python code that implements that math:
def cross_product(v1, v2):
return (v1[1] * v2[2] - v1[2] * v2[1],
v1[2] * v2[0] - v1[0] * v2[2],
v1[0] * v2[1] - v1[1] * v2[0])
def find_marks(sphereCenter, target, d):
lsc = list(sphereCenter)
lt0 = list(target)
lt1 = map(lambda c1, c0: (c1 - c0), lt0, lsc) # shift everything as if sphereCenter is (0,0,0)
rs2 = sum(map(lambda x: x ** 2, lt1)) # spehere radius**2
rs = rs2 ** 0.5
dv = d ** 2 / 2.0 / rs
dvf = d ** 2 / 2.0 / rs2
lcc = map(lambda c: c * (1 - dvf), lt1) # center of the circle in the orthogonal plane
rc = (d ** 2 - dv ** 2) ** 0.5 # orthogonal circle radius
relEps = 0.0001
absEps = relEps * rs
dir1 = (lt1[1], -lt1[0], 0) # select any direction orthogonal to the original vector
dl1 = (lt1[0] ** 2 + lt1[1] ** 2) ** 0.5
# if original vector is (0,0, z) then we've got dir1 = (0,0,0) but we can use (1,0,0) as our vector
if abs(dl1) < absEps:
dir1 = (rc, 0, 0)
dir2 = (0, rc, 0)
else:
dir1 = map(lambda c: rc * c / dl1, dir1)
dir2 = cross_product(lt1, dir1)
dl2 = sum(map(lambda c: c ** 2, dir2)) ** 0.5
dir2 = map(lambda c: rc * c / dl2, dir2)
p1 = map(lambda c0, c1, c2: c0 + c1 + c2, lsc, lcc, dir1)
p2 = map(lambda c0, c1, c2: c0 + c1 + c2, lsc, lcc, dir2)
p3 = map(lambda c0, c1, c2: c0 + c1 - c2, lsc, lcc, dir1)
p4 = map(lambda c0, c1, c2: c0 + c1 - c2, lsc, lcc, dir2)
return [tuple(p1), tuple(p2), tuple(p3), tuple(p4)]
For an extreme case
find_marks((0, 0, 0), (12, 5, 0), 13.0 * 2 ** 0.5)
i.e. for a circle of radius 13 with a center at (0,0,0), the target point lying on the big circle in the plane parallel to the xy-plane and d = sqrt(2)*R, the answer is
[(4.999999999999996, -12.000000000000004, 0.0),
(-5.329070518200751e-15, -2.220446049250313e-15, -13.0),
(-5.000000000000006, 12.0, 0.0),
(-5.329070518200751e-15, -2.220446049250313e-15, 13.0)]
So two points (2-nd and 4-th) are just two z-extremes and the other two are 90° rotations of the target point in the xy-plane which looks quite OK.
For a less extreme example:
find_marks((1, 2, 3), (13, 7, 3), 1)
which is the previous example with d reduced to 1 and with the original center moved to (1,2,3)
[(13.34882784191617, 6.06281317940119, 3.0),
(12.964497041420119, 6.985207100591716, 2.000739918710263),
(12.580166240924067, 7.907601021782242, 3.0),
(12.964497041420119, 6.985207100591716, 3.999260081289737)]
which also looks plausible
If I use matplotlib to generate a delaunay triangulation for a group of points, what is the most appropraite way of getting the circumcentres of the triangles that have been geenrated? I haven't yet managed to find an obvious method in the Triangulation library to do this.
You should be able to calculate it using matplotlib.delaunay.triangulate.Triangulation:
Triangulation(x, y)
x, y -- the coordinates of the points as 1-D arrays of floats
.
.
.
Attributes: (all should be treated as
read-only to maintain consistency)
x, y -- the coordinates of the points as 1-D arrays of floats.
circumcenters -- (ntriangles, 2) array of floats giving the (x,y)
coordinates of the circumcenters of each triangle (indexed by a triangle_id).
Adapted from one of the matplotlib examples (there is probably a cleaner way to do this, but it should work):
import matplotlib.pyplot as plt
import matplotlib.delaunay
import matplotlib.tri as tri
import numpy as np
import math
# Creating a Triangulation without specifying the triangles results in the
# Delaunay triangulation of the points.
# First create the x and y coordinates of the points.
n_angles = 36
n_radii = 8
min_radius = 0.25
radii = np.linspace(min_radius, 0.95, n_radii)
angles = np.linspace(0, 2*math.pi, n_angles, endpoint=False)
angles = np.repeat(angles[...,np.newaxis], n_radii, axis=1)
angles[:,1::2] += math.pi/n_angles
x = (radii*np.cos(angles)).flatten()
y = (radii*np.sin(angles)).flatten()
tt = matplotlib.delaunay.triangulate.Triangulation(x,y)
triang = tri.Triangulation(x, y)
# Plot the triangulation.
plt.figure()
plt.gca().set_aspect('equal')
plt.triplot(triang, 'bo-')
plt.plot(tt.circumcenters[:,0],tt.circumcenters[:,1],'r.')
plt.show()
Here is a function that computes them. It can also be used on other triangulation structures, e.g. scipy's Delaunay triangulation (see below).
def compute_triangle_circumcenters(xy_pts, tri_arr):
"""
Compute the centers of the circumscribing circle of each triangle in a triangulation.
:param np.array xy_pts : points array of shape (n, 2)
:param np.array tri_arr : triangles array of shape (m, 3), each row is a triple of indices in the xy_pts array
:return: circumcenter points array of shape (m, 2)
"""
tri_pts = xy_pts[tri_arr] # (m, 3, 2) - triangles as points (not indices)
# finding the circumcenter (x, y) of a triangle defined by three points:
# (x-x0)**2 + (y-y0)**2 = (x-x1)**2 + (y-y1)**2
# (x-x0)**2 + (y-y0)**2 = (x-x2)**2 + (y-y2)**2
#
# becomes two linear equations (squares are canceled):
# 2(x1-x0)*x + 2(y1-y0)*y = (x1**2 + y1**2) - (x0**2 + y0**2)
# 2(x2-x0)*x + 2(y2-y0)*y = (x2**2 + y2**2) - (x0**2 + y0**2)
a = 2 * (tri_pts[:, 1, 0] - tri_pts[:, 0, 0])
b = 2 * (tri_pts[:, 1, 1] - tri_pts[:, 0, 1])
c = 2 * (tri_pts[:, 2, 0] - tri_pts[:, 0, 0])
d = 2 * (tri_pts[:, 2, 1] - tri_pts[:, 0, 1])
v1 = (tri_pts[:, 1, 0] ** 2 + tri_pts[:, 1, 1] ** 2) - (tri_pts[:, 0, 0] ** 2 + tri_pts[:, 0, 1] ** 2)
v2 = (tri_pts[:, 2, 0] ** 2 + tri_pts[:, 2, 1] ** 2) - (tri_pts[:, 0, 0] ** 2 + tri_pts[:, 0, 1] ** 2)
# solve 2x2 system (see https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices)
det = (a * d - b * c)
detx = (v1 * d - v2 * b)
dety = (a * v2 - c * v1)
x = detx / det
y = dety / det
return (np.vstack((x, y))).T
On the data from #JoshAdel's answer above, adding the following code:
cc = compute_triangle_circumcenters(np.vstack([tt.x, tt.y]).T, tt.triangle_nodes)
plt.plot(cc[:, 0], cc[:, 1], ".k")
I get the following figure:
It can also be used on scipy.spatial.Delaunay like this:
from scipy.spatial import Delaunay
xy_pts = np.vstack([x, y]).T
dt = Delaunay(xy_pts)
cc = compute_triangle_circumcenters(dt.points, dt.simplices)