82 lines
1.6 KiB
Python
82 lines
1.6 KiB
Python
|
""""
|
||
|
|
Visualise the camera position for a non-json format dataset
|
||
|
|
"""
|
||
|
|
|
||
|
|
|
||
|
|
import numpy as np
|
||
|
|
import open3d as o3d
|
||
|
|
|
||
|
|
from camera_visualizer.visualize_cameras import (
|
||
|
|
frustums2lineset,
|
||
|
|
get_camera_frustum,
|
||
|
|
)
|
||
|
|
|
||
|
|
|
||
|
|
def format_str_to_array(input_str):
|
||
|
|
|
||
|
|
output = input_str.split(" ")
|
||
|
|
output = np.array(list(map(float, output)))
|
||
|
|
output = output.reshape((4, 4))
|
||
|
|
|
||
|
|
return output
|
||
|
|
|
||
|
|
|
||
|
|
# Paramerers
|
||
|
|
basepath = "/home/user/Downloads/lf_data/ship/camera_path"
|
||
|
|
number_of_pose = 199
|
||
|
|
img_size = [1008, 548]
|
||
|
|
camera_size = 0.1
|
||
|
|
camera_color = (0, 0.5, 1)
|
||
|
|
sphere_radius = 1.0
|
||
|
|
|
||
|
|
# Get instrisics
|
||
|
|
fd = open(basepath + "/intrinsics/000000.txt", "r")
|
||
|
|
K = fd.read()
|
||
|
|
fd.close()
|
||
|
|
|
||
|
|
# Format instrisics
|
||
|
|
K = format_str_to_array(K)
|
||
|
|
# print("K = ", K)
|
||
|
|
|
||
|
|
# Get pose
|
||
|
|
|
||
|
|
list_of_poses = []
|
||
|
|
|
||
|
|
for k in range(number_of_pose):
|
||
|
|
fd = open(basepath + f"/pose/{k:06}.txt", "r")
|
||
|
|
W2C = fd.read()
|
||
|
|
fd.close()
|
||
|
|
|
||
|
|
W2C = format_str_to_array(W2C)
|
||
|
|
C2W = np.linalg.inv(W2C)
|
||
|
|
list_of_poses.append(C2W)
|
||
|
|
|
||
|
|
# print("W2C = ", W2C)
|
||
|
|
|
||
|
|
|
||
|
|
# Draw everything
|
||
|
|
sphere = o3d.geometry.TriangleMesh.create_sphere(
|
||
|
|
radius=sphere_radius, resolution=25
|
||
|
|
)
|
||
|
|
sphere = o3d.geometry.LineSet.create_from_triangle_mesh(sphere)
|
||
|
|
sphere.paint_uniform_color((0.9, 0.9, 0.9))
|
||
|
|
|
||
|
|
coord_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(
|
||
|
|
size=0.5, origin=[0.0, 0.0, 0.0]
|
||
|
|
)
|
||
|
|
things_to_draw = [sphere, coord_frame]
|
||
|
|
|
||
|
|
|
||
|
|
frustums = []
|
||
|
|
|
||
|
|
for W2C in list_of_poses:
|
||
|
|
camera_frustum = get_camera_frustum(
|
||
|
|
img_size, K, W2C, frustum_length=camera_size, color=camera_color
|
||
|
|
)
|
||
|
|
frustums.append(camera_frustum)
|
||
|
|
|
||
|
|
cameras = frustums2lineset(frustums)
|
||
|
|
things_to_draw.append(cameras)
|
||
|
|
|
||
|
|
o3d.visualization.draw_geometries(things_to_draw)
|