problems with hand-eye robot calibration

[kouroshD]

We wanted to use the robot hand-eye calibration for calibrating a zed mini camera connected to the robot hand.
These are the problems we faced:

• as documented in Test camera in environment calibration #22 (comment) , we had the problem with realsense running. running Test camera in environment calibration #22 (comment) causes workstation freezing, so we had to restart.

• the aruco_board configuations as written in https://github.com/pairlab/franka_arm_infra/blob/ebb6b4fd4de964238b1ff9ee4e5929788029c863/robot_toolkit/config/robot_camera_calibration.yaml#L19C1-L24C1 is different from actual one added in https://github.com/pairlab/franka_arm_infra/tree/master/robot_toolkit/calibration_board. The number of columns, rows are different, the sizes are different.

• at the moment based on documentation, move_robot_automatically and robot_pose_collector (["ros_tf", "zmq"]) are entangled. However, when we tesed back in time, I am pretty sure we did move robot manually while using ros_tf. based on the code and documentation this is not possible. This entanglement seems to be unnecessary and need to be fixed:

robot_pose_collector:
values = ["ros_tf", "zmq"]
This specifies, wheter to read end-effector pose from a TF tree or using libfranka.
"ros_tf" only works when "move_robot_automatically" is True
"zmq" only works when "move_robot_automatically" is False

and

move_robot_automatically
values = [True or False]
If set to True, the robot moves its end-effector to randomly sampled configurations around the initial end-effector pose. "robot_pose_collector" has to be "ros_tf"
If set to false, the user is expected to move the end-effector manually and press "Enter" to collect each pose measurement. "robot_pose_colector" has to be "zmq"

Why we need this part of the code:
https://github.com/pairlab/franka_arm_infra/blob/ebb6b4fd4de964238b1ff9ee4e5929788029c863/robot_toolkit/robot_arm_algos/src/config_reader.py#L60-L62

• Finally, when we removed those lines of the code and running camera calibration with data collection, we had the following results:

INFO     [_data_collector.py:89] stopping data collection
Traceback (most recent call last):
  File "robot_camera_calibration.py", line 41, in <module>
    main(args)
  File "robot_camera_calibration.py", line 31, in main
    robot_camera_calibrator.run_calibration()
  File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/robot_camera_calibrator.py", line 45, in run_calibration
    results = self.run_calibration_all_solvers()
  File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py", line 46, in run_calibration_all_solvers
    out.append((self.run_calibration_solver(solver_name), solver_name))
  File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py", line 29, in run_calibration_solver
    output_rvec, output_tvec = cv2.calibrateHandEye(self.A_rvecs,
cv2.error: OpenCV(4.8.1) /io/opencv/modules/calib3d/src/calibration_handeye.cpp:524: error: (-7:Iterations do not converge) Rotation normalization issue: determinant(R) is null in function 'normalizeRotation'
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# cat INFO     [_data_collector.py:89] stopping data collection
Traceback (most recent call last):
  File "robot_camera_calibration.py", line 41, in <module>
    main(args)
  File "robot_camera_calibration.py", line 31, in main
    robot_camera_calibrator.run_calibration()
  File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/robot_camera_calibrator.py", line 45, in run_calibration
    results = self.run_calibration_all_solvers()
  File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py", line 46, in run_calibration_all_solvers
    out.append((self.run_calibration_solver(solver_name), solver_name))
  File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py", line 29, in run_calibration_solver
    output_rvec, output_tvec = cv2.calibrateHandEye(self.A_rvecs,
cv2.error: OpenCV(4.8.1) /io/opencv/modules/calib3d/src/calibration_handeye.cpp:524: error: (-7:Iterations do not converge) Rotation normalization issue: determinant(R) is null in function 'normalizeRotation'
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# cat robot_arm_algos/src/robot_camera_calibration/data/file_zed.txt
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.3142142157497399,-0.281232321905755,-1.5769491028769251,-0.08505167671485281,0.10869832690184308,0.39249290501489786
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.28773136905876123,-0.2209121417232348,-0.829223978364923,-0.20964332708926126,0.017531313822759916,0.3672961434396176
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.2570181779444119,-0.09746567994537958,-1.0699099031113966,-0.15603926586472833,-0.027514982324621445,0.29307311079805554
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.21535897793733225,-0.8021849904215289,-2.6183627779871346,0.0804890806733553,0.14759499758703634,0.302972669369931
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.14409498246604852,-0.2052178522624006,-1.504308288031509,-0.11400544279069431,0.1049010721062046,0.362044018415615
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.10768996297912165,-0.13775094708694482,-1.5388697554875341,-0.09747698001689804,0.09620180060062024,0.2818964855091452
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.0761867753585027,0.1071534366043921,1.6475733127998262,0.18841268404313805,-0.11382140603083668,0.4070216521176688
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.19160444938074436,0.2583456645203763,1.65504216147805,0.19073127701996334,-0.0506569473583837,0.3367004287030583
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.6170967953795665,-0.12106721094334894,1.2168673382563386,0.12034627122336379,-0.10865791043168911,0.40943607485617844
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.09648597580536017,0.369950401393866,1.7875017050482902,0.10220678523705298,-0.044714769070031656,0.3207522259643255
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.2617917295063692,0.29022046091289866,1.2313526959379266,0.15115300797894204,-0.029634361001287492,0.38088352021160315
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.18329912971167342,0.1809686748966991,1.5395180231517518,0.14863225520732926,-0.07761643915144989,0.373717923052905
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.19528621108989774,0.08985369436358373,1.6527944705778939,0.15209171133351346,-0.15196760613214502,0.31990031388617146
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.297447071725141,0.24085158978157642,1.546094400268486,0.14347003109971654,-0.1026537255446094,0.28603619027253285
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.03862430333273748,0.6825080702572005,2.7623647704478467,0.11155853079526043,0.06213323505774098,0.3183670489239787
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.31360633716881847,0.0025016099403963443,1.1930977189119265,0.12625739421008036,-0.11892818921360301,0.39162148275970066
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.31020300715752364,-0.36940737908506005,-1.524298466857078,-0.13405980383137303,0.06831699191997168,0.36715485470681464
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.4660244603311664,-0.2176859173440443,-1.0497244832213528,-0.12445600909599651,0.05747638488640651,0.34198397139094505
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.5430288027947624,-0.15697679934028275,-0.5509116306435331,-0.12872590786435661,-0.005455670150886657,0.3376021974329135
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.3085194934692491,-0.563938718316035,-2.108400115039594,0.016448794225385432,0.13990007202207771,0.3105786093327803
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# python3 robot_camera_calibration.py --config_file config/robot_zed_camera_calibration.yaml
WARNING - 2023-10-12 18:38:03,219 - rigid_transformations - autolab_core not installed as catkin package, RigidTransform ros methods will be unavailable
INFO     [_data_collector.py:142] Make sure you point the robot to a good initial                         configuration looking at the calibration tag
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_DANIILIDIS is: [[-0.99929219 -0.01292216 -0.03532908  0.05952314]
 [-0.01277403 -0.76678294  0.64177936 -0.2188662 ]
 [-0.03538291  0.6417764   0.76607513  0.06314412]
 [ 0.          0.          0.          1.        ]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([ 2.76434745 -9.86979657  5.75722915],[130.73247947  99.71161665  48.45092654])
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_HORAUD is: [[ 9.99472705e-01 -2.36764565e-02  2.22202025e-02  2.41210128e+15]
 [-2.36764565e-02 -9.99719639e-01 -2.63117199e-04 -1.49018579e+17]
 [ 2.22202025e-02 -2.63117199e-04 -9.99753066e-01  7.97629263e+15]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([101.57894737   0.           0.        ],[155.40166205   0.           0.        ])
/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_data_utils.py:26: UserWarning: Gimbal lock detected. Setting third angle to zero since it is not possible to uniquely determine all angles.
  rot_error = [difference_r.as](http://difference_r.as/)_euler('xyz', degrees=True)
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_PARK is: [[nan nan nan nan]
 [nan nan nan nan]
 [nan nan nan nan]
 [ 0.  0.  0.  1.]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([nan nan  0.],[nan nan  0.])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([nan nan nan],[nan nan nan])
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_TSAI is: [[ 1.00000000e+00 -3.25830441e-17  3.00673646e-18 -2.29293690e+15]
 [ 3.25830441e-17  1.00000000e+00 -7.57317884e-18  1.51590318e+17]
 [-3.00673646e-18  7.57317884e-18  1.00000000e+00 -8.11339294e+15]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([-99.21052632   0.           0.        ],[38.85041551  0.          0.        ])
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.3142142157497399,-0.281232321905755,-1.5769491028769251,-0.08505167671485281,0.10869832690184308,0.39249290501489786
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.28773136905876123,-0.2209121417232348,-0.829223978364923,-0.20964332708926126,0.017531313822759916,0.3672961434396176
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.2570181779444119,-0.09746567994537958,-1.0699099031113966,-0.15603926586472833,-0.027514982324621445,0.29307311079805554
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.21535897793733225,-0.8021849904215289,-2.6183627779871346,0.0804890806733553,0.14759499758703634,0.302972669369931
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.14409498246604852,-0.2052178522624006,-1.504308288031509,-0.11400544279069431,0.1049010721062046,0.362044018415615
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.10768996297912165,-0.13775094708694482,-1.5388697554875341,-0.09747698001689804,0.09620180060062024,0.2818964855091452
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.0761867753585027,0.1071534366043921,1.6475733127998262,0.18841268404313805,-0.11382140603083668,0.4070216521176688
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.19160444938074436,0.2583456645203763,1.65504216147805,0.19073127701996334,-0.0506569473583837,0.3367004287030583
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.6170967953795665,-0.12106721094334894,1.2168673382563386,0.12034627122336379,-0.10865791043168911,0.40943607485617844
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.09648597580536017,0.369950401393866,1.7875017050482902,0.10220678523705298,-0.044714769070031656,0.3207522259643255
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.2617917295063692,0.29022046091289866,1.2313526959379266,0.15115300797894204,-0.029634361001287492,0.38088352021160315
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.18329912971167342,0.1809686748966991,1.5395180231517518,0.14863225520732926,-0.07761643915144989,0.373717923052905
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.19528621108989774,0.08985369436358373,1.6527944705778939,0.15209171133351346,-0.15196760613214502,0.31990031388617146
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.297447071725141,0.24085158978157642,1.546094400268486,0.14347003109971654,-0.1026537255446094,0.28603619027253285
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.03862430333273748,0.6825080702572005,2.7623647704478467,0.11155853079526043,0.06213323505774098,0.3183670489239787
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.31360633716881847,0.0025016099403963443,1.1930977189119265,0.12625739421008036,-0.11892818921360301,0.39162148275970066
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.31020300715752364,-0.36940737908506005,-1.524298466857078,-0.13405980383137303,0.06831699191997168,0.36715485470681464
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.4660244603311664,-0.2176859173440443,-1.0497244832213528,-0.12445600909599651,0.05747638488640651,0.34198397139094505
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.5430288027947624,-0.15697679934028275,-0.5509116306435331,-0.12872590786435661,-0.005455670150886657,0.3376021974329135
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.3085194934692491,-0.563938718316035,-2.108400115039594,0.016448794225385432,0.13990007202207771,0.3105786093327803
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# python3 robot_camera_calibration.py --config_file config/robot_zed_camera_calibration.yaml
WARNING - 2023-10-12 18:38:03,219 - rigid_transformations - autolab_core not installed as catkin package, RigidTransform ros methods will be unavailable
INFO     [_data_collector.py:142] Make sure you point the robot to a good initial                         configuration looking at the calibration tag
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_DANIILIDIS is: [[-0.99929219 -0.01292216 -0.03532908  0.05952314]
 [-0.01277403 -0.76678294  0.64177936 -0.2188662 ]
 [-0.03538291  0.6417764   0.76607513  0.06314412]
 [ 0.          0.          0.          1.        ]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([ 2.76434745 -9.86979657  5.75722915],[130.73247947  99.71161665  48.45092654])
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_HORAUD is: [[ 9.99472705e-01 -2.36764565e-02  2.22202025e-02  2.41210128e+15]
 [-2.36764565e-02 -9.99719639e-01 -2.63117199e-04 -1.49018579e+17]
 [ 2.22202025e-02 -2.63117199e-04 -9.99753066e-01  7.97629263e+15]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([101.57894737   0.           0.        ],[155.40166205   0.           0.        ])
/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_data_utils.py:26: UserWarning: Gimbal lock detected. Setting third angle to zero since it is not possible to uniquely determine all angles.
  rot_error = [difference_r.as](http://difference_r.as/)_euler('xyz', degrees=True)
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_PARK is: [[nan nan nan nan]
 [nan nan nan nan]
 [nan nan nan nan]
 [ 0.  0.  0.  1.]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([nan nan  0.],[nan nan  0.])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([nan nan nan],[nan nan nan])
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_TSAI is: [[ 1.00000000e+00 -3.25830441e-17  3.00673646e-18 -2.29293690e+15]
 [ 3.25830441e-17  1.00000000e+00 -7.57317884e-18  1.51590318e+17]
 [-3.00673646e-18  7.57317884e-18  1.00000000e+00 -8.11339294e+15]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([-99.21052632   0.           0.        ],[38.85041551  0.          0.        ])

What are the data that is saved in the calib_data_file_name; from the documentation it not clear. Also, we were wondering why had the first 6 columns with fixed values?

[Ruthrash]

• I am curious why you are running realsense launch file when using zed camera.
• The config file is only an example, the user is expected to modify it based on the calibration tag that is being used. but if you think the default values has to be for the calibration target provided, that's fine, I will make that change and push it.
• We don't allow ros_tf right now due to the issue we discussed where the TF tree gets messed up when we hit the enable switch to make the robot go to status LED white mode. Until this is resolved, please use zmq pose collector provided.
  - This is also reflected by the same first 6 values in your data, the TF tree is not working as expected when the enabl switch is pressed down and the status LED is white, we also don't get any error which further causes confusion. you could also verify this by plotting the tf tree on RViz