About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Xtion/Kinect Camera TF Calibration

Description: Explains multiple ways of vision-based calibration for coordinates between head-mount camera and hands. With an advanced method you can even fine-tune joint offsets.

Tutorial Level: ADVANCED

Next Tutorial: Install or update software on ControllerBox

Introduction

Two different approaches are discussed in this page for HiroNXO's whole-body calibration (i.e. joint-level calibration is done at proprietary module level).

• Using Xtion/Kinect, cameras used in the opensource community as a standard depth sensor.

• (TBD) Headmount cameras HiroNXO comes with by default.

As always, we recommend any new feature to be tried on simulator first before the real robot. Use Gazebo to confirm it works then try it on the real robot.

Large part of this tutorial page uses NEXTAGE as an example. Replace as needed as always.

Xtion/Kinect Camera TF Calibration

Note that the work progress while writing this section can be seen in this ticket.

Xtion/Kinect on Gazebo Simulation

Since Xtion/Kinect is well intergrated to ROS/Gazebo, it is also easy for HiroNXO to incorporate those sensors on the simulation.

Launch Gazebo with Nextage robot model

$ roslaunch nextage_gazebo nextage_world.launch

Spawn models of Kinect and chekcerboard

After the robot finished the initial motion in Gazebo, launch gazebo_kinect_checkerboard_chest.launch or gazebo_kinect_checkerboard_chest.launch to spawn Kinect and the checkerborad models in Gazebo.

$ roslaunch nextage_calibration gazebo_kinect_checkerboard_chest.launch   #chest
or
$ roslaunch nextage_calibration gazebo_kinect_checkerboard_waist.launch   #waist

Run checkerboard detector

$ roslaunch nextage_calibration camera_checkerboard_chest.launch launch_openni2:=false
or
$ roslaunch nextage_calibration camera_checkerboard_waist.launch launch_openni2:=false

Run RViz

$ rviz -d `rospack find nextage_calibration`/conf/calibration.rviz

Get TF between camera link and parent link

Run tf_echo to get TF between camera link and parent link.

tf_echo for TF from camera_link to HEAD_JOINT1_Link:

$ rosrun tf tf_echo HEAD_JOINT1_Link camera_link

Example of tf_echo result:

At time 93.641
- Translation: [0.101, -0.014, 0.199]
- Rotation: in Quaternion [-0.001, 0.704, 0.002, 0.710]
            in RPY (radian) [0.126, 1.563, 0.130]
            in RPY (degree) [7.223, 89.561, 7.477]

After everything finished, press Ctrl-C to quit all nodes.

Xtion/Kinect on a real robot

Get the robot ready

Since we use ROS, run rtm_ros_bridge on your PC (tutorial).

$ roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch nameserver:=%HOSTNAME%

Run joint-level calibration. After the encoders check has finished, move the arms to the off-pose since we don't need them for now.

$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py -- --host nextage

:
(many connection messages)
:

[hrpsys.py] initialized successfully

:

In [1]: robot.checkEncoders()
In [2]: robot.goOffPose()
In [3]: robot.servoOff()

Install checkerboard on the robot

First, you must confirm the servo has turned off.

And then install a checkerboard on chest or waist of Nextage robot as shown below.

Checkerboard on chest

Checkerboard on waist

Turn on servo to keep the angle of CHEST_JOINT0 as 0

You should keep the angle of CHEST_JOINT0 as 0 [deg] when you use the checkerboard on chest.

In [1]: robot.servoOn()
In [2]: robot.goInitial()

Turn the camera to the checkerboard

Turn the camera so that the checkerboard appears using joint control.

Example of commands for using joint control

In [1]: robot.setJointAnglesOfGroup( 'head', [ 0.0, 45.0 ], 5.0 )
In [2]: robot.getJointAngles()

Run checkerboard detector

Xtion

$ roslaunch nextage_calibration camera_checkerboard_chest.launch  #chest
or
$ roslaunch nextage_calibration camera_checkerboard_waist.launch  #waist

Kinect

$ roslaunch nextage_calibration camera_checkerboard_chest.launch launch_openni2:=false  #chest
or
$ roslaunch nextage_calibration camera_checkerboard_waist.launch launch_openni2:=false  #waist

Checkerboard on chest

Checkerboard on waist

Run RViz

Run RViz for checking the operation status of the each functions.

$ rviz -d `rospack find nextage_calibration`/conf/calibration.rviz

RViz with checkerboard on chest

RViz with checkerboard on waist

Getting TF between camera link and parent link

Run tf_echo to get TF between camera link and parent link.

tf_echo for TF from camera_link to HEAD_JOINT1_Link

$ rosrun tf tf_echo HEAD_JOINT1_Link camera_link

Example of tf_echo result

At time 93.641
- Translation: [0.101, -0.014, 0.199]
- Rotation: in Quaternion [-0.001, 0.704, 0.002, 0.710]
            in RPY (radian) [0.126, 1.563, 0.130]
            in RPY (degree) [7.223, 89.561, 7.477]

Go off-pose and Turn off the servo

Move the arms to the off-pose and turn off the servo.

In [1]: robot.goOffPose()
In [2]: robot.servoOff()

Quit all nodes

Ctrl-C to quit all nodes.

Use of the aquired camera link TF

Finally, this step is the reason why we've gone through all the steps above -- let's apply the extrinsic camera parameters!

Apply the TF values of the real robot Xtion/Kinect camera through xtion_kinect.launch as arguments as below.

( Units: TF_COORD_XYZ [m], TF_COORD_RPY [rad] )

First don't forget to run rtm_ros_bridge on your Ubuntu PC.

$ roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch nameserver:=%HOSTNAME%

For Xtion:

$ roslaunch nextage_calibration xtion_kinect.launch TF_COORD_XYZ:='0.101 -0.014 0.199' TF_COORD_RPY:='0.126 1.563 0.130'

For Kinect, only difference is that the argument launch_openni2 needs to be false:

$ roslaunch nextage_calibration xtion_kinect.launch TF_COORD_XYZ:='0.101 -0.014 0.199' TF_COORD_RPY:='0.126 1.563 0.130' launch_openni2:=false

Optional calibration for Xtion camera

When your camera has no calibration infomation, you can still run camera calibration using the method in this page.

Calibrate Head-mount Camera and Hands coordinates (Work in progess)

Overview

(Feb 2017 update There's a claim that the methods described in this page is a bit too advanced. There's an ongoing work to provide calibration package so that users of HiroNXO wouldn't have to bother the same task.)

You'll learn/create:

• how to run head-mount camera ROS nodes that return pointclouds.

• Hand-eye calibration

• a modified URDF with links supporting cameras. And if you wish, you can put the calibration data into a new dedicated package.

Get the image and pointcloud from head-mount camera

You can get the output from HiroNXO's default head-mount camera via ROS by the following.

(TBD)

Hand-eye only calibration

(TBD)

More elaborated calibration

Before Calibration

Manually edit the robot's URDF (hironx_moveit_config/models/HiroNX.urdf for Hironx | nextage_description/urdf/NextageOpen.urdf for NEXTAGE Open) temporarily to add camera frames, joints and links.

Here's an example of HiroNX.urdf. The coordinate depends on your robot's setting.

+++ b/hironx_moveit_config/models/HiroNX.urdf
        @@ -652,4 +652,37 @@
             <limit lower="-0.523599" upper="0.523599" effort="100"
        velocity="0.5" />
             <dynamics damping="0.2" friction="0" />
           </joint>
        +
        +  <link name="camera_link"/>
        +  <link name="camera_depth_frame"/>
        +  <link name="camera_rgb_frame"/>
        +  <link name="camera_depth_optical_frame"/>
        +  <link name="camera_rgb_optical_frame"/>
        +
        +  <joint name="head_to_kinect_joint" type="fixed">
        +    <origin rpy="0 0 0" xyz="0.05 0.08 0.1"/>
        +    <parent link="HEAD_JOINT1_Link" />
        +    <child link="camera_link"/>
        +  </joint>
        +  <joint name="camera_base_link" type="fixed">
        +    <origin rpy="0 0 0" xyz="0 -0.02 0"/>
        +    <parent link="camera_link"/>
        +    <child link="camera_depth_frame"/>
        +  </joint>
        +  <joint name="camera_base_link1" type="fixed">
        +    <origin rpy="0 0 0" xyz="0 -0.045 0"/>
        +    <parent link="camera_link"/>
        +    <child link="camera_rgb_frame"/>

Do calibration

Collect Calibration Data

1. Make sure your hironx_ros_bridge node is running (hironx_ros_bridge_sim.launch or hironx_ros_bridge_real.launch).

2. Upload URDF to robot_description to the Parameter Server.

$ roslaunch hironx_calibration upload_urdf.launch

This launch will simply overwrite default robot_description parameter (generated from the whole-body COLLADA file without kinect frames) .

3. Since this changes default URDF model, we need to run robot_state_publisher to publish tf with the updated URDF. We also need to run kinect "without" tf publisher (TODO why?):

$ rosrun robot_state_publisher robot_state_publisher __name:=hrpsys_state_publisher
$ roslaunch hironx_kinect.launch publish_tf:=false

4. Finally, run calibration node. This will move robot arm and be careful, and this motion may cause self collision. Please watch the robot carefully and be ready to push emergency stop button.

$ roslaunch hironx_calibration capture_data.launch

Run calibration step

Run calibration program, this program uses a calibration data file generated during the calibration process and stored under tmp (ie. /tmp/hironx_calibration/cal_measurements.bag), meaning that if you reboot the computer, then you have to restart from the previous step.

$ roslaunch hironx_calibration calibrate_hironx.launch

Finally we get following results (in this example, errors in head_camera is no small enough...)

        Computing Jacobian.. (cycle #7)         RMS error: 0.962
        Errors Breakdown:
          head_camera: 9.528937
          arm_chain: 0.007444
        [INFO] [WallTime: 1411546682.199861] Writing original urdf to
        robot_uncalibrated_2014_09_24_17_15.xml
        Joint removed: arm_chain_cb
         xyz: [-0.08738123167896214, 0.0, -0.001496522196753334]
         rpy: (-1.6950600508827331, 0.29224067030639833, -0.057327689173656195)
        Updating xyz for head_to_kinect_joint
         old: [0.0, 0.0, 0.1]
         new: [0.062367406594775215, 0.07536259323525744, 0.12330820494245931]
        Updating rpy for head_to_kinect_joint
         old: [0.0, 0.0, 0.0]
         new: (-0.03346088258363324, 0.1383150116022393, -0.014468436806528857)
        The following joints weren't updated:

And change arguments of static_tf_publisher at https://github.com/start-jsk/rtmros_hironx/pull/252/files#diff-921a60a71a65fcd10fcb361bcd0971b2R8

Scenes from calibration with the robot

Checker board is held like this through the entire calibration process. NOTE that in this picture the head is replaced with xtion with a custom cover.

The image below is before the calibration (with no offset between head and camera):

Post calibration (not accurate enough maybe due to using xtion (carmine), instead of Kinect...)

Restart everything

It is safe to restart everything after calibration since we have changed robot_description and killed default robot_state_publisher).

(Advanced) Create calibration package

The calibration package hironx_calibration we've just gone through is created by following sets of commands and operations. For Hironx robot you can just utilize hironx_calibration package for your own robot as well, but for NEXTAGE Open you may create a similar package by following this.

Use calibration_setup_helper package to generate a package (example with Hironx:

$ rosrun calibration_setup_helper calibration_setup_helper.py `rospack find hironx_moveit_config`/config/HiroNX.urdf --base-link CHEST_JOINT0_Link --arm-tip-link RARM_JOINT5_Link --head-tip-link HEAD_JOINT1_Link  --arm-controller=rarm_controller/command  --head-controller=head_controller/command --head-camera-frame camera_rgb_optical_frame  --head-camera-joint head_to_kinect_joint

Make samples. At first, make sure turning off servo for all joints except CHEST_JOINT0 (in ipython terminal below):

IN[1]: robot.servoOff()
IN[2]: robot.servoOn('CHEST_JOINT0')

And move arm and press enter to store samples, usually we take 15-30 samples but not sure about appropriate number of samples.

$ rosrun hironx_calibration make_samples.py