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.

Categories

Resources