How to visualize colmap export [images.txt] in blender? - python
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]
Related
I want to have the pendulum blob in my double pendulum
In this code I want to have animation something like this. But I dont want the other pendulums that come into picture later. Just the initial one. Currently this is my output. This is the image after the animation completes. In the animation, I want to have a ball(blob) which plots the red lines and another one which plots the green lines. import numpy as np from numpy import cos, sin, arange, pi import matplotlib.pyplot as plt import matplotlib.animation as animation h = 0.0002 #the change in runge kutta figsize = 6 dpi = 1000 N = 200000 # iterations L1=1 #length 1 L2=1.5 #lenth 2 m1=50 #mass of bob 1 m2=1 #mass of bob2 g = 9.81#gravity theta_01 = (np.pi/180)*90 theta_02 = (np.pi/180)*60 w_1 = 0 w_2 = 0 # dw/dt function oft theta 1 def funcdwdt1(theta1,theta2,w1,w2): cos12 = cos(theta1 - theta2)#for wrirting the main equation in less complex manner sin12 = sin(theta1 - theta2) sin1 = sin(theta1) sin2 = sin(theta2) denom = cos12**2*m2 - m1 - m2 ans = ( L1*m2*cos12*sin12*w1**2 + L2*m2*sin12*w2**2 - m2*g*cos12*sin2 + (m1 + m2)*g*sin1)/(L1*denom) return ans # dw/dt function oft thetas 2 def funcdwdt2(theta2,theta1,w1,w2): cos12 = cos(theta1 - theta2) sin12 = sin(theta1 - theta2) sin1 = sin(theta1) sin2 = sin(theta2) denom = cos12**2*m2 - m1 - m2 ans2 = -( L2*m2*cos12*sin12*w2**2 + L1*(m1 + m2)*sin12*w1**2 + (m1 + m2)*g*sin1*cos12 - (m1 + m2)*g*sin2 )/(L2*denom) return ans2 # d0/dt function for theta 1 def funcd0dt1(w0): return w0 # d0/dt function for theta 2 def funcd0dt2(w0): return w0 X1= [] X2= [] Y1= [] Y2= [] def func(w1,w2, theta1,theta2): for i in range(N): k1a = h * funcd0dt1(w1) # gives theta1 k1b = h * funcdwdt1(theta1,theta2,w1,w2) # gives omega1 k1c = h * funcd0dt2(w2) # gives theta2 k1d = h * funcdwdt2(theta2,theta1,w1,w2) # gives omega2 k2a = h * funcd0dt1(w1 + (0.5 * k1b)) k2b = h * funcdwdt1(theta1 + (0.5 * k1a),theta2,w1,w2) k2c = h * funcd0dt2(w2 + (0.5 * k1d)) k2d = h * funcdwdt2(theta2 + (0.5 * k1c),theta1,w1,w2) k3a = h * funcd0dt1(w1 + (0.5 * k2b)) k3b = h * funcdwdt1(theta1 + (0.5 * k2a),theta2,w1,w2) k3c = h * funcd0dt2(w2 + (0.5 * k2d)) k3d = h * funcdwdt2(theta2 + (0.5 * k2c),theta1,w1,w2) k4a = h * funcd0dt1(w1 + k3b) k4b = h * funcdwdt1(theta1 + k3a,theta2,w1,w2) k4c = h * funcd0dt2(w2 + k3d) k4d = h * funcdwdt2(theta2 + k3c,theta1,w1,w2) #addidng the vakue aftyer the iterartions theta1 += 1 / 6 * (k1a + 2 * k2a + 2 * k3a + k4a) w1 +=1 / 6 * (k1b + 2 * k2b + 2 * k3b + k4b) theta2 += + 1 / 6 * (k1c + 2 * k2c + 2 * k3c + k4c) w2 += 1 / 6 * (k1d + 2 * k2d + 2 * k3d + k4d) x1 = L1 * sin(theta1) y1 = -L1 * cos(theta1) x2 = x1 + L2 * sin(theta2) y2 = y1 - L2 * cos(theta2) X1.append(x1) X2.append(x2) Y1.append(y1) Y2.append(y2) return x1,y1,x2,y2 print(func(w_1, w_2, theta_01, theta_02)) fig, ax = plt.subplots() l1, = ax.plot([], []) l2, = ax.plot([],[]) ax.set(xlim=(-3, 3), ylim=(-2,2)) def animate(i): l1.set_data(X1[:i], Y2[:i]) l2.set_data(X2[:i], Y2[:i]) return l1,l2, ani = animation.FuncAnimation(fig, animate, interval = 5, frames=len(X1)) # plt.show() ani.save('save.mp4', writer='ffmpeg')
Just add another line l3, = ax.plot([],[], '-ob', lw=2, ms=8) and in the animate function set its values to l3.set_data([0,X1[i],X2[i]], [0,Y1[i],Y2[i]]) Adapt line-width and marker-size as necessary. This should draw filled circles at the pendulum positions and the origin with lines connecting them. You should use Y1 in the l1 data. With a total pendulum length of 2.5, the vertical limits are too small. It is sufficient to use h = 0.005 #the change in runge kutta N = 5000 # iterations to get an animation with realistic speed. Or combine several RK4 steps for each frame. For minimum error you can use h=1e-3, smaller step sizes only lead to the accumulation of floating point errors dominating the method error.
Projectile with air resistance find time when y = 0
I have to find the time when y = 0. I have this formula y(t) = m/k(v0sinθ + mkg)(1−e^[−k/m * t])−m/k * (gt) I isolated the t like that t = [m/k(v0sinθ + mkg)(1−e^[−k/m * t]) * k] / gm I already did something similar for x=a(1− e^−x). Right now I don't see what I'm doing wrong. m = 0.5 k = 1 v0 = 20 theta = 25 g = 9.8 i = 1 a = ((m/k)*(v0 * np.sin(np.deg2rad(theta)) + (m/k) * g)) tolerance = 0.001 t0 = a t1 = a * (1 - exp(- (k/m) * a)) * k / (m*g) error = t0 - t1 while error > tolerance: t0 = t1 t1 = a * (1 - exp(- (k/m) * t1)) * k / (m*g) error = t0 - t1 i+=1
multiple arrays plot from for loop
I am new here (newbie) so the question is maybe obvious. I have coded a membrane shape. Now I variate the initial angle from 0 to 180 degrees with a interval (1 degrees) and plot the membranes on the same plot with the nested loop. However if i plot the membranes with various initial angle with the first loop, the membrane shows a different shape than with the nested loop. The first loop shows the correct form and the nested loop does something I dont want. I can't figure out in the code how this come. import numpy as np import matplotlib.pyplot as plt import math rho = 1000 g = 9.81 H_up = 5.15 # H - 2.7 # H = 7.85 #Water depth H_down = 2.15 # D - 2.7 # D = 4.85 h_d = 5.3 p = h_d * rho * g T = h_d/2 * p alfa = h_d / (H_up-H_down) T = 1/2 * alfa * rho * g * h_d**2 phi_0 = math.acos(rho * g / 2 * (H_up ** 2 - H_down ** 2) / T - 1) def geometry(p, phi): rho = 1000 g = 9.81 H_up = 5.15 # H - 2.7 H_down = 2.15 # D - 2.7 h_d = 5.3 # [m] gate height alfa = h_d / (H_up-H_down)#1.5 # 1*10**10 T = 1/2 * alfa * rho * g * h_d**2 #50000 # [N/m] 1/2 * rho * g * (H - D)**2 / (1 + np.cos(phi1)) # h_d/2 * h_d * rho * g t = 1.6 * 10 ** -2 # [m] thickness sheet sigma = T / t #[N/m^2] E = 3800 * 10**3 /t #3200 / t * 10 ** 3 # [N/m**2] 900*10**6 epsilon = sigma / E f = (1 + epsilon) dphi = - f * p / T dx = f * np.cos(phi) dy = f * np.sin(phi) return (dphi, dx, dy) steps = 100 dS = 30./steps phi = np.zeros(steps) x = np.zeros(steps) y = np.zeros(steps) p = np.zeros(steps) case = 1 for i in range (len(y)-1): phi[0] = phi_0 #geometry(p[i-1], phi[i])[3] phi[i] = phi[i-1] + geometry(p[i-1], 0)[0] * dS x[i] = x[i - 1] + geometry(p[i-1], phi[i])[1] * dS x[i+1] = x[i] y[i] = y[i - 1] + geometry(p[i-1], phi[i])[2] * dS y[i+1] = y[i] if y[i] > H_up and case == 1: case = 2 if y[i] < H_down and case == 2: case = 3 if y[i] < 0 and case == 3: break if case == 1: p[i] = (H_up - (H_up - y[i])) * rho * g if case == 2: p[i] = H_up * rho * g if case == 3: p[i] = (H_down - y[i]) * rho * g plt.plot(x,y) plt.xlabel('x [m]') plt.ylabel('y [m]') plt.show() phi1 = np.linspace(0, np.pi, 180) phi = np.zeros(steps) x = np.zeros(steps) y = np.zeros(steps) p = np.zeros(steps) case = 1 for j in range (len(phi1)): phi2 = phi1[j] for i in range (len(y)-1): phi[0] = phi2 #geometry(p[i-1], phi[i])[3] phi[i] = phi[i-1] + geometry(p[i-1], phi[i])[0] * dS x[i] = x[i - 1] + geometry(p[i-1], phi[i])[1] * dS y[i] = y[i - 1] + geometry(p[i-1], phi[i])[2] * dS if y[i - 1] > H_up and case == 1: case = 2 if y[i - 1] < H_down and case == 2: case = 3 if y[i - 1] < 0 and case == 3 and max(y) > 10: plt.plot(x, y) phi = np.zeros(steps) x = np.zeros(steps) y = np.zeros(steps) p = np.zeros(steps) break if case == 1: p[i] = (H_up - (H_up - y[i])) * rho * g if case == 2: p[i] = H_up * rho * g # upstream waterlevel - bottom # 40000 # [N/m^2] if case == 3: p[i] = (H_down - y[i]) * rho * g plt.show()
Rotation of arbitrary plane around a given axis results in inconsistent results
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...
multithreaded mandelbrot set
Is it possible to change the formula of the mandelbrot set (which is f(z) = z^2 + c by default) to a different one ( f(z) = z^2 + c * e^(-z) is what i need) when using the escape time algorithm and if possible how? I'm currently using this code by FB36 # Multi-threaded Mandelbrot Fractal (Do not run using IDLE!) # FB - 201104306 import threading from PIL import Image w = 512 # image width h = 512 # image height image = Image.new("RGB", (w, h)) wh = w * h maxIt = 256 # max number of iterations allowed # drawing region (xa < xb & ya < yb) xa = -2.0 xb = 1.0 ya = -1.5 yb = 1.5 xd = xb - xa yd = yb - ya numThr = 5 # number of threads to run # lock = threading.Lock() class ManFrThread(threading.Thread): def __init__ (self, k): self.k = k threading.Thread.__init__(self) def run(self): # each thread only calculates its own share of pixels for i in range(k, wh, numThr): kx = i % w ky = int(i / w) a = xa + xd * kx / (w - 1.0) b = ya + yd * ky / (h - 1.0) x = a y = b for kc in range(maxIt): x0 = x * x - y * y + a y = 2.0 * x * y + b x = x0 if x * x + y * y > 4: # various color palettes can be created here red = (kc % 8) * 32 green = (16 - kc % 16) * 16 blue = (kc % 16) * 16 # lock.acquire() global image image.putpixel((kx, ky), (red, green, blue)) # lock.release() break if __name__ == "__main__": tArr = [] for k in range(numThr): # create all threads tArr.append(ManFrThread(k)) for k in range(numThr): # start all threads tArr[k].start() for k in range(numThr): # wait until all threads finished tArr[k].join() image.save("MandelbrotFractal.png", "PNG")
From the code I infer that z = x + y * i and c = a + b * i. That corresponds f(z) - z ^2 + c. You want f(z) = z ^2 + c * e^(-z). Recall that e^(-z) = e^-(x + yi) = e^(-x) * e^i(-y) = e^(-x)(cos(y) - i*sin(y)) = e^(-x)cos(y) - i (e^(-x)sin(y)). Thus you should update your lines to be the following: x0 = x * x - y * y + a * exp(-x) * cos(y) + b * exp(-x) * sin(y); y = 2.0 * x * y + a * exp(-x) * sin(y) - b * exp(-x) * cos(y) x = x0 You might need to adjust maxIt if you don't get the level of feature differentiation you're after (it might take more or fewer iterations to escape now, on average) but this should be the mathematical expression you're after. As pointed out in the comments, you might need to adjust the criterion itself and not just the maximum iterations in order to get the desired level of differentiation: changing the max doesn't help for ones that never escape. You can try deriving a good escape condition or just try out some things and see what you get.