.

Author: rjsun06

GEMstack/offboard/calibration/tools/calibration.py

@@ -6,6 +6,8 @@
 import open3d as o3d
 from pathlib import Path
 from scipy.spatial.transform import Rotation as R
+from tools.save_cali import load_ex, save_ex, load_in, save_in
+from tools.visualizer import visualizer as vis
 
 def pc_relative(pc0,pc1):
     #get relative translation and rotation from pc0 to pc1
@@ -25,28 +27,14 @@ def pc_relative(pc0,pc1):
     print("Transformation Matrix:\n", reg_result.transformation)
     return(reg_result.transformation)
 
-def r_from_vector(direction):
-    direction = np.array(direction)
-    direction = direction / np.linalg.norm(direction)
-    target = np.array([0, 1, 0])
-    rotation_axis = np.cross(direction, target)
-    if np.allclose(rotation_axis, 0):
-        return R.from_quat([0, 0, 0, 1])
-    else:
-        angle = np.arccos(np.clip(np.dot(direction, target), -1.0, 1.0))
-        rotation_axis = rotation_axis / np.linalg.norm(rotation_axis)
-        r = R.from_rotvec(angle * rotation_axis)
-        return r
-
-#%%
-# img_dir = root + "/data/calib1/img"
-img_dir = root + "/data/test"
+#%%
+img_dir = root + "/data/calib1/img"
+# img_dir = root + "/data/test"
 output_dir = root + '/GEMstack/offboard/calibration/output'
-
 output_path = Path(output_dir)
 output_path.mkdir(exist_ok=True, parents=True)
 
-# 1. Feature extraction
+# 1. Feature extraction. google 'sift' to know what it does.
 extract_options = pycolmap.SiftExtractionOptions()
 extract_options.num_threads = 2
 pycolmap.extract_features(
@@ -57,9 +45,10 @@ def r_from_vector(direction):
     # device=pycolmap._core.Device.cuda
 )
 
-# 2. Feature matching
+# 2. Feature matching. match_sequential tries matching only on pairs close in filename ordering 
 match_options = pycolmap.SequentialMatchingOptions()
-match_options.overlap = 3
+match_options.overlap = 3   # how many subsequent images should be tried matching. 
+                            # 3 is too small actually. do larger if you figured out pycolmap+cuda
 pycolmap.match_sequential(
     database_path=output_path/"database.db",
     matching_options=match_options
@@ -71,7 +60,7 @@ def r_from_vector(direction):
 #     matching_options=match_options
 # )
 
-# 3. Sparse reconstruction
+# 3. Sparse reconstruction. ba_refine = 0 so it don't mess with the intrinsics we got in feature extraction stage.
 mapper_options = pycolmap.IncrementalPipelineOptions()
 mapper_options.ba_refine_focal_length = 0
 mapper_options.ba_refine_principal_point = 0
@@ -84,22 +73,26 @@ def r_from_vector(direction):
 )
 
 #%%
-dense_dir = output_path
-dense_dir.mkdir(exist_ok=True)
+#%%
+# this part tries 3d reconstruction with stereo depth given the images registered in the previous part.
+# but patch_match_stereo needs cuda and compiling pycolmap from source so haven't managed to run it.
 
-pycolmap.patch_math_stereo(
-    input_path=output_path,
-    output_path=dense_dir,
-    output_type="TXT",
-)
+# dense_dir = output_path
+# dense_dir.mkdir(exist_ok=True)
 
-pycolmap.stereo_fusion(
-    output_path=dense_dir,
-    workspace_path=dense_dir,
-    workspace_format="COLMAP",
-    # max_image_size=2000,
-    # gpu_index="0",
-)
+# pycolmap.patch_match_stereo(
+#     input_path=output_path,
+#     output_path=dense_dir,
+#     output_type="TXT",
+# )
+
+# pycolmap.stereo_fusion(
+#     output_path=dense_dir,
+#     workspace_path=dense_dir,
+#     workspace_format="COLMAP",
+#     max_image_size=2000,
+#     gpu_index="0",
+# )
 
 # 4. Depth Map Fusion (Global Registration)
 # fused_pc = pycolmap.dense_fusion(
@@ -110,34 +103,52 @@ def r_from_vector(direction):
 # )
 
 #%%
+#%%
+# get correspondence between image registrations and filenames, 
+# and find image142 is from oak
 [(id,reconstructions[0].image(id).name) for id in reconstructions[0].reg_image_ids()]
 
 #%%
+#make a copy because I was in jupyter lab trying to test different stuff with it
 rec = reconstructions[0].__copy__()
+
 #%%
+# anchor the coordination system to image #142 
+# because I found #142 is the first registered images from the front camera
 trans = rec.image(142).cam_from_world
 rec.transform(pycolmap.Sim3d(rotation=trans.rotation,translation=trans.translation))
-rec.transform(pycolmap.Sim3d(rotation=np.array(R.from_euler('yx',(np.pi/2,-np.pi/2)).as_quat())))
-rec.image(142).viewing_direction()
+print(rec.image(142).viewing_direction()) # shoud be a trivial direction
+
+#%%
+#%%
+pc = np.array([p.xyz for p in rec.points3D.values()])
+##################################################################################
+#so far we have got pointcloud reconstructed from camera images
+#################################################################################
+# tranform to vehicle coordination just for visualization
+camera_ex = load_ex(root + "/GEMstack/knowledge/calibration/gem_e4_oak.yaml",mode='matrix')
+pc = np.pad(pc,((0,0),(0,1)),constant_values=1) @ camera_ex.T[:,:3]
+
 #%%
 def load_scene(path):
     sc = np.load(path)['arr_0'] 
     sc = sc[~np.all(sc == 0, axis=1)] # remove (0,0,0)'s
     return sc
 
 #%%
-# import re
-# ind = re.match("oak(\d+)\.png$",base).group(1)
-ind = 1
-sc = load_scene(f"/mnt/GEMstack/data/calib1/pc/ouster{ind}.npz")
-
+#%%
+# get lidar pointcloud
+from save_cali import load_ex
+sc = load_scene(root + f"/data/calib1/pc/ouster1.npz")
+lidar_ex = load_ex(root + "/GEMstack/knowledge/calibration/gem_e4_ouster.yaml",mode='matrix')
+sc = np.pad(sc,((0,0),(0,1)),constant_values=1) @ lidar_ex.T[:,:3]
 
 #%%
 pc = np.array([p.xyz for p in rec.points3D.values()])
 #%%
 from visualizer import visualizer as vis
-import os
-os.environ['DISPLAY'] = ':0'
+#import os
+#os.environ['DISPLAY'] = ':0'
 #%%
 v = vis()
 v.add_pc(pc,color='red')

GEMstack/offboard/calibration/tools/visualizer.py

@@ -1,6 +1,4 @@
 import pyvista as pv
-import os
-os.environ['DISPLAY'] = ':0'
 class visualizer:
     def __init__(self):
         self.plotter = pv.Plotter(notebook=False)