Expose rendering API through QueryObejct/SceneGraph

1. Finishes off GeometryState API (image rendering API).
2. SceneGraph provides API for
   - Registering a renderer
   - Acquiriging SceneGraph-owned and managed render labels
   - Assigning/removing roles
3. QueryObject provided mechanism for creating render images.
4. Slight re-working of RigidBodyPlantBridge to support rendering.

attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc

@@ -25,7 +25,9 @@ using geometry::GeometryInstance;
 using geometry::IllustrationProperties;
 using geometry::MakePhongIllustrationProperties;
 using geometry::Mesh;
+using geometry::PerceptionProperties;
 using geometry::ProximityProperties;
+using geometry::render::RenderLabel;
 using geometry::SceneGraph;
 using geometry::Shape;
 using geometry::Sphere;
@@ -111,6 +113,17 @@ RigidBodyPlantBridge<T>::rigid_body_plant_state_input_port() const {
   return this->get_input_port(plant_state_port_);
 }
 
+template <typename T>
+int RigidBodyPlantBridge<T>::BodyForLabel(RenderLabel label) const {
+  if (label <= RenderLabel::kMaxUnreserved) {
+    return label_to_index_.at(label);
+  } else if (label == RenderLabel::kDontCare) {
+    return 0;  // world index.
+  } else {
+    return -1;
+  }
+}
+
 template <typename T>
 void RigidBodyPlantBridge<T>::RegisterTree(SceneGraph<T>* scene_graph) {
   // TODO(SeanCurtis-TRI): This treats all bodies in the tree as dynamic. Some
@@ -135,11 +148,20 @@ void RigidBodyPlantBridge<T>::RegisterTree(SceneGraph<T>* scene_graph) {
 
     // Default to the world body configuration.
     FrameId body_id = scene_graph->world_frame_id();
+    RenderLabel label;
     if (body.get_body_index() != tree_->world().get_body_index()) {
       // All other bodies register a frame and (possibly) get a unique label.
       body_id = scene_graph->RegisterFrame(
           source_id_,
           GeometryFrame(body.get_name(), body.get_model_instance_id()));
+
+      if (body.get_visual_elements().size() > 0) {
+        // We'll have the render label map to the body index.
+        // NOTE: This is only valid if the RBT is the only source of geometry.
+        // But given that the RBT is on the way out, why not?
+        label = RenderLabel(static_cast<int>(body.get_body_index()));
+        label_to_index_[label] = body.get_body_index();
+      }
     }
     body_ids_.push_back(body_id);
 
@@ -160,6 +182,12 @@ void RigidBodyPlantBridge<T>::RegisterTree(SceneGraph<T>* scene_graph) {
         const Vector4<double>& diffuse = visual_element.getMaterial();
         scene_graph->AssignRole(source_id_, id,
                                 MakePhongIllustrationProperties(diffuse));
+
+        // Perception properties -- diffuse color and per-body label.
+        PerceptionProperties perception;
+        perception.AddProperty("phong", "diffuse", diffuse);
+        perception.AddProperty("label", "id", label);
+        scene_graph->AssignRole(source_id_, id, perception);
       }
     }
     int collision_count = 0;

attic/multibody/rigid_body_plant/rigid_body_plant_bridge.h

@@ -5,6 +5,7 @@
 #include "drake/common/drake_copyable.h"
 #include "drake/geometry/frame_kinematics_vector.h"
 #include "drake/geometry/geometry_ids.h"
+#include "drake/geometry/render/render_label.h"
 #include "drake/geometry/scene_graph.h"
 #include "drake/multibody/rigid_body_tree.h"
 #include "drake/systems/framework/context.h"
@@ -120,6 +121,18 @@ class RigidBodyPlantBridge : public systems::LeafSystem<T> {
   const systems::InputPort<T>& rigid_body_plant_state_input_port()
       const;
 
+  /** Given a render label, reports the index of the RigidBody to which this
+   label was assigned. The RenderLabel::kDontCare label maps to the index of the
+   world body. The other reserved labels return a negative value.
+
+   @throws std::logic_error If the label doesn't otherwise specify a body.  */
+  int BodyForLabel(geometry::render::RenderLabel label) const;
+
+  /** Reports the frame id for the given body. */
+  geometry::FrameId FrameIdFromBody(const RigidBody<T>& body) const {
+    return body_ids_[body.get_body_index()];
+  }
+
  private:
   // Registers `this` system's tree's bodies and geometries to the given
   // geometry system.
@@ -139,6 +152,10 @@ class RigidBodyPlantBridge : public systems::LeafSystem<T> {
   int geometry_pose_port_{-1};
   int plant_state_port_{-1};
 
+  // Maps from a generated render label to the index of the rigid body assigned
+  // that label.
+  std::unordered_map<geometry::render::RenderLabel, int> label_to_index_;
+
   // Registered frames. In this incarnation, body i's frame_id is stored in
   // element i. This is because *all* frames are currently being registered
   // (regardless of weldedness or whether it has geometry).

geometry/BUILD.bazel

@@ -177,6 +177,7 @@ drake_cc_library(
         "//geometry/query_results:penetration_as_point_pair",
         "//geometry/query_results:signed_distance_pair",
         "//geometry/query_results:signed_distance_to_point",
+        "//geometry/render:render_label_manager",
         "//systems/framework",
         "//systems/rendering:pose_bundle",
     ],
@@ -330,6 +331,7 @@ drake_cc_googletest(
     deps = [
         ":geometry_state",
         "//common/test_utilities",
+        "//geometry/render:render_label_manager",
         "//geometry/test_utilities:dummy_render_engine",
     ],
 )
@@ -348,6 +350,7 @@ drake_cc_googletest(
         ":geometry_visualization",
         ":scene_graph",
         "//common/test_utilities:expect_throws_message",
+        "//geometry/test_utilities:dummy_render_engine",
     ],
 )
 

geometry/geometry_state.cc

@@ -32,6 +32,9 @@ using std::make_unique;
 using std::move;
 using std::swap;
 using std::to_string;
+using systems::sensors::ImageDepth32F;
+using systems::sensors::ImageLabel16I;
+using systems::sensors::ImageRgba8U;
 
 //-----------------------------------------------------------------------------
 
@@ -921,6 +924,58 @@ void GeometryState<T>::AddRenderer(
   render_engines_[move(name)] = move(renderer);
 }
 
+template <typename T>
+std::vector<std::string> GeometryState<T>::RegisteredRendererNames() const {
+  std::vector<std::string> names;
+  names.reserve(render_engines_.size());
+  for (const auto& name_engine_pair : render_engines_) {
+    names.push_back(name_engine_pair.first);
+  }
+  return names;
+}
+
+template <typename T>
+void GeometryState<T>::RenderColorImage(const render::CameraProperties& camera,
+                                        FrameId parent_frame,
+                                        const RigidTransformd& X_PC,
+                                        ImageRgba8U* color_image_out,
+                                        bool show_window) const {
+  const RigidTransformd X_WC = GetDoubleWorldPose(parent_frame) * X_PC;
+  const render::RenderEngine& engine =
+      GetRenderEngineOrThrow(camera.renderer_name);
+  // TODO(SeanCurtis-TRI): Invoke UpdateViewpoint() as part of a calc cache
+  //  entry. Challenge: how to do that with a parameter passed here?
+  const_cast<render::RenderEngine&>(engine).UpdateViewpoint(X_WC);
+  engine.RenderColorImage(camera, show_window, color_image_out);
+}
+
+template <typename T>
+void GeometryState<T>::RenderDepthImage(
+    const render::DepthCameraProperties& camera,
+    FrameId parent_frame, const RigidTransformd& X_PC,
+    ImageDepth32F* depth_image_out) const {
+  const RigidTransformd X_WC = GetDoubleWorldPose(parent_frame) * X_PC;
+  const render::RenderEngine& engine =
+      GetRenderEngineOrThrow(camera.renderer_name);
+  // See note in RenderColorImage() about this const cast.
+  const_cast<render::RenderEngine&>(engine).UpdateViewpoint(X_WC);
+  engine.RenderDepthImage(camera, depth_image_out);
+}
+
+template <typename T>
+void GeometryState<T>::RenderLabelImage(const render::CameraProperties& camera,
+                                        FrameId parent_frame,
+                                        const RigidTransformd& X_PC,
+                                        ImageLabel16I* label_image_out,
+                                        bool show_window) const {
+  const RigidTransformd X_WC = GetDoubleWorldPose(parent_frame) * X_PC;
+  const render::RenderEngine& engine =
+      GetRenderEngineOrThrow(camera.renderer_name);
+  // See note in RenderColorImage() about this const cast.
+  const_cast<render::RenderEngine&>(engine).UpdateViewpoint(X_WC);
+  engine.RenderLabelImage(camera, show_window, label_image_out);
+}
+
 template <typename T>
 std::unique_ptr<GeometryState<AutoDiffXd>> GeometryState<T>::ToAutoDiffXd()
     const {
@@ -1364,6 +1419,15 @@ const render::RenderEngine& GeometryState<T>::GetRenderEngineOrThrow(
       fmt::format("No renderer exists with name: '{}'", renderer_name));
 }
 
+template <typename T>
+RigidTransformd GeometryState<T>::GetDoubleWorldPose(FrameId frame_id) const {
+  if (frame_id == InternalFrame::world_frame_id()) {
+    return RigidTransformd::Identity();
+  }
+  const internal::InternalFrame& frame = GetValueOrThrow(frame_id, frames_);
+  return RigidTransformd(internal::convert_to_double(X_WF_[frame.index()]));
+}
+
 }  // namespace geometry
 }  // namespace drake
 

geometry/geometry_state.h

@@ -445,10 +445,50 @@ class GeometryState {
 
   //@}
 
+  //---------------------------------------------------------------------------
+  /** @name                Render Queries  */
+  //@{
+
   /** Implementation support for SceneGraph::AddRenderer().  */
   void AddRenderer(std::string name,
                    std::unique_ptr<render::RenderEngine> renderer);
 
+  /** Implementation support for SceneGraph::HasRenderer().  */
+  bool HasRenderer(const std::string& name) const {
+    return render_engines_.count(name) > 0;
+  }
+
+  /** Implementation support for SceneGraph::RendererCount().  */
+  int RendererCount() const { return static_cast<int>(render_engines_.size()); }
+
+  /** Implementation support for SceneGraph::RegisteredRendererNames().  */
+  std::vector<std::string> RegisteredRendererNames() const;
+
+  /** Implementation support for QueryObject::RenderColorImage().
+   @pre All poses have already been updated.  */
+  void RenderColorImage(const render::CameraProperties& camera,
+                        FrameId parent_frame,
+                        const math::RigidTransformd& X_PC,
+                        systems::sensors::ImageRgba8U* color_image_out,
+                        bool show_window) const;
+
+  /** Implementation support for QueryObject::RenderDepthImage().
+   @pre All poses have already been updated.  */
+  void RenderDepthImage(const render::DepthCameraProperties& camera,
+                        FrameId parent_frame,
+                        const math::RigidTransformd& X_PC,
+                        systems::sensors::ImageDepth32F* depth_image_out) const;
+
+  /** Implementation support for QueryObject::RenderLabelImage().
+   @pre All poses have already been updated.  */
+  void RenderLabelImage(const render::CameraProperties& camera,
+                        FrameId parent_frame,
+                        const math::RigidTransformd& X_PC,
+                        systems::sensors::ImageLabel16I* label_image_out,
+                        bool show_window) const;
+
+  //@}
+
   /** @name Scalar conversion */
   //@{
 
@@ -658,6 +698,10 @@ class GeometryState {
   const render::RenderEngine& GetRenderEngineOrThrow(
       const std::string& renderer_name) const;
 
+  // Utility function to facilitate getting a double-valued pose for a frame,
+  // regardless of T's actual type.
+  math::RigidTransformd GetDoubleWorldPose(FrameId frame_id) const;
+
   // NOTE: If adding a member it is important that it be _explicitly_ copied
   // in the converting copy constructor and likewise tested in the unit test
   // for that constructor.

geometry/query_object.cc

@@ -8,6 +8,12 @@ namespace drake {
 namespace geometry {
 
 using math::RigidTransform;
+using math::RigidTransformd;
+using render::CameraProperties;
+using render::DepthCameraProperties;
+using systems::sensors::ImageDepth32F;
+using systems::sensors::ImageLabel16I;
+using systems::sensors::ImageRgba8U;
 
 template <typename T>
 QueryObject<T>::QueryObject(const QueryObject& query_object) {
@@ -66,25 +72,23 @@ RigidTransform<T> QueryObject<T>::X_WG(GeometryId id) const {
 }
 
 template <typename T>
-std::vector<ContactSurface<T>>
-QueryObject<T>::ComputeContactSurfaces() const {
+std::vector<PenetrationAsPointPair<double>>
+QueryObject<T>::ComputePointPairPenetration() const {
   ThrowIfNotCallable();
 
-  // TODO(DamrongGuoy): Modify this when the cache system is in place.
   FullPoseUpdate();
   const GeometryState<T>& state = geometry_state();
-  return state.ComputeContactSurfaces();
+  return state.ComputePointPairPenetration();
 }
 
 template <typename T>
-std::vector<PenetrationAsPointPair<double>>
-QueryObject<T>::ComputePointPairPenetration() const {
+std::vector<ContactSurface<T>>
+QueryObject<T>::ComputeContactSurfaces() const {
   ThrowIfNotCallable();
 
-  // TODO(SeanCurtis-TRI): Modify this when the cache system is in place.
   FullPoseUpdate();
   const GeometryState<T>& state = geometry_state();
-  return state.ComputePointPairPenetration();
+  return state.ComputeContactSurfaces();
 }
 
 template <typename T>
@@ -93,7 +97,6 @@ QueryObject<T>::ComputeSignedDistancePairwiseClosestPoints(
     const double max_distance) const {
   ThrowIfNotCallable();
 
-  // TODO(SeanCurtis-TRI): Modify this when the cache system is in place.
   FullPoseUpdate();
   const GeometryState<T>& state = geometry_state();
   return state.ComputeSignedDistancePairwiseClosestPoints(max_distance);
@@ -111,6 +114,46 @@ QueryObject<T>::ComputeSignedDistanceToPoint(
   return state.ComputeSignedDistanceToPoint(p_WQ, threshold);
 }
 
+template <typename T>
+void QueryObject<T>::RenderColorImage(const CameraProperties& camera,
+                                      FrameId parent_frame,
+                                      const RigidTransformd& X_PC,
+                                      ImageRgba8U* color_image_out,
+                                      bool show_window) const {
+  ThrowIfNotCallable();
+
+  FullPoseUpdate();
+  const GeometryState<T>& state = geometry_state();
+  return state.RenderColorImage(camera, parent_frame, X_PC, color_image_out,
+                                show_window);
+}
+
+template <typename T>
+void QueryObject<T>::RenderDepthImage(const DepthCameraProperties& camera,
+                                      FrameId parent_frame,
+                                      const RigidTransformd& X_PC,
+                                      ImageDepth32F* depth_image_out) const {
+  ThrowIfNotCallable();
+
+  FullPoseUpdate();
+  const GeometryState<T>& state = geometry_state();
+  return state.RenderDepthImage(camera, parent_frame, X_PC, depth_image_out);
+}
+
+template <typename T>
+void QueryObject<T>::RenderLabelImage(const CameraProperties& camera,
+                                      FrameId parent_frame,
+                                      const RigidTransformd& X_PC,
+                                      ImageLabel16I* label_image_out,
+                                      bool show_window) const {
+  ThrowIfNotCallable();
+
+  FullPoseUpdate();
+  const GeometryState<T>& state = geometry_state();
+  return state.RenderLabelImage(camera, parent_frame, X_PC, label_image_out,
+                                show_window);
+}
+
 template <typename T>
 const GeometryState<T>& QueryObject<T>::geometry_state() const {
   // Some extra insurance in case some query *hadn't* called this.