calculate projection matrices

scripts/nuscenes_devkit/python-sdk/scripts/calculate_projection_matrix_numpy.py

@@ -0,0 +1,35 @@
+import numpy as np
+
+intrinsic_matrix = np.array([[2.82046004e+03, 0.00000000e+00, 9.60667182e+02],
+                             [0.00000000e+00, 2.81622151e+03, 5.25863289e+02],
+                             [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]], dtype=float)
+
+rotation_matrix = np.array([[-6.67942916e-04, -1.94233505e-01, 9.80955096e-01],
+                            [-9.99986586e-01, 5.16816386e-03, 3.42417940e-04],
+                            [-5.13624571e-03, -9.80941709e-01, -1.94234351e-01]], dtype=float)
+
+translation_vector = np.array([0.82720991e+00, -5.59124063e+00, 8.4336e+00]).T
+
+# matmul
+# - multiplication by scalars is not allowed
+# - stacks of matrices (n dim > 2) are broadcast together as if the matrices were elements
+
+# dot
+# - is equivalent for 2D arrays
+
+# 3x3 * 3x3 = 3x3
+kr_matrix = np.matmul(intrinsic_matrix, rotation_matrix)
+
+# 3x3 * 3x1 = 3x1
+t = np.matmul(kr_matrix, translation_vector)
+
+projection_matrix = np.zeros((3, 4))
+projection_matrix[0:3, 0:3] = kr_matrix
+projection_matrix[0:3, 3] = -t
+# print(projection_matrix)
+# [[-6.81812900e+00 -1.49018635e+03  2.58015008e+03  1.04349952e+04]
+#  [-2.81888470e+03 -5.01286539e+02 -1.01176390e+02 -1.13112515e+04]
+#  [-5.13624571e-03 -9.80941709e-01 -1.94234351e-01  8.43360000e+00]]
+
+
+print(projection_matrix)

scripts/nuscenes_devkit/python-sdk/scripts/calculate_projection_matrix_torch.py

@@ -0,0 +1,28 @@
+import torch
+
+intrinsic_matrix = torch.tensor([[2.82046004e+03, 0.00000000e+00, 9.60667182e+02],
+                                 [0.00000000e+00, 2.81622151e+03, 5.25863289e+02],
+                                 [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
+
+rotation_matrix = torch.tensor([[-6.67942916e-04, -1.94233505e-01, 9.80955096e-01],
+                                [-9.99986586e-01, 5.16816386e-03, 3.42417940e-04],
+                                [-5.13624571e-03, -9.80941709e-01, -1.94234351e-01]])
+
+translation_vector = torch.tensor([0.82720991e+00, -5.59124063e+00, 8.4336e+00])
+
+# 3x3 * 3x3 = 3x3
+kr_matrix = torch.mm(intrinsic_matrix, rotation_matrix)
+
+# 3x3 * 3x1 = 3x1
+t = torch.mv(kr_matrix, translation_vector)
+
+projection_matrix = torch.zeros((3, 4))
+projection_matrix[0:3, 0:3] = kr_matrix
+projection_matrix[0:3, 3] = -t
+
+print(projection_matrix)
+# tensor([[-6.8181e+00, -1.4902e+03,  2.5802e+03, -3.0086e+04],
+#         [-2.8189e+03, -5.0129e+02, -1.0118e+02,  3.8228e+02],
+#         [-5.1362e-03, -9.8094e-01, -1.9423e-01, -3.8423e+00]])
+
+

scripts/nuscenes_devkit/python-sdk/scripts/decompose_projection_matrix.py

@@ -0,0 +1,62 @@
+import numpy as np
+from numpy.linalg import det
+from numpy.linalg import inv
+from scipy.linalg import rq
+
+
+class Calibration:
+
+    def P_to_KRt(self, P):
+        '''
+
+        This function computes the decomposition of the projection matrix into intrinsic parameters, K, and extrinsic parameters Q (the rotation matrix) and t (the translation vector)
+
+        Usage:
+        K, R, t = P_to_KRt(P)
+
+        Input:
+        P: 3x4 projection matrix
+
+        Outputs:
+        K: 3x3 camera intrinsics
+        R: 3x3 rotation matrix (extrinsics)
+        t: 3x1 translation vector(extrinsics)
+
+        '''
+
+        M = P[0:3, 0:3]
+
+        R, Q = rq(M)
+
+        K = R / float(R[2, 2])
+
+        if K[0, 0] < 0:
+            K[:, 0] = -1 * K[:, 0]
+            Q[0, :] = -1 * Q[0, :]
+
+        if K[1, 1] < 0:
+            K[:, 1] = -1 * K[:, 1]
+            Q[1, :] = -1 * Q[1, :]
+
+        if det(Q) < 0:
+            print('Warning: Determinant of the supposed rotation matrix is -1')
+
+        P_3_3 = np.dot(K, Q)
+
+        P_proper_scale = (P_3_3[0, 0] * P) / float(P[0, 0])
+
+        t = np.dot(inv(K), P_proper_scale[:, 3])
+
+        return K, Q, t
+
+
+if __name__ == '__main__':
+    calib = Calibration()
+    P = np.array([[9.40487461e+02, -2.82009326e+03, -2.01081143e+02, -1.48499626e+04],
+                  [-3.11563010e+01, 1.47347593e+01, -2.86468986e+03, 2.42678068e+04],
+                  [9.80955096e-01, 3.42417940e-04, -1.94234351e-01, 8.28553591e-01]])
+
+    K, R, t = calib.P_to_KRt(P)
+    print("intrinsic matrix: ", K)
+    print("rotation matrix: ", R)
+    print("translation vector: ", t)

scripts/nuscenes_devkit/python-sdk/scripts/rename_files.py

@@ -0,0 +1,9 @@
+import os
+
+path = "../../../../input/providentia/20210302_sequence/pointclouds/"
+
+files = sorted(os.listdir(path))
+idx=0
+for file in files:
+  os.rename(path+file,path+str(idx).zfill(6)+".pcd")
+  idx=idx+1