manipulation station should load camera calibration from yaml

[RussTedrake]

@kmuhlrad has a version (python).
we also have a version in anzu.

just need to merge the two workflows and push to master.

[kmuhlrad]

Example config files: https://github.com/RobotLocomotion/6-881-examples/tree/master/perception/config

Parsing function: https://github.com/RobotLocomotion/6-881-examples/blob/master/perception/point_cloud_to_pose_system.py#L67

[kmuhlrad]

Any updates of what we want? Otherwise I'll continue to use my version for other perception-related tasks and make a C++ parser.

[RussTedrake]

@siyuanfeng-tri and @EricCousineau-TRI have a workflow that siyuan has partially implemented in the manipulation station already. might you guys comment?

[EricCousineau-TRI]

We try to keep the manifest of cameras (and their relevant) frames specified purely in the models (e.g. the SDFs, or our model directive YAML files, like what @gizatt had started on). Caveat is that we haven't stored too much per-camera information (intrinsics, other stuff), but that can be stored in a per-camera manifest, such that the physical relation of a camera (the frame it's attached to and the offset) is encoded in the physical model.

For example, we do our calibration, and then have that calibration generate an SDF file that has the cameras:

<model name="camera_collisions">  <!-- But it's kind of kinematics too... -->
  <link name="camera_123456"> ... </link>
</model>

Additionally (since at present SDF does not have a well-documented way of doing model composition), we add our wrist cameras using something like this:

directives:
- add_frame:
    name: camera_12345678
    X_PF:
      base_frame: iiwa_link_7
      translation: [-0.03167912, 0.04554264, 0.1120705]
      rotation_rpy_deg: [-12.24621, -0.5094894, -112.488]

Then, when we need to query the cameras from MBP, we just scan the available frames, and add the cameras:

std::vector<SimCamera> FindSimCameras(
    const drake::multibody::MultibodyPlant<double>& plant) {
  std::vector<SimCamera> sim_cameras;
  std::regex camera_name_regex("camera_([0-9]{12})");
  for (int i = 0; i < plant.num_frames(); i++) {
    const drake::multibody::Frame<double>& frame =
        plant.get_frame(drake::multibody::FrameIndex(i));
    std::smatch match;
    if (std::regex_match(frame.name(), match, camera_name_regex)) {
      SimCamera sim_camera(match.str(1), frame);
      sim_cameras.push_back(sim_camera);
      drake::log()->info("Found camera frame: {}", frame.name());
    }
  }
  return sim_cameras;
}

drake::systems::sensors::dev::RgbdCamera* AddSimCamera(
    const drake::geometry::dev::SceneGraph<double>& render_scene_graph,
    const drake::multibody::MultibodyPlant<double>& plant,
    const SimCamera& camera, drake::systems::DiagramBuilder<double>* builder) {
  DRAKE_DEMAND(builder);
  // TODO(eric.cousineau): Simplify this if drake#10247 is resolved.
  const drake::optional<drake::geometry::FrameId> body_id =
      plant.GetBodyFrameIdIfExists(camera.frame().body().index());
  DRAKE_THROW_UNLESS(body_id.has_value());
  const drake::Isometry3<double> X_PC =
      camera.frame().GetFixedPoseInBodyFrame();
  auto* camera_sys =
      builder->AddSystem<drake::systems::sensors::dev::RgbdCamera>(
          "camera_" + camera.serial(), *body_id,
          X_PC * SimCamera::X_BC().GetAsIsometry3().inverse(),
          camera.properties(), false);
  builder->Connect(render_scene_graph.get_query_output_port(),
                   camera_sys->query_object_input_port());
  return camera_sys;
}

I'm not sure if this is at a point of maturity where having it in Drake will be useful. At a minimum, it could either be put in dev/ or you could copy+paste the code, but there are still some things that need to be fleshed out that you may need sooner (e.g. different intrinsics or RGB-to-Depth extrinsics for the cameras).

Will follow-up on Slack with a couple more pieces of advice.

[RussTedrake]

closing for now; may revisit this summer.