| 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): |
| |
| dot = np.dot(q1, q2) |
|
|
| |
| if dot < 0.0: |
| q1 = -q1 |
| dot = -dot |
|
|
| |
| dot = np.clip(dot, -1.0, 1.0) |
|
|
| |
| theta = np.arccos(dot) * t |
| q3 = q2 - q1 * dot |
| q3 = q3 / np.linalg.norm(q3) |
|
|
| |
| 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 |
|
|
