Repository files navigation

A tool for estimating theiCubHeadCenter to RealSense transformation matrix when using the iCub RealSense holder.

• YARP
• iCub
• icub-contrib-common
• ViSP

  Note: if built from scratch, the CMake optionUSE_YARP can be safely disabled as it is not required for the software of this repository.

• yarp-devices-ros2

If you need to stream the transform between the robot root frame and the camera frame as a TF transform inROS 2, the devices from the above repositories should be part of your YARP installation. If streaming toROS 2 is not required, it is not required to install them.

git clone https://github.com/robotology/realsense-holder-calibration.gitmkdir buildcd buildcmake ../make install

Note: this repository use CMakeICUBcontribHelpers helpers and will automatically detect where to install the package. If therobotology-superbuild is used, the package will be installed in the superbuild install path.

1. Copy the templateyarpmanager application from<package_install_dir>/share/ICUBcontrib/templates/realsense-holder-calibration.xml to e.g..local/share/yarp/applications
2. Import the context of this package usingyarp-config context --import realsense-holder-calibration
3. Open the local configuration file.local/share/yarp/context/realsense-holder-calibration/config.ini and make sure that:
   • the camera intrinsics in[CAMERA_INTRINSICS] match those of the adopted RGB input
   • therobot_name is the correct one
   • theeye_version matches that of the used robot
   • suitable calibration poses are provided (the order istorso_yaw,torso_roll,torso_pitch,neck_pitch,neck_roll,neck_yaw)
   • the number of posesnumber_of_poses matches the actual number of poses

If aRealSense camera is used and it is accessed via the associatedyarpdev, it is possible to obtain the intrinsic parameters of the RGB sensor using

yarp rpc /depthCamera/rpc:i>visr get intp

If the robot of interest isiCubGenova01 the configuration filesconfig_iCubGenova01_holder_tilt.ini andconfig_iCubGenova01_holder_no_tilt.ini are available in the contextrealsense-holder-calibration after a succesfull installation of the package. They can be loaded by running the application using the parameter

--from config_iCubGenova01_holder[_no]_tilt.ini

Please check the intrinsic parameters of your camera, and eventually change them, before using these configuration files.

1. Copy the templateyarpmanager application from<package_install_dir>/share/ICUBcontrib/templates/realsense-holder-publisher.xml to e.g..local/share/yarp/applications

2. Import the context of this package usingyarp-config context --import realsense-holder-publisher

3. The local configuration file.local/share/yarp/context/realsense-holder-publisher/config.ini will contain:

   • the robot name
   • the desired update period of the module
   • the eye version of the robot
   • (optional) the absolute path to the calibration matrix file (produced by the modulerealsense-holder-calibration)

   Additionally, the following parameters are also available:

   • theuse_ros2 parameter which enable/disables streaming to ROS 2 as a TF transform
   • theros_src_frame_id that decides the parent id of the TF transform
   • theros_dest_frame_id that decides the child id of the TF transform

If the path of the calibration matrix file is not provided, the module will search for it in.local/share/yarp/context/realsense-holder-publisher/eMc.txt.

Print thechessboard provided byVISP. If possible print it on aA3 paper.

1. Make sure that iCub is up and running (the torso and the head are required)
2. Move in a folder where the data will be saved, e.g.cd <data_folder>
3. Open theyarpmanager from<data_folder>
4. Open theEye-hand_calibration application
5. Run all the applications and connect the ports

This will also run theyarpdev for the RealSense camera. If a different camera is used, please do not run theyarpdev and change the input port names in theyarpmanager window accordingly.

6. Open an RPC client viayarp rpc /realsense-holder-calibration/rpc:i
7. Typestart to start the data acquisition

Warning: the robot will execute the poses as provided in the configuration file and will wait forwait_time before moving to the next pose. Robot motion can be stopped at any time by typingstop in the RPC client. The parameterwait_time can be specified in the configuration file.

Note: Make sure that the chessboard is completely visibile in all calibration poses, e.g. by checking on theyarpview. If not, move the chessboard or change the torso and head joints configuration in the local configuration file, close the module by typingquit, re-open it and start the procedure again.

8. The robot will go back in home position (torso and neck set to zero) after the data acquisition is complete.

Open a terminal and runrealsense-holder-calibration-process having the following synopsis:

Synopsis: realsense-holder-calibration-process <path_to_visp_build> <path_to_images> <number_poses> <width> <height> <square_size (meters)>

where

• <path_to_visp_build> is theViSP build folder
• <path_to_images> is<data_folder> (as indicated in the sectionhow to collect data)
• <number_poses is the same as the parameternumber_of_poses in the configuration file
• <width> is the width of chessboard
• <height> is the height of the chessboard
• <square_size> is the length of the side of the square in the chessboard (in meters)

If the providedchessboard is used, then<width> = 9,<height> = 6 and<square_size (meters)> = 0.036 (assuming that it has been printed on a A3 paper). Please verify the length using a ruler given that the options of your printer might alter it.

After running the script, the list of collected images will be shown. Click using the left button of the mouse to move to the next pose. It is important that the chessboard is detected inall the images.

After all images have been considered, the script will provide the calibration matrix (from theiCubHeadCenter frame to the RealSense RGB frame) as output, e.g.

Transformation from iCubHeadCenter to RealSense RGB frame: 0.9986644239   0.03441962741 -0.03853125538 -0.05194061188-0.03017476666  0.9939304797   0.1057907607  -0.1212326766 0.0419386677  -0.1044867974   0.9936416141   0.02958705726 0              0              0              1

The output can also be found intxt andyaml format in<data_folder>/eMc.txt and<data_folder>/eMc.yaml. The latter provides the transformation as a 6-dimensional vector in the form(x, y, z, u_x, u_y, u_z) where(u_x, u_y, u_z) is the product between the axis and the angle of the axis/angle parametrization of the rotation matrix.

Variant no-tilt 0.9987039757   0.02941588291 -0.04153401947 -0.05439578875-0.02502241057  0.9944078874   0.102600353   -0.1173573506 0.04431983652 -0.1014280992   0.9938551669   0.02902883402 0              0              0              1

Variant tilt 0.9986796783   0.03750826048 -0.0351002926 -0.05794521346-0.001885871738 0.7095898996   0.7046123884 -0.05406200527 0.0513355981  -0.7036158787   0.7087237484  0.06424080642 0              0              0             1

1. Make sure that iCub is up and running (the torso and the head are required)
2. Open theyarpmanager
3. Open theEye-hand_calibration publisher application
4. Specify the desired configuration file in the parameters using--from <nome_of_config_file> if any
5. Runrealsense-holder-publisher

The pose of the camera will be available from/realsense-holder-publisher/pose:o as a list of<x> <y> <z> <axis_x> <axis_y> <axis_z> <angle> numbers where<x>,<y> and<z> are the 3D coordinates of the camera in the robot root frame, while the<axis_?> and<angle> are the axis/angle representation of the rotation matrix from the robot root frame to the camera reference frame.

If ROS is enabled, the pose will be streamed as a TF transform between the framesros_src_frame_id andros_dst_frame_id.

This repository is maintained by:

	@xenvre
	@gabrielecaddeo