Skip to content

Commit

Permalink
Add the abstract RenderEngine
Browse files Browse the repository at this point in the history
The RenderEngine is the common base-class for all perception-role
renderers. It provides some core functionality for interacting with the
GeometryState (supported by NVI methods).

The interface of the RenderEngine should largely mirror that of the older
RgbdRenderer interface.

This commit excludes the *label* images so that it can be reviewed in
parallel with the RenderLabel PR. Once both have landed, the label image
will be included in the RenderEngine interface.

Remove render labels from render engine
  • Loading branch information
SeanCurtis-TRI committed Apr 23, 2019
1 parent 430bb9d commit 411f242
Show file tree
Hide file tree
Showing 6 changed files with 656 additions and 1 deletion.
5 changes: 4 additions & 1 deletion geometry/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -257,7 +257,10 @@ drake_cc_library(
name = "utilities",
srcs = ["utilities.cc"],
hdrs = ["utilities.h"],
deps = ["//common"],
deps = [
"//common",
"//math:geometric_transform",
],
)

# -----------------------------------------------------
Expand Down
29 changes: 29 additions & 0 deletions geometry/render/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -13,12 +13,32 @@ package(default_visibility = ["//visibility:public"])
drake_cc_package_library(
name = "render",
deps = [
":render_engine",
":render_label",
":render_label_class",
":render_label_manager",
],
)

# A build target for code that wants to interact with the *idea* of a render
# engine but doesn't want any build dependencies on the implementation details.
drake_cc_library(
name = "render_engine",
srcs = ["render_engine.cc"],
hdrs = [
"camera_properties.h",
"render_engine.h",
],
deps = [
"//geometry:geometry_index",
"//geometry:geometry_roles",
"//geometry:shape_specification",
"//geometry:utilities",
"//math:geometric_transform",
"//systems/sensors:image",
],
)

drake_cc_library(
name = "render_label",
srcs = ["render_label.cc"],
Expand Down Expand Up @@ -54,6 +74,15 @@ drake_cc_library(

# === test/ ===

drake_cc_googletest(
name = "render_engine_test",
deps = [
":render_engine",
"//common/test_utilities",
"//geometry:geometry_ids",
],
)

drake_cc_googletest(
name = "render_label_test",
deps = [
Expand Down
89 changes: 89 additions & 0 deletions geometry/render/render_engine.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
#include "drake/geometry/render/render_engine.h"

namespace drake {
namespace geometry {
namespace render {

using math::RigidTransformd;

std::unique_ptr<RenderEngine> RenderEngine::Clone() const {
return std::unique_ptr<RenderEngine>(DoClone());
}

optional<RenderIndex> RenderEngine::RegisterVisual(
GeometryIndex index, const drake::geometry::Shape& shape,
const PerceptionProperties& properties,
const RigidTransformd& X_WG, bool needs_updates) {
optional<RenderIndex> render_index =
DoRegisterVisual(shape, properties, X_WG);
if (render_index) {
if (needs_updates) {
update_indices_.insert({*render_index, index});
} else {
anchored_indices_.insert({*render_index, index});
}
}
return render_index;
}

optional<GeometryIndex> RenderEngine::RemoveGeometry(RenderIndex index) {
// The underlying engine doesn't know if the geometry to remove or the
// geometry that gets moved requires updates or not. As such, the removed
// index and the moved index can arbitrarily come from either mapping.
// Possible scenarios:
// Remove index in map A
// Case 1: nothing moved
// 1. Remove the (remove index, remove GeometryIndex) pair from A.
// 2. return nullopt.
// Case 2: moved geometry in A (i.e., _same_ map)
// 1. Remove the (moved index, moved GeometryIndex) pair from A.
// 2. Add the (removed index, moved GeometryIndex) pair into A.
// 3. Return moved GeometryIndex.
// Case 3: moved geometry in B (i.e., _different_ map)
// 1. Remove the (removed index, removed GI) pair from A.
// 2. Remove the (moved index, moved GI) pair from B.
// 3. Add the (removed index, moved GI) pair to B.
// 4. Return moved GeometryIndex.

// Given an index, return a pointer to the anchored/update index map in which
// this index belongs.
auto get_index_map = [this](const RenderIndex r_index) {
auto iter = update_indices_.find(r_index);
if (iter != update_indices_.end()) return &update_indices_;
iter = anchored_indices_.find(r_index);
if (iter != anchored_indices_.end()) return &anchored_indices_;
throw std::logic_error(fmt::format(
"Error finding RenderIndex ({}) as either dynamic or anchored",
r_index));
};

// Determine map A.
std::unordered_map<RenderIndex, GeometryIndex>* map_A = get_index_map(index);

GeometryIndex moved_internal_index;
optional<RenderIndex> moved_index = DoRemoveGeometry(index);

// Determine map B.
std::unordered_map<RenderIndex, GeometryIndex>* map_B = nullptr;
if (moved_index) {
map_B = get_index_map(*moved_index);
moved_internal_index = (*map_B)[*moved_index];
}

// Examine cases:
map_A->erase(index);
if (map_B == nullptr) { // Case 1.
return nullopt;
} else if (map_A == map_B) { // Case 2.
map_A->erase(*moved_index);
(*map_A)[index] = moved_internal_index;
} else { // Case 3.
map_B->erase(*moved_index);
(*map_B)[index] = moved_internal_index;
}
return moved_internal_index;
}

} // namespace render
} // namespace geometry
} // namespace drake
184 changes: 184 additions & 0 deletions geometry/render/render_engine.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,184 @@

#pragma once

#include <memory>
#include <unordered_map>
#include <vector>

#include <Eigen/Dense>

#include "drake/common/drake_copyable.h"
#include "drake/common/drake_optional.h"
#include "drake/geometry/geometry_index.h"
#include "drake/geometry/geometry_roles.h"
#include "drake/geometry/render/camera_properties.h"
#include "drake/geometry/shape_specification.h"
#include "drake/geometry/utilities.h"
#include "drake/math/rigid_transform.h"
#include "drake/systems/sensors/image.h"

namespace drake {
namespace geometry {
namespace render {

/** The engine for performing rasterization operations on geometry. This
includes rgb images and depth images. The coordinate system of
%RenderEngine's viewpoint `R` is `X-right`, `Y-down` and `Z-forward`
with respect to the rendered images.
Output image format:
- RGB (ImageRgba8U) : the RGB image has four channels in the following
order: red, green, blue, and alpha. Each channel is represented by
a uint8_t.
- Depth (ImageDepth32F) : the depth image has a depth channel represented
by a float. For a point in space `P`, the value stored in the depth
channel holds *the Z-component of the position vector `p_RP`.*
Note that this is different from the range data used by laser
range finders (like that provided by DepthSensor) in which the depth
value represents the distance from the sensor origin to the object's
surface. */
class RenderEngine : public ShapeReifier {
public:
RenderEngine() = default;

virtual ~RenderEngine() = default;

/** Clones the render engine -- making the RenderEngine compatible with
copyable_unique_ptr. */
std::unique_ptr<RenderEngine> Clone() const;

/** Registers a shape specification and returns the optional index of the
corresponding render geometry. The geometry can be uniquely referenced in
this engine (and copies of this engine) by its geometry index. The renderer
is allowed to examine the given `properties` and choose to _not_ register
the geometry.
@param index The geometry index of the shape to register.
@param shape The shape specification to add to the render engine.
@param properties The perception properties provided for this geometry.
@param X_WG The pose of the geometry relative to the world frame W.
@param needs_updates If true, the geometry's pose will be updated via
UpdatePoses().
@returns A unique index for the resultant render geometry (nullopt if not
registered).
@throws std::runtime_error if the shape is an unsupported type. */
optional<RenderIndex> RegisterVisual(
GeometryIndex index,
const Shape& shape, const PerceptionProperties& properties,
const math::RigidTransformd& X_WG, bool needs_updates = true);

/** Removes the geometry indicated by the given `index` from the engine.
It may move another geometry into that index value to maintain a contiguous
block of indices. If it does so, it returns the internal geometry index of
the geometry that got moved into the `index` position.
@param index The _render_ index of the geometry to remove.
@returns The _geometry_ index of the geometry that _now_ maps to the _render_
`index` if the `index` was recycled.
@throws std::logic_error if the index is invalid. */
optional<GeometryIndex> RemoveGeometry(RenderIndex index);

/** Updates the poses of all geometries marked as "needing update" (see
RegisterVisual()).
@param X_WGs The poses of *all* geometries in SceneGraph (measured and
expressed in the world frame). The pose for a geometry is
accessed by that geometry's GeometryIndex. */
template <typename T>
void UpdatePoses(const std::vector<math::RigidTransform<T>>& X_WGs) {
for (auto pair : update_indices_) {
RenderIndex render_index = pair.first;
GeometryIndex geometry_index = pair.second;
DoUpdateVisualPose(render_index,
geometry::internal::convert(X_WGs[geometry_index]));
}
}

/** Updates the renderer's viewpoint with given pose X_WR.
@param X_WR The pose of renderer's viewpoint in the world coordinate
system. */
virtual void UpdateViewpoint(const math::RigidTransformd& X_WR) const = 0;

/** Renders the registered geometry into the given color (rgb) image.
@param camera The intrinsic properties of the camera.
@param show_window If true, the render window will be displayed.
@param[out] color_image_out The rendered color image. */
virtual void RenderColorImage(
const CameraProperties& camera, bool show_window,
systems::sensors::ImageRgba8U* color_image_out) const = 0;

/** Renders the registered geometry into the given depth image. In contrast to
the other rendering operations, depth images don't have an option to display
the window; generally, basic depth images are not readily communicative to
humans.
@param camera The intrinsic properties of the camera.
@param[out] depth_image_out The rendered depth image. */
virtual void RenderDepthImage(
const DepthCameraProperties& camera,
systems::sensors::ImageDepth32F* depth_image_out) const = 0;

protected:
// Allow derived classes to implement Cloning via copy-construction.
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(RenderEngine)

/** The NVI-function for sub-classes to implement actual geometry
registration. If the derived class chooses not to register this particular
shape, it should return nullopt.
A derived render engine can choose not to register geometry because, e.g., it
doesn't have default properties. This is the primary mechanism which enables
different renderers to use different geometries for the same frame.
For example, a low-fidelity renderer may use simple geometry whereas a
high-fidelity renderer would require a very detailed geometry. Both
geometries would have PerceptionProperties, but, based on the provided
property groups and values, one would be accepted and registered with one
render engine implementation and the other geometry with another render
engine. */
virtual optional<RenderIndex> DoRegisterVisual(
const Shape& shape, const PerceptionProperties& properties,
const math::RigidTransformd& X_WG) = 0;

/** The NVI-function for updating the pose of a render geometry (identified
by index) to the given pose X_WG.
@param index The index of the render geometry whose pose is being set.
@param X_WG The pose of the render geometry in the world frame. */
virtual void DoUpdateVisualPose(RenderIndex index,
const math::RigidTransformd& X_WG) = 0;

/** The NVI-function for removing the geometry at the given `index`. If the
implementation _moves_ another geometry into this slot, the pre-move index
for the moved geometry is returned.
@param index The index of the geometry to remove.
@return The original index of the geometry that got moved to `index`. It
must be the case that the return value is either nullopt _or_ it
must not be equal to `index`. */
virtual optional<RenderIndex> DoRemoveGeometry(RenderIndex index) = 0;

/** The NVI-function for cloning this render engine. */
virtual std::unique_ptr<RenderEngine> DoClone() const = 0;

friend class RenderEngineTester;

private:
// The following two maps store all registered render index values to the
// corresponding geometry's internal index. It should be the case that the
// keys of the two maps are disjoint and span all of the valid render index
// values (i.e., [0, number of actors - 1]).
// The mapping is generally necessary to facilitate updates and geometry
// removal.

// The mapping from render index to internal index for those geometries whose
// poses must be updated in UpdateVisualPose.
std::unordered_map<RenderIndex, GeometryIndex> update_indices_;

// The mapping from render index to internal index of all other geometries.
std::unordered_map<RenderIndex, GeometryIndex> anchored_indices_;
};

} // namespace render
} // namespace geometry
} // namespace drake
Loading

0 comments on commit 411f242

Please sign in to comment.