File size: 2,215 Bytes
ea66a99 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 | import numpy as np
from scipy.spatial.transform import Rotation as R
from scene.utils import Camera
from copy import deepcopy
def rotation_matrix_to_quaternion(rotation_matrix):
return R.from_matrix(rotation_matrix).as_quat()
def quaternion_to_rotation_matrix(quat):
return R.from_quat(quat).as_matrix()
def quaternion_slerp(q1, q2, t):
# Compute the quaternion dot product.
dot = np.dot(q1, q2)
# Flip one quaternion when needed to follow the shortest interpolation path.
if dot < 0.0:
q1 = -q1
dot = -dot
# Clamp numerical error before applying arccos.
dot = np.clip(dot, -1.0, 1.0)
# Compute the interpolation angle.
theta = np.arccos(dot) * t
q3 = q2 - q1 * dot
q3 = q3 / np.linalg.norm(q3)
# Compute the interpolated quaternion.
return np.cos(theta) * q1 + np.sin(theta) * q3
def bezier_interpolation(p1, p2, t):
return (1 - t) * p1 + t * p2
def linear_interpolation(v1, v2, t):
return (1 - t) * v1 + t * v2
def smooth_camera_poses(cameras, num_interpolations=5):
smoothed_cameras = []
smoothed_times = []
total_poses = len(cameras) - 1 + (len(cameras) - 1) * num_interpolations
time_increment = 10 / total_poses
for i in range(len(cameras) - 1):
cam1 = cameras[i]
cam2 = cameras[i + 1]
quat1 = rotation_matrix_to_quaternion(cam1.orientation)
quat2 = rotation_matrix_to_quaternion(cam2.orientation)
for j in range(num_interpolations + 1):
t = j / (num_interpolations + 1)
interp_orientation_quat = quaternion_slerp(quat1, quat2, t)
interp_orientation_matrix = quaternion_to_rotation_matrix(interp_orientation_quat)
interp_position = linear_interpolation(cam1.position, cam2.position, t)
interp_time = i*10 / (len(cameras) - 1) + time_increment * j
newcam = deepcopy(cam1)
newcam.orientation = interp_orientation_matrix
newcam.position = interp_position
smoothed_cameras.append(newcam)
smoothed_times.append(interp_time)
smoothed_cameras.append(cameras[-1])
smoothed_times.append(1.0)
return smoothed_cameras, smoothed_times
|