#########################################################################
Instructions to run the calibration. See below for instructions in how
to use the result

IMPORTANT: Before using the result you need to update your kinematic
model using the fixed offset stored in

 arm_fiducil_cal/calib/fixed_offset.txt

#########################################################################

1) Git clone the repository into your home directory

$ cd
$ git clone git://github.com/usc-clmc/usc-arm-calibration.git

2) Add the following lines to your .bashrc

source /opt/ros/fuerte/setup.bash
export ROSCONSOLE_CONFIG_FILE=~/usc-arm-calibration/rosconsole
export ROS_PACKAGE_PATH=~/usc-arm-calibration:${ROS_PACKAGE_PATH}

3) source the new .bashrc

$ source ~/.bashrc

4) install required debian packages

$ rosdep init
$ rosdep update
$ rosdep install usc_fiducial_calibration
$ apt-get install octave

5) set some symlinks to point at the correct calibration files

$ rosrun arm_scripts cal-set agt
roscd: No such package/stack 'arm_learn_hand_eye_calibration/data'
roscd: No such package/stack 'arm_sl_user/armUser/config'

(ignore these error messages)

6) make

$ rosmake --pre-clean usc_fiducial_calibration

7) start a roscore is a separate terminal

$ roscore

8) (cross fingers) and run the calibration code

$ roslaunch usc_fiducial_calibration fiducial_calibration.launch

9) check the progress in rviz (optional) (copy rviz files first)

$ copy ~/usc-arm-calibration/config/dot_rviz ~/.rviz
$ rosrun rviz rviz

#########################################################################
Instructions in howto use the result
#########################################################################

There is a ROS independent lib in usc-arm-calibration/arm_gp_lib that
reads the generated parameters and can be used to get head offset 
corrections. Please see

usc-arm-calibration/arm_fiducial_cal/src/head_corrector.cpp

for a simple example.

IMPORTANT: Update your kinematic model using the fixed offset stored 
in arm_fiducil_cal/calib/fixed_offset.txt

#########################################################################
To Verify the kinematic model
#########################################################################

To verify the kinematic model we picked a whole lot of head configurations
and computed the forward kinematics.

To generate these files you need to run

$ roslaunch arm_calibrate_extrinsics verify_head_calibration.launch 

This script will use the (current) calibrated "fixed_offset.txt" and the current "models.txt".

We stored the joint angles in

arm_fiducial_cal/calib/verify_forward_kinematics_input.txt - format: <LOWER_PAN> <LOWER_TILT> <UPPER_PAN> <UPPER_TILT>

and the corresponding cartesian poses in

arm_fiducial_cal/calib/verify_forward_kinematics_head_output.txt - foramt: <x> <y> <z> <qw> <qx> <qy> <qz>

The corresponding left bumblebee poses (head + fixed calibrated offset) are in

arm_fiducial_cal/calib/verify_forward_kinematics_left_bb_output.txt - foramt: <x> <y> <z> <qw> <qx> <qy> <qz>

The corresponding left bumblebee poses after correction are in

arm_fiducial_cal/calib/verify_forward_kinematics_gp_correction_output.txt - foramt: <x> <y> <z> <qw> <qx> <qy> <qz>

The GP corrections used are in

arm_fiducial_cal/calib/verify_forward_kinematics_gp_corrections_only_output.txt - foramt: <x> <y> <z> <r> <p> <y>

Thus, in order to verify that our kinematics model of the head, you should get
the "same" cartesian poses for all three frames.