multi_step_cov_estimator: added 'default_initial_pose' parameter

[dgossow]

This will replace the previously used value [0 0 0 0 0 0] as initial pose guess by an (optional) parameter placed in the system.yaml. On dewey, the calibration woudn't converge otherwise.

Not sure about this. Maybe this should go into the 'checkerboards' section.

Example system.yaml:

base_link: base_link

sensors:

  chains:
    torso_chain:
      root: base_link
      tip: head_tilt_link
      cov:
       joint_angles: [0.01, 0.01, 0.01, 0.01, 0.01]
      gearing: [1.0, 1.0, 1.0, 1.0, 1.0]

  rectified_cams:
    head_camera:
      chain_id: torso_chain
      frame_id: head_camera_rgb_optical_frame
      baseline_shift: 0.0
      baseline_rgbd: 0.075
      f_shift: 0.0
      cx_shift: 0.0
      cy_shift: 0.0
      cov: {u: 0.25, v: 0.25, x: 0.25}
    base_camera:
      chain_id: NONE
      frame_id: base_camera_rgb_optical_frame
      baseline_shift: 0.0
      baseline_rgbd: 0.075
      f_shift: 0.0
      cx_shift: 0.0
      cy_shift: 0.0
      cov: {u: 0.25, v: 0.25, x: 0.25}

  tilting_lasers: {}

checkerboards:
  cb_7x6:
    corners_x: 7
    corners_y: 6
    spacing_x: 0.108
    spacing_y: 0.108

transforms:
  dummy_transform:   [ 0, 0, 0, 0, 0, 0 ]

default_initial_pose: [ 3, 0, 0, 0, 0, 0 ]

[ahendrix]

From a configuration perspective, I think this belongs in the sample configuration, since each sample potentially expects the checkerboard in a different position.

I suspect doing that properly will be a real pain to implement, since you'll probably have to pass the pose guesses through the bag file to the estimator.

[mikeferguson]

So this patch is really a huge hack. The more proper way (and what Austin alludes to above with checkerboards in different positions) is really to set a "best sensor", and have the system set the starting guess pose of each checkerboard sample to the value obtained via reprojection of the checkerboard based on a particular sample data. (for instance, on the PR2, you might suggest the tilt laser is the "best sensor" and it will use that to reproject where all poses are.

This of course does lead to a problem: if your best sensor is the arm, well, floating checkerboards are screwed. In this case, perhaps we can rename this parameter to "default_floating_initial_pose" or similar, and then we have implemented the simple version, and can later come back and make the more complex "best sensor" approach happen.

Which actually brings me to another thing: "best sensor" should probably be an array, so that you could say for the PR2 that left_arm then right_arm, and then when left_arm fails for a checkerboard held in the right_arm, you can fall back to right_arm. Yeah, that sounds like quite a bit of code. For now, let's just get "floating" into the name of this parameter and merge it (perhaps also put a comment with reference to this thread of future "TODO: add alternate methods of defining default poses guesses")

[dgossow]

Made the requested changes.

[dgossow]

fixed the default_floating_initial_pose thing