A ROS package to perform camera registration for the dVRK robot.
Setup notes This script was tested on ROS noetic and the dVRK release 2.2.1+
On Ubuntu 20.04 you might need to compile gscam (see notes in dvrk_video package: https://github.com/jhu-dvrk/dvrk_video). You might also have to install scipy with pip3 in your user directory using pip3 install --user scipy==1.4.0.
The goal of this package is to perform an hand-eye calibration between a dVRK PSM arm and the camera, ideally a stereo endoscope. This can be specially useful for groups that don't have a full patient cart or a SUJ controller. This script will tell you where the PSM is located with respect to the camera or ECM. This will allow you to use the teleoperation components provided along the dVRK stack. This code can also be used instead of the SUJ since the accuracy can be higher than the SUJs (SUJs are within 5cm cube reporting PSMs wrt ECM, the camera registration script seems to achieve an accuracy closer to 5mm to 10mm cube).
Most of the dVRK groups have an original endoscope and CCUs from Intuitive Surgical Inc with SDI outputs so we will assume a stereo camera even though this hand-eye calibration uses only one channel (mono). The registration uses a small ArUco marker mounted on the shaft of the instrument (you can find an STL model for the mount in the assets directory as well as ArUco .svg files). The ArUco marker can be removed once the registration is performed. The overall steps are:
- Calibrate the camera
- Run the camera registration script
- Manually move the PSM to safe locations to define the boundaries of the search space
- The code will compute a convex hull based on the user provided poses
- Let the code move the PSM around to collect data while not colliding with the environment
- Optionally, validate the results
- Copy-paste the resulting transformation to the dVRK console or your suj-fixed.json
There are two specific cases:
- Fixed camera. In this case, the registration transformation can be used in your console.json to define the
base-frame. - Camera held by and ECM. In this case, you should have a console.json that uses an SUJ of type
SUJ_Fixed. The SUJ has a "kinematic" configuration file calledsuj-fixed.jsonby convention. The result of the registration will need to be copy-pasted in this file. Example of configuration files using the Fixed SUJ can be found in https://github.com/dvrk-config/dvrk_config_jhu/tree/main/jhu-daVinci-Si.
The camera registration script will automatically save the correct transformation based on the -e command line option. If you specify an ECM, the script assume you are using the fixed SUJ. Otherwise, it assumes you just need the PSM base frame for the console.json.
You will need to create a new package to store the calibration results. At that point, choose a name for your camera (aka stereo rig). For this example, we will use jhu_daVinci:
cd ~/catkin_ws/src
catkin_create_pkg jhu_daVinci
catkin build
source ~/catkin_ws/devel/setup.bash # you have a new package so you need to source againThere is no difference but for sake of demonstration, we will create the package under a different name, say "depstech", a familiar brand of cheap USB cameras.
cd ~/catkin_ws/src
catkin_create_pkg depstech
catkin build
source ~/catkin_ws/devel/setup.bash # you have a new package so you need to source againThe following is based on dVRK provided launch files to capture the stereo video using Decklink SDI frame grabbers. If your frame grabbers are different you will have to create your own launch files. Note that you have to provide the name of your stereo rig:
roslaunch dvrk_video stereo_decklink_1280x1024.launch stereo_rig_name:=jhu_daVinciThe following example is based on a video4linux compatible source:
roslaunch dvrk_video gscam_v4l.launch camera_name:=depstechThe calibration is performed using the ROS provided application, please refer to their documentation for the parameters (http://wiki.ros.org/camera_calibration). You need to make sure the video stream is started and you are using the correct rig name.
rosrun camera_calibration cameracalibrator.py --approximate 0.1 --size 12x10 --square 0.0045 right:=/jhu_daVinci/right/image_raw left:=/jhu_daVinci/left/image_raw left_camera:=/jhu_daVinci/left right_camera:=/jhu_daVinci/rightThe command line above assumes you're using a 12x10 calibration grid and the size of each square is 4.5mm. Once the calibration is performed, don't forget to save and commit using the GUI. You should now have two new files in the catkin package you created for the stereo rig under calibrations: left.yaml and right.yaml.
You can find some calibration checkerboards ready to print in the assets directory. Make sure the scale is correct after printing. The simplest solution is to measure 10 consecutive squares and verify that it's 10 times the claimed square size in mm. Also to note, the checkerboard size (e.g. 12x10) is based on the number of corners between the squares. For example, the 12x10 actually has 13x11 squares.
The only difference is the parameters:
rosrun camera_calibration cameracalibrator.py --size 12x10 --square 0.0045 image:=/depstech/image_raw camera:=/depstechAt that point, you need to stop the launch file used for the video acquisition and restart it with the stereo_proc parameter set to True. This will add a ROS node to compute the rectified images and publish the camera parameters needed for the camera registration.
roslaunch dvrk_video stereo_decklink_1280x1024.launch stereo_rig_name:=jhu_daVinci stereo_proc:=Trueroslaunch dvrk_video gscam_v4l.launch camera_name:=depstech mono_proc:=TrueStart a dVRK console with the PSM you mean to register. If you have an ECM and want to register it as well, you need a console file with at least the PSM you want to register as well as the ECM. We strongly recommend to use a console.json file with no base frame set for the PSM and ECM. If you're using the Fixed SUJ, make sure the transformation in suj-fixed.json are all set to identity.
To perform the hand-eye calibration, you need to attach the calibration ArUco marker to the PSM shaft, ideally about 70mm from the tip. The exact position doesn't matter. You can 3D print the ArUco mount provided in the assets directory to hold the ArUco marker. You will need to tap the smaller hole and use a screw to push against the instrument's shaft.
The camera_registration.py script first asks the user to move a the PSM to create a convex hull in which it is safe to move. During this stage, the PSM should be powered and you can use the clutch button to free the arm. Move the tip without worrying if the ArUco marker is visible and try to create the largest volume possible in front of the camera. When you're done, place the PSM so the ArUco marker is facing the camera and is close to the center of the volume you just created. The script will then move the robot automatically to collect data for the hand-eye calibration. To run the script:
rosrun dvrk_camera_registration camera_registration.py -p PSM2 -m 0.01 -c /jhu_daVinci/left-p is used to indicate which PSM to use and -c is the namespace for your camera. If you also have an ECM, add -e ECM:
rosrun dvrk_camera_registration camera_registration.py -p PSM2 -m 0.01 -c /jhu_daVinci/left -e ECMAfter collecting the data, the script will generate a couple of .json files with the transformation between the camera PSM and the camera. The file with -open-cv is used for the validation script below.
We also provide a script that overlays the estimated pose of the PSM end-effector based on the telemetry. You can run this script right after the camera registration. Make sure the video is still running with stereo_proc set to True.
rosrun dvrk_camera_registration vis_gripper_pose.py -p PSM2 -c /jhu_daVinci/left -H PSM2-registration-open-cv.jsonIf you don't have an ECM (i.e. a fixed camera), the registration scripts creates a PSM<x>_registration-dVRK.json file that contains a transformation you can copy-paste in your dVRK console.json to define the PSM base-frame. If you have an ECM, the content of PSM<x>_registration-dVRK.json needs to be copy-pasted in the kinematic file for the Fixed SUJ (aka suj-fixed.json).