I am trying to create a matrix with 3 columns and p rows that contains p rows of x, y and z values, later i transpose this matrix and go on. The problem is I do not know how to create this matrix. Any tips?enter code here
time=np.arange(0,100,1)
fphi = 2 #kampo phi daznis
ftheta = 3 #kampo i daznis
Amp = np.pi/2
phi = ()
theta = ()
print(time)
points = []
for p in time:
phi = 2*np.pi*fphi*p
theta = Amp*np.sin(2*np.pi*ftheta*p)
x = r * np.cos(phi)
y = r * np.sin(phi) * np.cos(theta) - h *
np.sin(theta)
z = r * np.sin(phi) * np.sin(theta) + h *
np.cos(theta)
points = np.vstack((points, [x, y, z]))
import numpy as np
r=10
h=8
time=np.arange(0,100,1)
fphi = 2 #kampo phi daznis
ftheta = 3 #kampo i daznis
Amp = np.pi/2
phi = ()
theta = ()
print(time)
points = []
for p in time:
phi = 2*np.pi*fphi*p
theta = Amp*np.sin(2*np.pi*ftheta*p)
x = r * np.cos(phi)
y = r * np.sin(phi) * np.cos(theta) - h * np.sin(theta)
z = r * np.sin(phi) * np.sin(theta) + h * np.cos(theta)
points.append([x,y,z])
result_matrix = np.matrix(points)
U can use np.matrix() with a 2-d nested list as parameter
Related
I am trying to rotate a triad of orthogonal axis, which origin is located on the surface of a sphere. The aim would be to rotate them in order to pass from Cartesian coordinates, to radial and transverse component of the sphere. I tried to find a python function but I haven't find anything good for my case so I built it by myself.
First I computed the three angles:
#Center of the sphere
xc = 7500
yc = 7500
zc = 8960
#File where the coordinates are located
data = pd.read_csv('file_test', header = None,delimiter =' ')
xa = np.array(data.iloc[1:,1])
ya = np.array(data.iloc[1:,2])
za = np.array(data.iloc[1:,0])
#Angles definition
theta = np.arctan2((ya-yc),(xa-xc))
phi = np.arctan2((za-zc),(xa-xc))
gamma = np.arctan2((za-zc),(ya-yc))
Then I rotate the vector x,y,z in this way:
def rotate_receivers(x, y, z, a, b, c):
x1 = x * np.cos(b) * np.cos(c) - y * (np.sin(c) * np.cos(b)) + z * (np.sin(b))
y1= x * (np.sin(a) * np.sin(b) *np.cos(c) + np.cos(a) * np.sin(c)) + y * ( - np.sin(b) * np.sin(a) * np.sin(c) + np.cos(c) * np.cos(a)) - z * (np.sin(a) * np.cos(b))
z1 = x * ( - np.cos(a) * np.sin(b) * np.cos(c) + np.sin(a) * np.sin(c)) + y * ( np.sin(b) * np.sin(c) * np.cos(a) + np.sin(a) * np.cos(c)) + z * np.cos(a) * np.cos(b)
return x1, y1, z1
Now I am expecting to rotate them in this way:
x1,y1,z1 = rotate_receivers(x[i],y[i],z[i],gamma[i],phi[i],theta[i])
Now the rotation works only for some angles and I don't understand why. It has to be something wrong maybe in the way I defined the angles but I don't understand what.
Thanks!
I have a colmap export of the camera pose file which named "images.txt".
# Image list with two lines of data per image:
# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME
# POINTS2D[] as (X, Y, POINT3D_ID)
# Number of images: 2, mean observations per image: 2
1 0.851773 0.0165051 0.503764 -0.142941 -0.737434 1.02973 3.74354 1 P1180141.JPG
2362.39 248.498 58396 1784.7 268.254 59027 1784.7 268.254 -1
2 0.851773 0.0165051 0.503764 -0.142941 -0.737434 1.02973 3.74354 1 P1180142.JPG
1190.83 663.957 23056 1258.77 640.354 59070
Then I want to import it into Blender.
So I wrote the following code.
First I read the text, and split it to quaternion and translation.
And then calculate optical center use -R^T*t, and camera rotation use R^T
colmap_pose = r"/Users/chunibyo/Desktop/images.txt"
rc_pose = r'flight_log.csv'
image_list_file = r'select.bat'
pose_list_file = r'blender.py'
import math
import numpy as np
import open3d as o3d
def euler_from_quaternion(x, y, z, w):
"""
Convert a quaternion into euler angles (roll, pitch, yaw)
roll is rotation around x in radians (counterclockwise)
pitch is rotation around y in radians (counterclockwise)
yaw is rotation around z in radians (counterclockwise)
"""
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll_x = math.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch_y = math.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw_z = math.atan2(t3, t4)
return roll_x, pitch_y, yaw_z # in radians
def quaternion_rotation_matrix(q0, q1, q2, q3):
# w, x, y ,z
"""
Covert a quaternion into a full three-dimensional rotation matrix.
Input
:param Q: A 4 element array representing the quaternion (q0,q1,q2,q3)
Output
:return: A 3x3 element matrix representing the full 3D rotation matrix.
This rotation matrix converts a point in the local reference
frame to a point in the global reference frame.
"""
# Extract the values from Q
# First row of the rotation matrix
r00 = 2 * (q0 * q0 + q1 * q1) - 1
r01 = 2 * (q1 * q2 - q0 * q3)
r02 = 2 * (q1 * q3 + q0 * q2)
# Second row of the rotation matrix
r10 = 2 * (q1 * q2 + q0 * q3)
r11 = 2 * (q0 * q0 + q2 * q2) - 1
r12 = 2 * (q2 * q3 - q0 * q1)
# Third row of the rotation matrix
r20 = 2 * (q1 * q3 - q0 * q2)
r21 = 2 * (q2 * q3 + q0 * q1)
r22 = 2 * (q0 * q0 + q3 * q3) - 1
# 3x3 rotation matrix
rot_matrix = np.array([[r00, r01, r02],
[r10, r11, r12],
[r20, r21, r22]])
return rot_matrix
def rotmat2qvec(R):
Rxx, Ryx, Rzx, Rxy, Ryy, Rzy, Rxz, Ryz, Rzz = R.flat
K = np.array([
[Rxx - Ryy - Rzz, 0, 0, 0],
[Ryx + Rxy, Ryy - Rxx - Rzz, 0, 0],
[Rzx + Rxz, Rzy + Ryz, Rzz - Rxx - Ryy, 0],
[Ryz - Rzy, Rzx - Rxz, Rxy - Ryx, Rxx + Ryy + Rzz]]) / 3.0
eigvals, eigvecs = np.linalg.eigh(K)
qvec = eigvecs[[3, 0, 1, 2], np.argmax(eigvals)]
if qvec[0] < 0:
qvec *= -1
return qvec
def qvec2rotmat(qvec):
# w, x, y, z
return np.array([
[1 - 2 * qvec[2] ** 2 - 2 * qvec[3] ** 2,
2 * qvec[1] * qvec[2] - 2 * qvec[0] * qvec[3],
2 * qvec[3] * qvec[1] + 2 * qvec[0] * qvec[2]],
[2 * qvec[1] * qvec[2] + 2 * qvec[0] * qvec[3],
1 - 2 * qvec[1] ** 2 - 2 * qvec[3] ** 2,
2 * qvec[2] * qvec[3] - 2 * qvec[0] * qvec[1]],
[2 * qvec[3] * qvec[1] - 2 * qvec[0] * qvec[2],
2 * qvec[2] * qvec[3] + 2 * qvec[0] * qvec[1],
1 - 2 * qvec[1] ** 2 - 2 * qvec[2] ** 2]])
def main():
with open(colmap_pose) as f:
lines = f.readlines()
rc_lines = []
image_list = []
pose_list = []
point_cloud = []
accuracy = 10
for index, temp_line in enumerate(lines):
line = temp_line.strip()
if not line.startswith('#') and (line.endswith('.png') or line.endswith('jpg')):
temp = line.split(' ')
qw = float(temp[1])
qx = float(temp[2])
qy = float(temp[3])
qz = float(temp[4])
tx = float(temp[5])
ty = float(temp[6])
tz = float(temp[7])
name = temp[9]
rotation_matrix = qvec2rotmat([qw, qx, qy, qz])
optical_center = -rotation_matrix.T # np.array([tx, ty, tz])
tx = float(optical_center[0])
ty = float(optical_center[1])
tz = float(optical_center[2])
_qw, _qx, _qy, _qz = rotmat2qvec(rotation_matrix.T)
roll_x, pitch_y, yaw_z = euler_from_quaternion(_qx, _qy, _qz, _qw)
rc_lines.append(
f"{name},{tx},{ty},{tz},{accuracy},{accuracy},{accuracy},{math.degrees(yaw_z)},{math.degrees(pitch_y)},{math.degrees(roll_x)},{accuracy},{accuracy},{accuracy}\n")
image_list.append(f"xcopy {name} select\n")
# f"camera_object{index}.rotation_euler = Euler(({yaw_z}, {pitch_y}, {roll_x}), 'ZYX')\n" \
# f"camera_object{index}.rotation_quaternion = Quaternion(({qw}, {qx}, {qy}, {qz}))\n" \
blender_script = f"camera_data{index} = bpy.data.cameras.new(name='{name}')\n" \
f"camera_object{index} = bpy.data.objects.new('{name}', camera_data{index})\n" \
f"bpy.context.scene.collection.objects.link(camera_object{index})\n" \
f"camera_object{index}.location = ({tx}, {ty}, {tz})\n" \
f"camera_object{index}.rotation_euler = Euler(({yaw_z}, {pitch_y}, {roll_x}), 'ZYX')\n" \
f"camera_object{index}.rotation_mode = 'ZYX'\n" \
f"bpy.data.cameras['{name}'].lens = 30\n\n"
pose_list.append(blender_script)
point_cloud.append([tx, ty, tz])
# selected image
with open(image_list_file, 'w') as f:
f.write('#echo off\nmkdir select\n')
f.writelines(image_list)
# blender
with open(pose_list_file, 'w') as f:
f.write('import bpy\nfrom mathutils import Euler, Quaternion\n\n')
f.writelines(pose_list)
# ply
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.array(point_cloud))
o3d.io.write_point_cloud("pcl.ply", pcd, write_ascii=True)
if __name__ == '__main__':
main()
But I got the wrong rotation of the cameraļ¼the original pose and the imported pose are shown in the figure.
Could you give me some help, please?
After loading R and T try this and use quaternions for camera direction.
w2c = np.eye(4)
w2c[:3,:3] = R
w2c[:3,3] = T
c2w = np.linalg.inv(w2c)
# quaternions
q = Rotation.from_matrix(c2w[:3, :3).as_quat()
# translation
t = c2w[:3, 3]
I am trying to curve fit the following equation with parameters d, D, Ar, Tr each of them bounded in some range. The physical constants are: gamma = 26.76E7, n = 6.59E28, Ad = 2.099E-20
The equation is broken into several parts.
#Fitting function
def func(x, d, D, Ar, Tr):
pi = math.pi
#define the variables
w1 = 2 * pi * x
z1 = (2 * w1 * d**2 / D)**0.5
w2 = 2 *w1
z2 = ( 2 * w2 * d**2 / D)**0.5
J1 = (
(1 + 5 * z1 / 8 + z1**2 / 8) /
( 1 + z1 + z1**2 / 2 + z1**3 / 6 + z1**5 / 81 + z1**6 / 648)
)
J2 = (
(1 + 5 * z2 / 8 + z2**2 / 8) /
( 1 + z2 + z2**2 / 2 + z2**3 / 6 + z2**5 / 81 + z2**6 / 648)
)
gamma = 26.76E7 #unit = 1/(T*s)
n = 6.59E28
#define the normalization constant A_d
Ad = (8 / 45) * pi * gamma**4* sc.hbar**2 * (
sc.mu_0 / (4 * pi)
)**2 * n
R1_diff = Ad * (J1 + 4 * J2 ) / (d * D )
R1_rot = Ar * (Tr / (1 + w1**2 * Tr**2) + 4 * Tr / (
1 + w2**2 * Tr**2)
)
R1_IL = R1_diff + R1_rot
return R1_IL
This is the x and y data sets -
#Experimental x and y data points
xData = np.array([
2.00E+07
1.42E+07
1.01E+07
7.16E+06
7.16E+06
5.09E+06
3.61E+06
2.57E+06
1.82E+06
1.29E+06
9.20E+05
6.53E+05
4.63E+05
3.29E+05
2.34E+05
1.66E+05
1.18E+05
8.39E+04
5.96E+04
4.24E+04
3.00E+04])
yData = np.array([
7.89E+00
8.79E+00
9.58E+00
1.02E+01
1.03E+01
1.08E+01
1.13E+01
1.17E+01
1.20E+01
1.23E+01
1.25E+01
1.28E+01
1.29E+01
1.30E+01
1.31E+01
1.31E+01
1.35E+01
1.31E+01
1.33E+01
1.35E+01
1.34E+01])
The range for the parameters are: d = [3.00E-10, 4.00E-10], D = [12.5E-12, 14.00E-12], Ar = [3.00E9, 4.00E9], Tr = [1.00E-10, 2.5E-10]
This is the code that I wrote which is not giving me the correct fit and the right parameter values.
popt, pcov = curve_fit(func, xData, yData, bounds = ([3.00E-10,12.5E-12,2.5E9,1.00E-10],[4.0E-10,14.0E-12,4.00E9,2.00E-10]))
print(popt)
#x values for the fitted function
xFit = np.arange(3.00E+04, 2.00E+07, 10.00)
#Plot the fitted function
plt.loglog(xFit, func(xFit, *popt), 'r') #label='fit params: d=%5.3f, D=%5.3f, Ar=%5.3f, Tr=%5.3f' % tuple(popt))
#Plot experimental data points
plt.loglog(xData, yData, 'bo') #label='experimental-data')
plt.xlabel('f1')
plt.ylabel('R1')
plt.legend()
plt.show()
Anything would be really appreciated. The curve_fit is way off when I run this code.
For information only :
The power function below appears to be well fitted to the data.
I am trying to rotate and translate arbitrary planes around some arbitrary axis.
For testing purposes I have written a simple python program that rotates a random plane around the X axis in degrees.
Unfortunately when checking the angle between the planes I get inconsistent results. This is the code:
def angle_between_planes(plane1, plane2):
plane1 = (plane1 / np.linalg.norm(plane1))[:3]
plane2 = (plane2/ np.linalg.norm(plane2))[:3]
cos_a = np.dot(plane1.T, plane2) / (np.linalg.norm(plane1) * np.linalg.norm(plane2))
print(np.arccos(cos_a)[0, 0])
def test():
axis = np.array([1, 0, 0])
theta = np.pi / 2
translation = np.array([0, 0, 0])
T = get_transformation(translation, axis * theta)
for i in range(1, 10):
source = np.append(np.random.randint(1, 20, size=3), 0).reshape(4, 1)
target = np.dot(T, source)
angle_between_planes(source, target)
It prints:
1.21297144225
1.1614420953
1.48042948278
1.10098697889
0.992418096794
1.16954303911
1.04180591409
1.08015300394
1.51949177153
When debugging this code I see that the transformation matrix is correct, as it shows that it is
I'm not sure what's wrong and would love any assistance here.
*
The code that generates the transformation matrix is:
def get_transformation(translation_vec, rotation_vec):
r_4 = np.array([0, 0, 0, 1]).reshape(1, 4)
rotation_vec= rotation_vec.reshape(3, 1)
theta = np.linalg.norm(rotation_vec)
axis = rotation_vec/ theta
R = get_rotation_mat_from_axis_and_angle(axis, theta)
T = translation_vec.reshape(3, 1)
R_T = np.append(R, T, axis = 1)
return np.append(R_T, r_4, axis=0)
def get_rotation_mat_from_axis_and_angle(axis, theta):
axis = axis / np.linalg.norm(axis)
a, b, c = axis
omct = 1 - np.cos(theta)
ct = np.cos(theta)
st = np.sin(theta)
rotation_matrix = np.array([a * a * omct + ct, a * b * omct - c * st, a * c * omct + b * st,
a * b * omct + c * st, b * b * omct + ct, b * c * omct - a * st,
a * c * omct - b * st, b * c * omct + a * st, c * c * omct + ct]).reshape(3, 3)
rotation_matrix[abs(rotation_matrix) < 1e-8] = 0
return rotation_matrix
The source you generate is not a vector. In order to be one, it should have its fourth coordinate equal to zero.
You could generate valid ones with:
source = np.append(np.random.randint(1, 20, size=3), 0).reshape(4, 1)
Note that your code can't be tested as you pasted it in your question: for example, vec = vec.reshape(3, 1) in get_transformation uses vec that hasn't been defined anywhere before...
I am trying to crate a program that randomly generates line segments using parametric equations. What I have created kinds of does the job, but instead of the lines being disconnected from one another they form one continues line. This is what I have written in python.
enter import numpy as np
import random as rand
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.set_aspect("equal")
npoints = 10
V = np.zeros(npoints)
def point1 (npoints):
x0 = np.zeros(npoints)
y0 = np.zeros(npoints)
z0 = np.zeros(npoints)
for k in range (npoints):
theta = rand.uniform(0.0, np.pi)
phi = rand.uniform(0.0, (2 * np.pi))
x0[k] = 10 * np.sin(phi) * np.cos(theta)
y0[k] = 10 * np.sin(phi) * np.sin(theta)
z0[k] = 10 * np.cos(theta)
return np.array([x0,y0,z0])
def point2 (npoints):
x1 = np.zeros(npoints)
y1 = np.zeros(npoints)
z1 = np.zeros(npoints)
for j in range (npoints):
theta = rand.uniform(0.0, np.pi)
phi = rand.uniform(0.0, (2 * np.pi))
x1[j] = 10 * np.sin(phi) * np.cos(theta)
y1[j] = 10 * np.sin(phi) * np.sin(theta)
z1[j] = 10 * np.cos(theta)
return np.array([x1,y1,z1])
n = 10
def t_parameter(n):
t = np.zeros(n)
for i in range (n):
t[i] = rand.uniform(-10,10)
return np.array([t])
p1 = point1(npoints)
p2 = point2(npoints)
V = p2-p1
d = t_paramiter(n)
Lx = d*V[0]+p1[0]
Ly = d*V[1]+p1[1]
Lz = d*V[2]+p1[2]
ax.plot_wireframe(Lx,Ly,Lz)
When I run the code this is what is generated plot of what is generated. What I would like to code to do is keep the values of the initial point and direction vector constant while just updating the d with random values.
I have tried doing something like this
Lx = np.zeros(npoints)
Ly = np.zeros(npoints)
Lz = np.zeros(npoints)
for i in range (n):
Lx[i] = d[i]*V[i]+p1[i]
Ly[i] = d[i]*V[i]+p1[i]
Lz[i] = d[i]*V[i]+p1[i]
but I get an error "setting an array element with a sequence".