Enable multi-step optimization

[Naoki-Hiraoka]

Hello. I am Naoki Hiraoka, robot_calibration user.

This pull request is to enable multi-step optimization like this and this.

Multi-step optimization can be used with calibrate.yaml like below

cal_steps:
  00_Estimating_Checkerboard_Locations:
    base_link: base_link: torso_lift_link
    models:
       - name: arm
         type: chain
         frame: wrist_roll_link
       - name: camera
         type: camera3d
         frame: head_camera_rgb_optical_frame
         topic: /head_camera/depth_registered/points
    free_frames:
      - name: checkerboard
        x: true
        y: false
        z: true
        roll: false
        pitch: true
        yaw: false
    error_blocks:
      - name: hand_eye
        type: chain3d_to_chain3d
        model_a: camera
        model_b: arm
  01_Adding_Camera_Locations:
    base_link: base_link: torso_lift_link
    models:
       - name: arm
         type: chain
         frame: wrist_roll_link
       - name: camera
         type: camera3d
         frame: head_camera_rgb_optical_frame
         topic: /head_camera/depth_registered/points
    free_frames:
      - name: checkerboard
        x: true
        y: false
        z: true
        roll: false
        pitch: true
        yaw: false
     - name: head_camera_rgb_joint
       x: true
       y: true
       z: true
       roll: true
       pitch: true
       yaw: true
     - name: head_pan_joint
       x: true
       y: true
       z: true
       roll: true
       pitch: true
       yaw: true
    error_blocks:
      - name: hand_eye
        type: chain3d_to_chain3d
        model_a: camera
        model_b: arm
  02_Joint_Offset:
    base_link: base_link: torso_lift_link
    models:
       - name: arm
         type: chain
         frame: wrist_roll_link
       - name: camera
         type: camera3d
         frame: head_camera_rgb_optical_frame
         topic: /head_camera/depth_registered/points
     free_params:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - upperarm_roll_joint
      - elbow_flex_joint
      - forearm_roll_joint
      - wrist_flex_joint
      - wrist_roll_joint
      - head_tilt_joint
      - camera_fx
      - camera_fy
      - camera_cx
      - camera_cy
      - camera_z_offset
      - camera_z_scaling
    free_frames:
      - name: checkerboard
        x: true
        y: false
        z: true
        roll: false
        pitch: true
        yaw: false
     - name: head_camera_rgb_joint
       x: true
       y: true
       z: true
       roll: true
       pitch: true
       yaw: true
     - name: head_pan_joint
       x: true
       y: true
       z: true
       roll: true
       pitch: true
       yaw: true
    error_blocks:
      - name: hand_eye
        type: chain3d_to_chain3d
        model_a: camera
        model_b: arm

[mikeferguson]

Quickly scanned this -- will take longer to review. Is this designed to be backwards compatible? Or do all users have to update to have cal_steps?

[Naoki-Hiraoka]

Thank you for your response.

This pull request is designed to be backwards compatible. If cal_steps is not set, the program works as before.

[mikeferguson]

@Naoki-Hiraoka I'm closing this in favor of #73 which does the same thing, but with slighty less changes to the internals and a little less bookkeeping.