process videos for robot positions in videos

07-extrinsic_calibration_UWB.py

@@ -87,7 +87,7 @@ def calibrate_uwb_extrinsic(intrinsic_calib_path, intrinsic_calib_path_undistort
         cv2.namedWindow(fname, cv2.WINDOW_NORMAL)
         cv2.resizeWindow(fname, (1280, 720))
         cv2.imshow(fname, frame)
-        cv2.waitKey(0)
+        cv2.waitKey(1000)
         cv2.destroyAllWindows()
 
         print("Frame name: ", fname)
@@ -216,7 +216,7 @@ def load_uwb_locations(path):
 
     # Select RMSE values
     RMSE_vals = df[['rmse']].values.tolist()
-    RMSE_vals = np.array(RMSE_vals)*10.0 # Nx1 # convert to mm from cm
+    RMSE_vals = np.array(RMSE_vals)*1000.0 # Nx1 # convert to mm from m
 
     # print("T_iu_vecs:", T_iu_vecs)
     # print("RMSE_vals:", RMSE_vals)

calibration_files/world_to_inertialUWB.yaml

@@ -4,29 +4,29 @@ R_oi: !!opencv-matrix
    rows: 3
    cols: 3
    dt: d
-   data: [ 9.9829162825139406e-01, -3.8121198566319200e-03,
-       5.8303453631656059e-02, 5.6729683912329397e-03,
-       9.9947864649722651e-01, -3.1784471455475284e-02,
-       -5.8151890707113414e-02, 3.2060925411948361e-02,
-       9.9779279144966637e-01 ]
+   data: [ 9.9968699358215751e-01, -3.1043220016651611e-03,
+       -2.4824948088109214e-02, 2.9373853693367429e-03,
+       9.9997284893502025e-01, -6.7581920637651968e-03,
+       2.4845253668645574e-02, 6.6831562669676745e-03,
+       9.9966896960566487e-01 ]
 R_io: !!opencv-matrix
    rows: 3
    cols: 3
    dt: d
-   data: [ 9.9829162825139406e-01, 5.6729683912329397e-03,
-       -5.8151890707113414e-02, -3.8121198566319200e-03,
-       9.9947864649722651e-01, 3.2060925411948361e-02,
-       5.8303453631656059e-02, -3.1784471455475284e-02,
-       9.9779279144966637e-01 ]
+   data: [ 9.9968699358215751e-01, 2.9373853693367429e-03,
+       2.4845253668645574e-02, -3.1043220016651611e-03,
+       9.9997284893502025e-01, 6.6831562669676745e-03,
+       -2.4824948088109214e-02, -6.7581920637651968e-03,
+       9.9966896960566487e-01 ]
 T_oi: !!opencv-matrix
    rows: 3
    cols: 1
    dt: d
-   data: [ -5.9723460862570846e+03, -1.9151986073894097e+03,
-       2.5663160667325144e+02 ]
+   data: [ -5.9290545153093199e+03, -1.8991242694821713e+03,
+       -1.9005018068639905e+02 ]
 T_io: !!opencv-matrix
    rows: 3
    cols: 1
    dt: d
-   data: [ 5.9879315732363320e+03, 1.8832049659809420e+03,
-       3.1269660447441311e+01 ]
+   data: [ 5.9374989879869454e+03, 1.8819371469108717e+03,
+       2.9964151179901279e+01 ]

readme.md

@@ -187,6 +187,6 @@ Keep pressing 'SPACE' in your keyboard to capture as many as images you want. Th
 Assuming the UWB xyz locations are also captured in the previous step along with the images, this step finds (calibrates) the relative pose between the vision system (the world frame that is previously found in extrinsic calibration) and the UWB anchors origin. 
 
 ```
-python3 07-extrinsic_calibration_UWB.py --calib_file ./calibration_files/camera.yml --calib_file_undistorted ./calibration_files/camera_undistorted.yml --calib_file_aruco ./calibration_files/camera_aruco.yml  --image_dir ./calibration_images_uwb --prefix image_  --image_format png --square_size 65 --width 8 --height 7 --uwb_tags_info_file ./uwb_tags_info.csv --rmse_threshold 60 --save_file ./calibration_files/world_to_inertialUWB.yaml
+python3 07-extrinsic_calibration_UWB.py --calib_file ./calibration_files/camera.yml --calib_file_undistorted ./calibration_files/camera_undistorted.yml --calib_file_aruco ./calibration_files/camera_aruco.yml  --image_dir ./calibration_images_uwb --prefix image_  --image_format png --square_size 65 --width 8 --height 7 --uwb_tags_info_file ./uwb_tags_info2.csv --rmse_threshold 40 --save_file ./calibration_files/world_to_inertialUWB.yaml
 ```
 

uwb_tags_info2.csv

@@ -0,0 +1,31 @@
+x,y,z,rmse
+9.08886045360242,2.45565854465822,0.0335179216902524,0.0369491273382499
+9.50137513944123,3.29874788989115,-0.000357066705900355,0.0327622511892619
+9.52996464832073,2.66396232099651,-0.0830193731527579,0.022004021267988
+9.07254425595029,2.19165242484872,-0.0293582747193913,0.0152028451624767
+9.26917493005188,0.98597575633097,-0.119388572678183,0.0664428246784961
+9.86010159475142,1.28220476061537,-0.0364925831314516,0.0281302888916521
+8.57032746000429,1.51646015549426,-0.0194915447391909,0.0164731136495681
+8.13118442895625,1.32017228880272,0.0525797826557223,0.0259169333168978
+7.81924966181995,2.40149710946637,0.0343776954101313,0.00909058881495586
+7.60594548045297,2.1634659967057,-0.00755541767707377,0.067681000881502
+7.74971305922102,2.61505799418292,0.0673061577632208,0.0276753640211709
+8.32276048383699,3.09014799806959,-0.00310006079980074,0.276934724399954
+6.24577677861785,3.22297542902414,0.061739933222384,0.0505563002479043
+6.7932729754522,3.36388604191922,0.0530696252431585,0.0374090162333852
+6.5723178012528,2.64728811475934,0.124709231970312,0.0520460752514465
+5.85717707870188,2.14025238673228,0.018953005182875,0.315237480022292
+6.48321681648035,0.709361346666735,-0.0531581527720754,0.069544179623089
+5.78239518167318,0.595002211528913,0.00738554929806792,0.0512039386362971
+4.80495919157962,0.433066715093211,0.043075309739131,0.099660984389799
+5.49249838353186,1.12512585620405,0.0223379816952019,0.397170758191237
+4.70886692386246,1.35280639750763,-0.00382318624049813,0.0767025555010329
+5.34303193217982,1.95279424233094,-0.0909957686119158,0.267554771719633
+4.85169879112779,3.65092865044467,-0.00197528763999932,0.0740141726688402
+5.04313285192807,3.36522381640366,0.0249363790648658,0.030374372720579
+3.64676410067731,2.90518696682342,0.104550607634855,0.0423872105835802
+4.45548339749366,2.61823581423718,-0.0459737301690111,0.267096222201086
+3.64729950036192,1.33424450203489,0.0422707186955628,0.0367887457407899
+3.69229600187024,1.51737673588368,0.107970774222216,0.0554220543041784
+3.5087302344801,0.545444814410833,0.073503080252263,0.0136077835335259
+3.75801942385391,0.82179649304434,0.128911264170161,0.0397365237435822