Better visualisation

README.md

@@ -1,6 +1,8 @@
 # Multiple view Camera calibration tool
 This tool allows to compute the intrinsic and extrinsic camera parameters of a set of synchronized cameras with overlapping field of view. The intrinsics estimation is based on the OpenCV's camera calibration framework and it is used on each camera separately. In the extrinsics estimation, an initial solution (extrinsic parameters) is computed first using a linear approach then refined using bundle adjustment.  The output are camera poses (intrinsic matrix, distortion parameters, rotations and translations) w.r.t. either the first camera or a global reference system.
 
+![alt text](https://github.com/lcit/multiview_calib/blob/master/images/visualisation.png?raw=true)
+
 ## Prerequisites
 
 - numpy

scripts/visualisation.py

@@ -0,0 +1,194 @@
+import argparse
+import numpy as np
+import plotly
+import plotly.graph_objects as go
+from scipy.spatial.distance import pdist
+
+from multiview_calib import utils 
+
+class CameraWireframe:
+
+    def __init__(self, scale=1):
+        self.scale = scale
+        self.vertices = np.array([
+            [-1, -1, 0],   # Bottom center
+            [1, -1, 0],   # Top vertex 1
+            [1, 1, 0],  # Bottom vertex 2
+            [-1, 1, 0], # Bottom vertex 3
+            [-1, -1, 3],   # Bottom center
+            [1, -1, 3],   # Top vertex 1
+            [1, 1, 3],  # Bottom vertex 2
+            [-1, 1, 3], # Bottom vertex 3
+            [-5, -3, 5],   # Bottom center
+            [5, -3, 5],   # Top vertex 1
+            [5, 3, 5],  # Bottom vertex 2
+            [-5, 3, 5], # Bottom vertex 3
+        ]) * scale
+
+        self.edges = [
+            (0, 1), (1, 2), (2, 3), (0, 3),  # Bottom edges
+            (0+4, 1+4), (1+4, 2+4), (2+4, 3+4), (0+4, 3+4),   # Top edges
+            (0, 4), (1, 1+4), (2, 2+4), (3, 3+4),   # Bottom to top
+            (0+8, 1+8), (1+8, 2+8), (2+8, 3+8), (0+8, 3+8),   # Top edges
+            (4, 8), (5, 1+8), (6, 2+8), (7, 3+8),   # Top edges
+        ]
+
+        self.camera_position = np.array([0,0,0])
+        self.camera_orientation = np.eye(3)
+
+        self.vertices_system = np.array([
+            [0,0,0],
+            [7.5,0,0],
+            [0,7.5,0],
+            [0,0,7.5]
+        ])*self.scale
+
+        self.edges_system = [(0,1), (0,2), (0,3)]
+
+    def transform(self, R, t):        
+        self.camera_position = np.array(t).ravel()
+        self.camera_orientation = np.array(R)
+        self.vertices = self.vertices @ self.camera_orientation.T + self.camera_position.reshape(1,3)
+        self.vertices_system = self.vertices_system @ self.camera_orientation.T + self.camera_position.reshape(1,3)
+        return self
+
+    def get_plotly_wireframe(self):
+        edge_x = []
+        edge_y = []
+        edge_z = []
+        x, y, z = zip(*self.vertices)
+        for s, e in self.edges:
+            # Add None between two edge points to draw separate lines
+            edge_x += [x[s], x[e], None]
+            edge_y += [y[s], y[e], None]
+            edge_z += [z[s], z[e], None]
+
+        return dict(x=edge_x, y=edge_y, z=edge_z)
+
+    def get_plotly_coordinate_system(self):
+        x, y, z = zip(*self.vertices_system)
+        return (dict(x=[x[0], x[1], None], 
+                     y=[y[0], y[1], None], 
+                     z=[z[0], z[1], None]), # x-axis arrow
+                dict(x=[x[0], x[2], None], 
+                     y=[y[0], y[2], None], 
+                     z=[z[0], z[2], None]), # y-axis arrow
+                dict(x=[x[0], x[3], None], 
+                     y=[y[0], y[3], None], 
+                     z=[z[0], z[3], None])) # z-axis arrow
+
+def main(filename_poses="poses.json",
+         filename_points="points.json"):
+
+    poses = utils.json_read(filename_poses)
+    points = None
+    if filename_points is not None:
+        points = utils.json_read(filename_points)
+
+    # estimate scale of the camera wireframes
+    camera_positions = np.array([
+        np.dot(-np.array(x['R']).T, np.array(x['t'])) for name, x in poses.items()
+    ])
+    scale = pdist(camera_positions).mean()/50
+
+    plotly_data = []
+
+    # add camera wireframes to plot
+    coordinate_systems = [dict(x=[], y=[], z=[]) for _ in range(3)]
+    for camera_name, pose in poses.items():
+        R = np.array(pose['R'])
+        t = np.array(pose['t'])
+        R_inv = R.T
+        t_inv = -R.T @ t.reshape(3,1)
+        camera = CameraWireframe(scale=scale).transform(R_inv, t_inv)
+
+        plotly_data.append(
+            go.Scatter3d(
+                **camera.get_plotly_wireframe(),
+                mode='lines',
+                line=dict(width=4),
+                name=camera_name
+            )
+        )
+
+        # combine arrows of the coordinate systems
+        arrows = camera.get_plotly_coordinate_system()
+        for i in range(3):
+            coordinate_systems[i]['x'] += arrows[i]['x']
+            coordinate_systems[i]['y'] += arrows[i]['y']
+            coordinate_systems[i]['z'] += arrows[i]['z']
+
+    plotly_data.append(
+        go.Scatter3d(
+            **coordinate_systems[0],
+            mode='lines',
+            line=dict(color="red", width=6),
+            name="camera systems x-axis"
+        )
+    )
+    plotly_data.append(
+        go.Scatter3d(
+            **coordinate_systems[1],
+            mode='lines',
+            line=dict(color="green", width=6),
+            name="camera systems y-axis"
+        )
+    )
+    plotly_data.append(
+        go.Scatter3d(
+            **coordinate_systems[2],
+            mode='lines',
+            line=dict(color="blue", width=6),
+            name="camera systems z-axis"
+        )
+    )
+
+    # add points
+    if points is not None:
+        _points = np.array(points['points_3d'])
+        plotly_data.append(
+            go.Scatter3d(
+                x=_points[:,0],
+                y=_points[:,1],
+                z=_points[:,2],
+                mode='markers',
+                marker=dict(
+                    size=4,
+                    opacity=1
+                ),
+                name="points"
+            )
+        )
+
+    fig = go.Figure(data=plotly_data)
+    fig.update_layout(
+        scene=dict(
+            xaxis_title='X',
+            yaxis_title='Y',
+            zaxis_title='Z',
+            aspectmode="data"
+    ))
+
+    fig.show()
+
+
+if __name__ == "__main__":
+
+    def str2bool(v):
+        if v.lower() in ('yes', 'true', 't', 'y', '1'):
+            return True
+        elif v.lower() in ('no', 'false', 'f', 'n', '0'):
+            return False
+        else:
+            raise argparse.ArgumentTypeError('Boolean value expected.')    
+
+    parser = argparse.ArgumentParser()  
+    parser.add_argument("--filename_poses", "-p", type=str, required=True, default="global_poses.json",
+                        help='JSON file containing the camera poses.')    
+    parser.add_argument("--filename_points", "-o", type=str, required=False, default=None,
+                        help='JSON file containing the 3d points.')
+
+    args = parser.parse_args()
+    main(**vars(args))
+
+# python visualisation.py -p global_poses.json -o global_triang_points.json