From ee030ce624c7cd201aefabfee6f61abc8254ddea Mon Sep 17 00:00:00 2001
From: Solal Nathan <otthorn@crans.org>
Date: Tue, 29 Jun 2021 15:08:30 +0200
Subject: [PATCH] Visualise non-json camera position for a dataset

---
 visualize_camera_path.py | 81 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 81 insertions(+)
 create mode 100644 visualize_camera_path.py

diff --git a/visualize_camera_path.py b/visualize_camera_path.py
new file mode 100644
index 0000000..d36a554
--- /dev/null
+++ b/visualize_camera_path.py
@@ -0,0 +1,81 @@
+""""
+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)