[render] Expose rendering API through QueryObejct/SceneGraph (RobotLo…

…comotion#11593)

* Expose rendering API through QueryObejct/SceneGraph

1. Finishes off GeometryState API (image rendering API).
2. SceneGraph provides API for
   - Registering a renderer
   - 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

@@ -348,6 +348,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,
+                                        bool show_window,
+                                        ImageRgba8U* color_image_out) 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,
+                                        bool show_window,
+                                        ImageLabel16I* label_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.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

@@ -48,13 +48,18 @@ using FrameIdSet = std::unordered_set<FrameId>;
 
 //@}
 
+// TODO(SeanCurtis-TRI): Move GeometryState into `internal` namespace (and then
+//  I can kill the `@note` in the class documentation).
+
 /**
  The context-dependent state of SceneGraph. This serves as an AbstractValue
  in the context. SceneGraph's time-dependent state includes more than just
  values; objects can be added to or removed from the world over time. Therefore,
  SceneGraph's context-dependent state includes values (the poses) and
  structure (the topology of the world).
 
+ @note This is intended as an internal class only.
+
  @tparam T The scalar type. Must be a valid Eigen scalar.
 
  Instantiated templates for the following kinds of T's are provided:
@@ -357,15 +362,34 @@ class GeometryState {
    "SceneGraph::RemoveRole()".  */
   int RemoveRole(SourceId source_id, GeometryId geometry_id, Role role);
 
-  /** Implementation of
-   @ref SceneGraph::RemoveFromRenderer(const std::string&, SourceId, FrameId)
-   "SceneGraph::RemoveFromRenderer()".  */
+  // TODO(SeanCurtis-TRI): These two "RemoveFromRenderer()" methods are not
+  // currently exposed in the SceneGraph API. They've been left here so that
+  // they (and their unit tests) don't have to be recreated. I need to
+  // determine definitively if these methods *should* exist (in which case I
+  // put them in the SceneGraph API or not, in which case I can remove them
+  // entirely).
+
+  /** For every geometry directly registered to the frame with the given
+   `frame_id`, if it has been added to the renderer with the given
+   `renderer_name` it is removed from that renderer.
+   @return The number of geometries affected by the removal.
+   @throws std::logic_error if 1) `source_id` does not map to a registered
+                            source, 2) `frame_id` does not map to a registered
+                            frame, 3) `frame_id` does not belong to
+                            `source_id` (unless `frame_id` is the world frame
+                            id), or 4) the context has already been
+                            allocated.  */
   int RemoveFromRenderer(const std::string& renderer_name, SourceId source_id,
                          FrameId frame_id);
 
-  /** Implementation of
-   @ref SceneGraph::RemoveFromRenderer(const std::string&, SourceId, GeometryId)
-   "SceneGraph::RemoveFromRenderer()".  */
+  /** Removes the geometry with the given `geometry_id` from the renderer with
+   the given `renderer_name`, _if_ it has previously been added.
+   @return The number of geometries affected by the removal (0 or 1).
+   @throws std::logic_error if 1) `source_id` does not map to a registered
+                            source, 2) `geometry_id` does not map to a
+                            registered geometry, 3) `geometry_id` does not
+                            belong to `source_id`, or 4) the context has already
+                            been allocated.  */
   int RemoveFromRenderer(const std::string& renderer_name, SourceId source_id,
                          GeometryId geometry_id);
 
@@ -445,10 +469,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,
+                        bool show_window,
+                        systems::sensors::ImageRgba8U* color_image_out) 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,
+                        bool show_window,
+                        systems::sensors::ImageLabel16I* label_image_out) const;
+
+  //@}
+
   /** @name Scalar conversion */
   //@{
 
@@ -658,6 +722,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.