Test camera in environment calibration

[kouroshD]

Test We use aruco board: we place the ee at the origin of the board; we check the FK (TF) or ee pose to be similar to the one measured by the camera.

[kouroshD]

To this end, to first create the calibration tag, print the PDF provided in calibration/calibration_tag directory.

• [x] [ calibration/calibration_tag directory]() here it is better to link to the folder.

You could use the (to be) provided CAD files for the finger tips and gripping points.

• [x] If this is not provied yet, can we add it as a work to do in our task follow-up documentation

. Now make the Franka End-effector with custom finger tips grasp the calibration tag with the attached custom gripping points,

Having a figure here would help the user to understand better.

[kouroshD]

We had to stop sudo docker-compose -f docker/workstation_computer/docker-compose-gui.yml up. After running it again, all of the changes that were made from previous weekes were gone and only we had bare-minimum docker running as wirtten in the Dockerfile. We had to even generate new public ssh-keys and etc.... @Ruthrash

[n-yoshikawa]

We have tested camera calibration, but encountered some problems.

1. If we restart the docker with sudo docker-compose -f docker/workstation_computer/docker-compose-gui.yml up, the container was initialized and we had to set up the environment again. To avoid initialization, I used docker start <container_id> instead of docker compose. I don't think the initialization by docker compose is the expected behavior, but I haven't checked the documentation.
2. If we unplug and plug the camera after the docker container is started, the camera is not recognized. We need to restart the docker container if you unplug the camera after starting the container. When the camera is not recognized, you will see the following errors when you run roslaunch realsense2_camera rs_camera.launch.

   [ WARN] [1680641802.255076679]: Device 1/1 failed with exception: failed to set power state
   [ERROR] [1680641802.255096532]: The requested device with serial number 141222070426 is NOT found. Will Try again.

3. Even if the camera is recognized, rostopic echo /camera/color/image_raw showed nothing. I saw the following warning and error in roslaunch realsense2_camera rs_camera.launch:

   [ WARN] [1680641547.537678384]: Hardware Notification:USB CAM overflow,1.68064e+12,Error,Hardware Error
   04/04 20:52:28,629 WARNING [140348000343808] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11

   I tried roslaunch realsense2_camera rs_camera.launch initial_reset:=true, which was written in a realsense-ros issue#1663, but it didn't solve the problem and I had the similar errors when the camera is not recognized.

[Ruthrash]

I believe error 3 happens when the camera is not connected to a USB 3.0 port. can you confirm if the camera is indeed connected to USB 3.0?

[kouroshD]

I believe error 3 happens when the camera is not connected to a USB 3.0 port. can you confirm if the camera is indeed connected to USB 3.0?

We have connected it to a USB-C. Dp you know what changes connecting to a usb-c or usb-3.

[Ruthrash]

We have tested camera calibration, but encountered some problems.

1. If we restart the docker with sudo docker-compose -f docker/workstation_computer/docker-compose-gui.yml up, the container was initialized and we had to set up the environment again. To avoid initialization, I used docker start <container_id> instead of docker compose. I don't think the initialization by docker compose is the expected behavior, but I haven't checked the documentation.
2. If we unplug and plug the camera after the docker container is started, the camera is not recognized. We need to restart the docker container if you unplug the camera after starting the container. When the camera is not recognized, you will see the following errors when you run roslaunch realsense2_camera rs_camera.launch.

[ WARN] [1680641802.255076679]: Device 1/1 failed with exception: failed to set power state
[ERROR] [1680641802.255096532]: The requested device with serial number 141222070426 is NOT found. Will Try again.

3. Even if the camera is recognized, rostopic echo /camera/color/image_raw showed nothing. I saw the following warning and error in roslaunch realsense2_camera rs_camera.launch:

[ WARN] [1680641547.537678384]: Hardware Notification:USB CAM overflow,1.68064e+12,Error,Hardware Error
 04/04 20:52:28,629 WARNING [140348000343808] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11

I tried roslaunch realsense2_camera rs_camera.launch initial_reset:=true, which was written in a realsense-ros issue#1663, but it didn't solve the problem and I had the similar errors when the camera is not recognized.

Perhaps the issue is related to this discussion- https://github.com/IntelRealSense/realsense-ros/issues/669

[n-yoshikawa]

roslaunch realsense2_camera rs_camera.launch filters:=pointcloud enable_infra1:=false enable_infra2:=false

worked!

[Ruthrash]

To this end, to first create the calibration tag, print the PDF provided in calibration/calibration_tag directory.

• [x] calibration/calibration_tag directory here it is better to link to the folder.

You could use the (to be) provided CAD files for the finger tips and gripping points.

• [x] If this is not provied yet, can we add it as a work to do in our task follow-up documentation

. Now make the Franka End-effector with custom finger tips grasp the calibration tag with the attached custom gripping points,

Having a figure here would help the user to understand better.

• CAD files provided here: https://github.com/pairlab/franka_arm_infra/tree/master/calibration/models_4_3d_printing

• updated README with images

[n-yoshikawa]

This is how we ran the calibration:

roslaunch realsense2_camera rs_camera.launch filters:=pointcloud enable_infra1:=false enable_infra2:=false

cd ~/git/frankapy/catkin_ws/src/franka_control
python3 ee_robotiq_transformation.py

cd ~/git/calibration
python3 ROSCameraRobotCalibration.py --camera_in_hand False --robot_base_frame_name /panda_link0 --ee_frame_name /virtual_eef_link --rgb_image_topic /camera/color/image_raw --rgb_camera_info_topic /camera/color/camera_info

[n-yoshikawa]

Here is the result of the camera in env calibration:

stopping data collection
solver =  0
full dataset error (9.552241969799356, 16.718924381170783)
[[ 0.9954175  -0.0380757  -0.08771688]
 [ 0.06608569  0.93691486  0.34325387]
 [ 0.06911361 -0.34747774  0.9351377 ]]
[ 2.96567526 -0.0891181   1.3772578 ]
solver =  1
full dataset error (9.238283512492595, 70.71164689693629)
[[-0.8499093  -0.31467202  0.42265317]
 [-0.10805919 -0.68097394 -0.72429117]
 [ 0.51572996 -0.66125336  0.54476288]]
[2.92724207 0.34053585 0.02981043]
solver =  2
full dataset error (9.238245824861705, 72.65764896316963)
[[-0.85224683 -0.31481655  0.41781082]
 [-0.10522777 -0.67917465 -0.72639446]
 [ 0.51244751 -0.66303267  0.545697  ]]
[2.92988195 0.34062296 0.0302995 ]
solver =  3
Traceback (most recent call last):
  File "ROSCameraRobotCalibration.py", line 204, in <module>
    main(args)
  File "ROSCameraRobotCalibration.py", line 175, in main
    calibration_object.run_robot_camera_calibration()
  File "/root/git/calibration/base_robot_camera_calibration.py", line 108, in run_robot_camera_calibration
    backend.Run()
  File "/root/git/calibration/backend.py", line 87, in Run
    rot, trans = cv2.calibrateHandEye(A_rot, A_trans, B_rot, B_trans,method=self.solver)
cv2.error: OpenCV(4.7.0) /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'

• there was an error for solver 3
• to us, it seems the calibration values are not correct, at least based on the translation terms cc @Ruthrash

[Ruthrash]

This error happens when the relative transforms you have collected are parallel in atleast on axis. could you share with me the pose data that was collected?

[kouroshD]

here are the images of the testing setup:

[n-yoshikawa]

This error happens when the relative transforms you have collected are parallel in atleast on axis. could you share with me the pose data that was collected?

Here is the full terminal outputs:

 python3 ROSCameraRobotCalibration.py --camera_in_hand False --robot_base_frame_name /panda_link0 --ee_frame_name /virtual_eef_link --rgb_image_topic /camera/color/image_raw --rgb_camera_info_topic /camera/color/camera_info^C
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git# cd calibration/
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/calibration# python3 ROSCameraRobotCalibration.py --camera_in_hand False --robot_base_frame_name /panda_link0 --ee_frame_name /virtual_eef_link --rgb_image_topic /camera/color/image_raw --rgb_camera_info_topic /camera/color/camera_info
WARNING:root:autolab_core not installed as catkin package, RigidTransform ros methods will be unavailable
waiting for image msg
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 1.3813359438730828
#### Very high reprojection error ####
#### Ignoring this sample ####
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 1.241068647508526
#### Very high reprojection error ####
#### Ignoring this sample ####
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 1.0641597671661873
#### Very high reprojection error ####
#### Ignoring this sample ####
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.8836595105270262
#### Very high reprojection error ####
#### Ignoring this sample ####
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.815832342957489
#### Very high reprojection error ####
#### Ignoring this sample ####
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.22723072597372468
accepted pairs of pose, no. of frames processed 1 6
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.33153588489901875
relative calibration tag rotation in degrees, translation in m:  [-1.2024009  20.00299629  0.35347125] [0.05131023 0.00022347 0.06230053]
#### Very high reprojection error ####
#### Ignoring this sample ####
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.16985625731679718
relative calibration tag rotation in degrees, translation in m :  [-6.73301083 -6.41343573  3.77293284] [0.01049362 0.01183462 0.07889703]
accepted pairs of pose, no. of frames processed 2 8
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.1950653420957561
relative calibration tag rotation in degrees, translation in m :  [-12.82490246  -7.69574873  -0.45607807] [-0.03603338  0.03933149  0.01088893]
accepted pairs of pose, no. of frames processed 3 9
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.24638505830780752
relative calibration tag rotation in degrees, translation in m :  [32.879323    9.00549901  5.92358635] [ 0.00793196 -0.077561   -0.13750455]
accepted pairs of pose, no. of frames processed 4 10
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.388222031631212
relative calibration tag rotation in degrees, translation in m:  [ 0.52801691  1.63640312 -0.84240258] [-0.01357964 -0.01309072  0.01058603]
#### Very high reprojection error ####
#### Ignoring this sample ####
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.17583238573863225
relative calibration tag rotation in degrees, translation in m :  [-6.56456193 -4.24948288  0.61915019] [0.01634206 0.07400556 0.01387593]
accepted pairs of pose, no. of frames processed 5 12
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 1.1962482544961919
relative calibration tag rotation in degrees, translation in m:  [-3.1813132   6.10674304 -1.24040313] [-0.00405161 -0.04957628  0.15376658]
#### Very high reprojection error ####
#### Ignoring this sample ####
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.25638112807656765
relative calibration tag rotation in degrees, translation in m :  [-3.2089141  -4.98681325  0.86697922] [ 0.00351404 -0.00750603 -0.04202231]
accepted pairs of pose, no. of frames processed 6 14
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.24916467279681886
relative calibration tag rotation in degrees, translation in m :  [10.6739569   8.19262796  1.43044171] [-0.00098166 -0.10738971  0.00567211]
accepted pairs of pose, no. of frames processed 7 15
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.2795162768276547
relative calibration tag rotation in degrees, translation in m :  [-13.88319635 -28.21504923   0.27418206] [-0.05540528  0.08150029 -0.02048596]
accepted pairs of pose, no. of frames processed 8 16
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.2478852556377724
relative calibration tag rotation in degrees, translation in m :  [ 3.85363795 11.53651378  1.15539187] [0.02248981 0.00343801 0.00809554]
accepted pairs of pose, no. of frames processed 9 17
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.18431366634554044
relative calibration tag rotation in degrees, translation in m :  [1.790684   8.16185175 1.17394452] [0.05624631 0.00706151 0.00447045]
accepted pairs of pose, no. of frames processed 10 18
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.3107893084819464
relative calibration tag rotation in degrees, translation in m:  [ -3.20380762 -22.4439979    1.19149676] [-0.03142517 -0.00911154 -0.00626972]
#### Very high reprojection error ####
#### Ignoring this sample ####
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.28011229635929613
relative calibration tag rotation in degrees, translation in m :  [ 3.54518802 20.91082437 -2.0658343 ] [-0.02849471  0.0163454   0.00300353]
accepted pairs of pose, no. of frames processed 11 20
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.30155811214997214
relative calibration tag rotation in degrees, translation in m:  [-6.467774    0.35673467 -1.90453357] [-0.01687847  0.09207677  0.0149727 ]
#### Very high reprojection error ####
#### Ignoring this sample ####
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.22530397495931076
relative calibration tag rotation in degrees, translation in m :  [  1.29270986 -12.92273871   1.09242792] [ 0.00351916 -0.0586572   0.00367277]
accepted pairs of pose, no. of frames processed 12 22
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.2570613143450217
relative calibration tag rotation in degrees, translation in m :  [-22.21085547 -18.24382798  -1.71231708] [-0.06932019  0.1174323   0.03566144]
accepted pairs of pose, no. of frames processed 13 23
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.1751038221190834
relative calibration tag rotation in degrees, translation in m :  [14.25880313 10.04266708  6.0076276 ] [ 0.05016543 -0.02638049 -0.0581339 ]
accepted pairs of pose, no. of frames processed 14 24
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.3508567386873099
relative calibration tag rotation in degrees, translation in m:  [-20.14685646  -6.78175823   0.47058197] [-0.03858481  0.12043452  0.08709191]
#### Very high reprojection error ####
#### Ignoring this sample ####
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.19332118637641038
relative calibration tag rotation in degrees, translation in m :  [4.97187364 4.91919075 1.20209389] [ 0.03855473 -0.08831447  0.01612733]
accepted pairs of pose, no. of frames processed 15 26
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.26192004541778596
relative calibration tag rotation in degrees, translation in m :  [ -3.39310741 -15.01395735  -0.45849301] [-0.05116302  0.05958535 -0.05061618]
accepted pairs of pose, no. of frames processed 16 27
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.19499710321794186
relative calibration tag rotation in degrees, translation in m :  [ 1.29027956 16.08215418 -0.50166293] [ 0.04459871 -0.00742759  0.02512873]
accepted pairs of pose, no. of frames processed 17 28
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.20464630306478632
relative calibration tag rotation in degrees, translation in m :  [ 5.99853955  7.56615611 -1.01745008] [-0.02552202 -0.05473754 -0.00438805]
accepted pairs of pose, no. of frames processed 18 29
Press Enter to continue collecting current sample....else space bar to stop
reprojection error 0.18590727791345654
relative calibration tag rotation in degrees, translation in m :  [ -9.12296669 -14.63264714   0.49294842] [-0.01873609  0.07032454  0.00771376]
accepted pairs of pose, no. of frames processed 19 30
Press Enter to continue collecting current sample....else space bar to stop  
stopping data collection
solver =  0
full dataset error (9.552241969799356, 16.718924381170783)
[[ 0.9954175  -0.0380757  -0.08771688]
 [ 0.06608569  0.93691486  0.34325387]
 [ 0.06911361 -0.34747774  0.9351377 ]]
[ 2.96567526 -0.0891181   1.3772578 ]
solver =  1
full dataset error (9.238283512492595, 70.71164689693629)
[[-0.8499093  -0.31467202  0.42265317]
 [-0.10805919 -0.68097394 -0.72429117]
 [ 0.51572996 -0.66125336  0.54476288]]
[2.92724207 0.34053585 0.02981043]
solver =  2
full dataset error (9.238245824861705, 72.65764896316963)
[[-0.85224683 -0.31481655  0.41781082]
 [-0.10522777 -0.67917465 -0.72639446]
 [ 0.51244751 -0.66303267  0.545697  ]]
[2.92988195 0.34062296 0.0302995 ]
solver =  3
Traceback (most recent call last):
  File "ROSCameraRobotCalibration.py", line 204, in <module>
    main(args)
  File "ROSCameraRobotCalibration.py", line 175, in main
    calibration_object.run_robot_camera_calibration()
  File "/root/git/calibration/base_robot_camera_calibration.py", line 108, in run_robot_camera_calibration
    backend.Run()
  File "/root/git/calibration/backend.py", line 87, in Run
    rot, trans = cv2.calibrateHandEye(A_rot, A_trans, B_rot, B_trans,method=self.solver)
cv2.error: OpenCV(4.7.0) /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/calibration# python3 ROSCameraRobotCalibration.py --camera_in_hand False --robot_base_frame_name /panda_link0 --ee_frame_name /virtual_eef_link --rgb_image_topic /camera/color/image_raw --rgb_camera_info_topic /camera/color/camera_info^C

I didn't see if any data has been logged in a file. But this should help with your requested data.

[kouroshD]

• @Ruthrash will try on his end first. @n-yoshikawa and I will run again the experiment with larger rotations greater than 20 deg relative orientation, it should be OK.