Allow PlaneFinder to find planes in complex frames

[mikeferguson]

Right now, the plane finder assumes that all visible points, that are within the bounding box, are part of the plane. This works fine for things like "robot views the ground" -- but if anything else enters the frame, it's problematic. It also doesn't let us handle frames with other items/clutter in them, or do more interesting things like find the face plane of the gripper (which has long been something I've wanted to be able to do).

Proposed improvements to PlaneFinder:

• Actually have it "find planes" by segmenting the image into N planes, each of which are larger than some minimum_points_per_plane (a new parameter).
• Provide a new set of parameters that allow you to specify reference point & normal vector, in an arbitrary frame, that you would like to find the "closest" plane to. If these parameters are not set, the largest plane will be selected (which is basically backwards compatible with today). For something like "find the ground", you could specify a plane in the base_link. For gripper calibration, you could specify the plane to be nearest the center of the gripper, and provide the normal so we can't pick up the "side" of the gripper.

[mikeferguson]

A first pass of this is in #109 - where we at least find the plane and only sample from points in that plane - this really only works when > 75-80% of the frame is the plane of interest

[mikeferguson]

#124 implements the normal vector alignment - although not in an arbitrary frame.

[mikeferguson]

#191 drastically speeds up the plane finder

[mikeferguson]

While the normal does need to be specified in the same transform frame that the limits are, this seems to actually be a decent setup.

I was able to extract the gripper face on the UBR-1 using the following config:

gripper_plane_finder:
      type: robot_calibration::PlaneFinder
      topic: /head_camera/depth_registered/points
      camera_info_topic: /head_camera/depth_registered/camera_info
      camera_sensor_name: gripper_plane_camera
      transform_frame: gripper_link
      min_x: 0.02
      max_x: 0.1
      min_y: -0.15
      max_y: 0.15
      min_z: 0.0
      max_z: 0.15
      tolerance: 0.005
      points_max: 100
      debug: false
    ground_plane_finder: