Robotic Ultrasound Needle Placement and Tracking: Robot-to-Robot Calibration

Computer Integrated Surgery 2 Project - 5/5/16

Authors: Christopher Hunt <chunt11@jhu.edu>
         Matthew Walmer <mwalmer3@jhu.edu>
		 
This is a robot to robot calibration package, designed for two robotic
manipulators with an "eye-in-hand" configuration, i.e. a camera mounted on each
robot's end effector.
-------------------------------------------------------------------------------

DEPENDENCIES:
-MATLAB Computer Vision Toolbox
-eul2rotm.m (MATLAB Robotics Toolbox, included)
-tsai_calibration.m (CalTech Calibration Toolbox, included)
-rodrigues.m (CalTech Calibration Toolbox, included)

CONTENTS:
1) Checkerboard Calibration
2) ARToolKit Calibration
3) RGB-D Features and Depth Calibration
4) Error Analysis

-------------------------------------------------------------------------------
1) Checkerboard Calibration

Programs:
	- calibrate_handeye.m
		- Hand-eye calibration function. Computes the transformation from
		  the camera frame to the robot end-effector frame (gHc).
	- calibrate_checkerboard_script.m
		- Computes the robot to robot transformation (b1Hb2) using a
		  checkerboard image set from each robot.

Data:
	- Set of checkerboard images with corresponding robot end-effector pose
	  when image was taken, stored in a .xlsx file.
	- See example data sets located in "Checkerboard data" for file
	  structure.
		  
Instructions:
	1) Position the calibration checkerboard in the a position visible
	   to both robots. It is preferable that the board is roughly 600mm
	   from the robot bases, so that checkerboard images can be captured
	   at angles all around the board.
	2) Command robot 1 to a series of positions such that the entire
	   checkerboard is visible to the camera. At each position, capture
	   an image and log the pose of the robot end-effector with respect
	   to the robot base frame. At least 20 images should be taken, but
	   30 is recommended.
	3) Repeat step 2 with Robot 2 WITHOUT moving the calibration
	   checkerboard.
	4) Place your images and pose xlsx files in the "Checkerboard data"
	   folder or another folder of your choice. See example data for
	   how the files data should be structured.
	5) Open calibrate_checkerboard_script.m and set the file path to
	   your data on lines 14 and 15.
	6) Run calibrate_checkerboard_script.m. The final robot to robot
	   transformation is b1Hb2.


-------------------------------------------------------------------------------
2) ARToolKit Calibration

MATLAB Programs:
	- ATK_calibrate_handeye.m
		- Function to perform the hand-eye calibration using ARToolKit marker
		  pose data packaged in a .mat
	- ATK_calibrate_handeye_script.m
		- Simple shell script to run an ARToolKit hand-eye calibration
	- ATK_calibration_script.m
		- Script to compute the robot to robot calibration
	- ATK_get_marker_pose.m
		- Computes the average marker pose with respect to the robot base
		  frame using a specified hand-eye calibration.
	- ATK_process_raw_data.m
		- Script to process raw ARToolKit data (robot poses and marker
		  poses) into a .mat file.

ARToolKit Programs:
	- camera_calib.exe
		- A camera calibration program included with ARToolKit. Generates
		  a camera parameters fill compatible with other ARToolKit programs.
	- simpleLite.exe
		- An example program that comes with ARToolKit modified to print
		  the current marker pose with respect to the camera frame on a
		  "t" keypress. Works with the classic "Hiro" marker.
	- multi.exe
		- Another example ARToolKit program modified just like simpleLite.exe.
		  This program should be used to track multi-markers.

Data:
	- Raw data of robot poses was recorded in a xlsx file. Marker pose data
	  is output by the ARToolKit executables "simpleLite" and "multi"
	  pre-formatted into MATLAB syntax, to make it easy to copy and paste it
	  into a Matlab script.
	- ATK_process_raw_data.m is a script to process the robot pose xlsx file
	  and marker pose data into a .mat file, containing:
		- bHg - n x 1 cell, each containing a 4x4 homogenous transformation
                matrix representing a robot gripper to base frame
				transformation
        - cHm - n x 1 cell, each containing a 4x4 homogenous transformation
                matrix representing a marker frame to camera frame
				transformation
	- Some Matlab programs load or create a hand-eye transformation in a .mat
	  file. This transformation is a 4x4 homogenous transformation matrix named
	  gHc.

Instructions:
	1) Use ARToolKit's camera calibration program "calib_camera.exe" to
	   generate a camera_para.dat file for use with other ARToolKit programs.
	   Place camera_para.data in the "ARToolKit/bin/Data" folder. For more 
	   information visit:
	   http://artoolkit.org/documentation/doku.php?id=2_Configuration:config_camera_calibration
	2) Position the ARToolKit marker in the common workspace.
	3) If using a multimarker, copy the correct multimarker .dat file from
	   "ARToolKit/multimarker dat files" into "ARToolKit/bin/Data/multi".
	4) Connect robot 1's camera. Run "simpleLite.exe" if using the Hiro marker.
	   If using a multimarker, run "multi.exe".
	5) Command robot 1 to a number of positions such that the marker is visible
	   to the camera and the marker is detected. At each position, press "t" to
	   print the current marker pose (cHm) to the command line, and log the
	   robot end-effect pose (gHb). Once done, copy the marker pose data from
	   the command line to a plain text file or into ATK_process_raw_data.
	6) Use ATK_process_raw_data to generate a .mat file with the recorded data.
	7) Repeat the above procedure with robot 2 WITHOUT moving the calibration
	   marker.
	8) To generate a hand-eye calibration using this data, open
	   ATK_calibrate_handeye_script.m. Set the input data file paths and the
	   output file paths for the handeye.mat file, then run.
	9) To generate a robot to robot calibration, open 
	   ATK_calibrate_handeye_script.m. Set the input file paths for the
	   ARToolKit and robot pose data and the hand-eye transformation for each
	   robot. After running, the final calibration is b1Hb2.


-------------------------------------------------------------------------------
3) RGB-D Features and Depth Calibration


Programs:
	- calibrate_handeye.m
		- Handeye calibration function. Computes the transformation from
		  the camera frame to the robot end-effector frame (gHc).
	- calibrate_surf_script.m
		- Computes the base to base transformation between two robots using
		  3D extrapolated SURF features from a common calibration scene
Data:
	- Set of checkerboard images with corresponding robot end-effector pose
	  when image was taken, stored in a .xlsx file.
	- Set of calibration scene images with corresponding robot end-effector
	  pose when image was taken, stored in a .xlsx file.

Instructions:
	1) Position the calibration checkerboard in the a position visible
	   to both robots. It is preferable that the board is roughly 600mm
	   from the robot bases, so that checkerboard images can be captured
	   at angles all around the board.
	2) Command robot 1 to a series of positions such that the entire
	   checkerboard is visible to the camera. At each position, capture
	   an image and log the pose of the robot end-effector with respect
	   to the robot base frame. At least 20 images should be taken, but
	   30 is recommended.
	3) Repeat step 2 with Robot 2 WITHOUT moving the calibration
	   checkerboard.
	4) Set up a calibration scene. The scene should be random objects 
           with distinct visual features in a position visible to both 
           robots.
	5) Command robot 1 to a series of positions such that the entire
	   scene is visible to the camera. At each position, capture
	   an image and log the pose of the robot end-effector with respect
	   to the robot base frame. A minimum of 1 image must be taken, 
	   however, calibration accuracy increases noticeably with 
          increased images taken.
	   
	6) Repeat step 5 with Robot 2 WITHOUT moving the calibration scene.

	4) Place your images and pose xlsx files in the “SURF data"
	   folder or another folder of your choice. See example data for
	   how the files data should be structured.
	5) Open calibrate_surf_script.m and set the correct variables for 
          lines 22 - 28. These variables include the file paths where data
          is stored and tuning variables.
	6) Run calibrate_surf_script.m. The final robot to robot
	   transformation is b1Hb2.

-------------------------------------------------------------------------------
4) Error Analysis

Programs:
	- error_test_script.m
		- A script to compare robot to robot transformations from any of the
		  above methods to a ground truth transformation. This function also
		  prints a visualization of the error.

Data:
	- Ground truth points created by fitting both robots with pointers and
	  positioning them so both pointer tips are touching. For an example
	  ground truth data set, see "data/GT_pointer_data_16_04_24.mat".
	- A homogenous robot-to-robot transformation, generated by one of the
	  above calibration methods, stored in variable "H".
	  
Instructions:
	1) Generate ground truth points. First perform a pivot calibration for
	   each of the robot's pointers. We used KUKA's built in 4 point pivot
	   calibration.
	2) Command robot 1 to an arbitrary position in the workspace which can
	   be reached by robot 2.
	3) Manually command robot 2 to a position such that the two pointer tips
	   are exactly touching. Record both robots poses.
	4) Repeat this processes several times, sampling random positions
	   distributed evenly around the workspace.
	5) Store the ground truth point data in a .mat file. See
	   "data/GT_pointer_data_16_04_24.mat" for reference.
	6) Open error_test_script.m. Set the path for the ground truth points
	   .mat file. Set variable H to the calibrated transformation to test
	   and then run. Additionally variables can be altered to configure the
	   error visualization diagram.