From 15942afb622cbd43d503e57598d7c28ba4c11aaa Mon Sep 17 00:00:00 2001
From: Kazadhum <diogoscsv@gmail.com>
Date: Fri, 3 May 2024 15:00:36 +0100
Subject: [PATCH] now gets camera intrinsics from dataset to calculate B #939

---
 .../other_calibrations/rwhe_calib_ali.py      | 30 ++++++++++++++++++-
 1 file changed, 29 insertions(+), 1 deletion(-)

diff --git a/atom_evaluation/scripts/other_calibrations/rwhe_calib_ali.py b/atom_evaluation/scripts/other_calibrations/rwhe_calib_ali.py
index c2d7205b..575b1e6b 100755
--- a/atom_evaluation/scripts/other_calibrations/rwhe_calib_ali.py
+++ b/atom_evaluation/scripts/other_calibrations/rwhe_calib_ali.py
@@ -14,10 +14,27 @@
 """
 
 import argparse
+import numpy as np
 
 from atom_core.dataset_io import loadResultsJSON
 from atom_core.atom import getTransform
 
+
+def getCameraIntrinsicsFromDataset(dataset, camera):
+    # Camera intrinsics (from the dataset) needed to calculate B
+    K = np.zeros((3, 3), np.float32)
+    D = np.zeros((5, 1), np.float32)
+    K[0, :] = dataset['sensors'][camera]['camera_info']['K'][0:3]
+    K[1, :] = dataset['sensors'][camera]['camera_info']['K'][3:6]
+    K[2, :] = dataset['sensors'][camera]['camera_info']['K'][6:9]
+    D[:, 0] = dataset['sensors'][camera]['camera_info']['D'][0:5]
+
+    height = dataset['sensors'][camera]['camera_info']['height']
+    width = dataset['sensors'][camera]['camera_info']['width']
+    image_size = (height, width)
+
+    return K, D, image_size
+
 def main():
     
     # Parse command line arguments
@@ -31,6 +48,7 @@ def main():
                     "language. Example: lambda name: int(name) > 5 , to load only collections 6, 7, and onward.")
     ap.add_argument("-bln", "--base_link_name", type=str, required=False, default="base_link", help="Name of the robot base link frame.")
     ap.add_argument("-eeln", "--end_effector_link_name", type=str, required=False, default="flange", help="Name of the end-effector link frame.")
+    ap.add_argument("-c", "--camera", help="Camera sensor name.", type=str, required=True)
     
     args = vars(ap.parse_args())
 
@@ -38,6 +56,7 @@ def main():
     collection_selection_function = args['collection_selection_function']
     base_link_name = args['base_link_name']
     end_effector_link_name = args['end_effector_link_name']
+    camera = args['camera']
 
     # Read dataset file
     dataset, json_file = loadResultsJSON(json_file, collection_selection_function)
@@ -47,10 +66,17 @@ def main():
     # GET A and B matrices to solve AX=ZB #
     ########################################
 
+    # Get camera intrinsics from dataset
+    K, D, image_size = getCameraIntrinsicsFromDataset(
+        dataset=dataset,
+        camera=camera
+        )
+
     # Initialize list of A and B matrices (one for each collection)
     AA = [] 
     BB = []
     
+    
     for collection_key, collection in dataset['collections'].items():
         
         print("Calculating A and B matrices for collection " + collection_key + "...")
@@ -64,7 +90,9 @@ def main():
         )
 
         AA.append(A)
-        
+
+        # B is the transform from the camera to the calibration pattern. We can get it from the solvePnP() method, using the detections in the dataset
+
 
 if __name__ == '__main__':
     main()
\ No newline at end of file