now calculates the B matrix for each collection #939

lardemua · May 3, 2024 · be1aeb0 · be1aeb0
1 parent bb4713a
commit be1aeb0
Showing 1 changed file with 11 additions and 0 deletions.
diff --git a/atom_evaluation/scripts/other_calibrations/rwhe_calib_ali.py b/atom_evaluation/scripts/other_calibrations/rwhe_calib_ali.py
@@ -15,9 +15,11 @@
 
 import argparse
 import numpy as np
+import cv2
 
 from atom_core.dataset_io import loadResultsJSON
 from atom_core.atom import getTransform
+from atom_core.geometry import traslationRodriguesToTransform
 
 def getPatternConfig(dataset, pattern):
     # Pattern configs
@@ -163,6 +165,15 @@ def main():
 
         imgpoints_camera.append(tmp_imgpoints_camera)
 
+        retval, rvec, tvec = cv2.solvePnP(objp, imgpoints_camera[0], K, D)
+
+        tf_pattern2opticalframe = traslationRodriguesToTransform(tvec, rvec)
+
+        # B is the inverse of pattern2opticalframe
+
+        B = np.linalg.inv(tf_pattern2opticalframe)
+
+        BB.append(B)
 
 
 if __name__ == '__main__':