From 3c5d9c5eef88ca89d21a8bc4c48dc6b7ad9c337b Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Tue, 18 Jun 2019 10:14:23 -0700 Subject: [PATCH] Cleanup --- geometry/geometry_state.cc | 8 ++++---- geometry/geometry_state.h | 8 ++++---- geometry/query_object.cc | 16 ++++++++-------- geometry/query_object.h | 24 +++++++++--------------- geometry/scene_graph.cc | 14 +++++--------- geometry/scene_graph.h | 2 +- geometry/test/geometry_state_test.cc | 7 +++++++ geometry/test/query_object_test.cc | 4 ++-- geometry/test/scene_graph_test.cc | 3 --- 9 files changed, 40 insertions(+), 46 deletions(-) diff --git a/geometry/geometry_state.cc b/geometry/geometry_state.cc index d30fe7667dbe..9b7c54f4c092 100644 --- a/geometry/geometry_state.cc +++ b/geometry/geometry_state.cc @@ -938,8 +938,8 @@ template void GeometryState::RenderColorImage(const render::CameraProperties& camera, FrameId parent_frame, const RigidTransformd& X_PC, - ImageRgba8U* color_image_out, - bool show_window) const { + 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); @@ -966,8 +966,8 @@ template void GeometryState::RenderLabelImage(const render::CameraProperties& camera, FrameId parent_frame, const RigidTransformd& X_PC, - ImageLabel16I* label_image_out, - bool show_window) const { + 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); diff --git a/geometry/geometry_state.h b/geometry/geometry_state.h index eda9854b6c63..f333f62188c6 100644 --- a/geometry/geometry_state.h +++ b/geometry/geometry_state.h @@ -493,8 +493,8 @@ class GeometryState { void RenderColorImage(const render::CameraProperties& camera, FrameId parent_frame, const math::RigidTransformd& X_PC, - systems::sensors::ImageRgba8U* color_image_out, - bool show_window) const; + bool show_window, + systems::sensors::ImageRgba8U* color_image_out) const; /** Implementation support for QueryObject::RenderDepthImage(). @pre All poses have already been updated. */ @@ -508,8 +508,8 @@ class GeometryState { void RenderLabelImage(const render::CameraProperties& camera, FrameId parent_frame, const math::RigidTransformd& X_PC, - systems::sensors::ImageLabel16I* label_image_out, - bool show_window) const; + bool show_window, + systems::sensors::ImageLabel16I* label_image_out) const; //@} diff --git a/geometry/query_object.cc b/geometry/query_object.cc index 8d7becc31d88..1c70c57e5c39 100644 --- a/geometry/query_object.cc +++ b/geometry/query_object.cc @@ -118,14 +118,14 @@ template void QueryObject::RenderColorImage(const CameraProperties& camera, FrameId parent_frame, const RigidTransformd& X_PC, - ImageRgba8U* color_image_out, - bool show_window) const { + bool show_window, + ImageRgba8U* color_image_out) const { ThrowIfNotCallable(); FullPoseUpdate(); const GeometryState& state = geometry_state(); - return state.RenderColorImage(camera, parent_frame, X_PC, color_image_out, - show_window); + return state.RenderColorImage(camera, parent_frame, X_PC, show_window, + color_image_out); } template @@ -144,14 +144,14 @@ template void QueryObject::RenderLabelImage(const CameraProperties& camera, FrameId parent_frame, const RigidTransformd& X_PC, - ImageLabel16I* label_image_out, - bool show_window) const { + bool show_window, + ImageLabel16I* label_image_out) const { ThrowIfNotCallable(); FullPoseUpdate(); const GeometryState& state = geometry_state(); - return state.RenderLabelImage(camera, parent_frame, X_PC, label_image_out, - show_window); + return state.RenderLabelImage(camera, parent_frame, X_PC, show_window, + label_image_out); } template diff --git a/geometry/query_object.h b/geometry/query_object.h index 9a385c02313d..6b017b7240da 100644 --- a/geometry/query_object.h +++ b/geometry/query_object.h @@ -357,16 +357,10 @@ class QueryObject { extrinsic and intrinsic properties and %QueryObject renders into the provided image. - Eventually, there will be multiple renderers that can be invoked which vary - in the fidelity of the images they produce. Currently, only the low fidelity - renderer is implemented. Invocation on a higher level of fidelity will throw - an exception. As additional renderers get added, they will be engaged via - this same interface. - */ //@{ @@ -376,13 +370,13 @@ class QueryObject { @param camera The intrinsic properties of the camera. @param parent_frame The id for the camera's parent frame. @param X_PC The pose of the camera body in the world frame. - @param[out] color_image_out The rendered color image. - @param show_window If true, the render window will be displayed. */ + @param show_window If true, the render window will be displayed. + @param[out] color_image_out The rendered color image. */ void RenderColorImage(const render::CameraProperties& camera, FrameId parent_frame, const math::RigidTransformd& X_PC, - systems::sensors::ImageRgba8U* color_image_out, - bool show_window) const; + bool show_window, + systems::sensors::ImageRgba8U* color_image_out) const; /** Renders a depth image for the given `camera` posed with respect to the indicated parent frame P. @@ -406,13 +400,13 @@ class QueryObject { @param camera The intrinsic properties of the camera. @param parent_frame The id for the camera's parent frame. @param X_PC The pose of the camera body in the world frame. - @param[out] label_image_out The rendered label image. - @param show_window If true, the render window will be displayed. */ + @param show_window If true, the render window will be displayed. + @param[out] label_image_out The rendered label image. */ void RenderLabelImage(const render::CameraProperties& camera, FrameId parent_frame, const math::RigidTransformd& X_PC, - systems::sensors::ImageLabel16I* label_image_out, - bool show_window) const; + bool show_window, + systems::sensors::ImageLabel16I* label_image_out) const; //@} diff --git a/geometry/scene_graph.cc b/geometry/scene_graph.cc index 7b04d42787ee..0e4f70ad3cd1 100644 --- a/geometry/scene_graph.cc +++ b/geometry/scene_graph.cc @@ -18,14 +18,10 @@ namespace geometry { using render::RenderLabel; using systems::Context; using systems::InputPort; -using systems::LeafContext; using systems::LeafSystem; using systems::rendering::PoseBundle; -using systems::SystemOutput; -using systems::SystemSymbolicInspector; using systems::SystemTypeTag; using std::make_unique; -using std::unordered_map; using std::vector; namespace { @@ -107,7 +103,7 @@ SceneGraph::SceneGraph(const SceneGraph& other) : SceneGraph() { // time. // Therefore, for SourceIds i and j, the if i > j, then the port indices for // source i must all be greater than those for j. - std::vector source_ids; + vector source_ids; for (const auto pair : other.input_source_ids_) { source_ids.push_back(pair.first); } @@ -137,7 +133,7 @@ bool SceneGraph::SourceIsRegistered(SourceId id) const { } template -const systems::InputPort& SceneGraph::get_source_pose_port( +const InputPort& SceneGraph::get_source_pose_port( SourceId id) const { ThrowUnlessRegistered(id, "Can't acquire pose port for unknown source id: "); return this->get_input_port(input_source_ids_.at(id).pose_port); @@ -224,7 +220,7 @@ int SceneGraph::RendererCount() const { } template -std::vector SceneGraph::RegisteredRendererNames() const { +vector SceneGraph::RegisteredRendererNames() const { return initial_state_->RegisteredRendererNames(); } @@ -365,7 +361,7 @@ PoseBundle SceneGraph::MakePoseBundle() const { // *model*. // TODO(SeanCurtis-TRI): This happens *twice* now (once here and once in // CalcPoseBundle); might be worth refactoring/caching it. - std::vector dynamic_frames; + vector dynamic_frames; for (const auto& pair : g_state.frames_) { const FrameId frame_id = pair.first; if (frame_id == world_frame_id()) continue; @@ -403,7 +399,7 @@ void SceneGraph::CalcPoseBundle(const Context& context, // Collect only those frames that have illustration geometry -- based on the // *model*. - std::vector dynamic_frames; + vector dynamic_frames; for (const auto& pair : g_state.frames_) { const FrameId frame_id = pair.first; if (frame_id == world_frame_id()) continue; diff --git a/geometry/scene_graph.h b/geometry/scene_graph.h index 61aabac59d5d..b8a88419a751 100644 --- a/geometry/scene_graph.h +++ b/geometry/scene_graph.h @@ -481,7 +481,7 @@ class SceneGraph final : public systems::LeafSystem { @param name The unique name of the renderer. @param renderer The `renderer` to add. @throws std::logic_error if the name is not unique, or geometry has already - been registered. */ + been registered. */ void AddRenderer(std::string name, std::unique_ptr renderer); diff --git a/geometry/test/geometry_state_test.cc b/geometry/test/geometry_state_test.cc index 265af57a2fac..ace252100568 100644 --- a/geometry/test/geometry_state_test.cc +++ b/geometry/test/geometry_state_test.cc @@ -3391,6 +3391,13 @@ TEST_F(GeometryStateNoRendererTest, PerceptionRoleWithoutRenderer) { 0); } +// TODO(SeanCurtis-TRI): The `Render*Image` interface is insufficiently tested. +// GeometryState is a thin wrapper on the render engine, but GeometryState is +// responsible for: +// 1. Confirming the parent frame is valid. +// 2. Updating the camera pose. +// 3. Calling the appropriate render engine. + } // namespace } // namespace geometry } // namespace drake diff --git a/geometry/test/query_object_test.cc b/geometry/test/query_object_test.cc index 03d2d66b1e5e..8f81e848b513 100644 --- a/geometry/test/query_object_test.cc +++ b/geometry/test/query_object_test.cc @@ -190,7 +190,7 @@ TEST_F(QueryObjectTest, DefaultQueryThrows) { RigidTransformd X_WC = RigidTransformd::Identity(); ImageRgba8U color; EXPECT_DEFAULT_ERROR(default_object->RenderColorImage( - properties, FrameId::get_new_id(), X_WC, &color, false)); + properties, FrameId::get_new_id(), X_WC, false, &color)); ImageDepth32F depth; EXPECT_DEFAULT_ERROR(default_object->RenderDepthImage( @@ -198,7 +198,7 @@ TEST_F(QueryObjectTest, DefaultQueryThrows) { ImageLabel16I label; EXPECT_DEFAULT_ERROR(default_object->RenderLabelImage( - properties, FrameId::get_new_id(), X_WC, &label, false)); + properties, FrameId::get_new_id(), X_WC, false, &label)); #undef EXPECT_DEFAULT_ERROR } diff --git a/geometry/test/scene_graph_test.cc b/geometry/test/scene_graph_test.cc index ab46c77be1f4..dfeb498ab36a 100644 --- a/geometry/test/scene_graph_test.cc +++ b/geometry/test/scene_graph_test.cc @@ -30,14 +30,11 @@ namespace geometry { using Eigen::Isometry3d; using internal::DummyRenderEngine; -using math::RigidTransformd; -using render::RenderLabel; using systems::Context; using systems::rendering::PoseBundle; using systems::System; using std::make_unique; using std::unique_ptr; -using std::unordered_map; // Friend class for working with QueryObjects in a test context. class QueryObjectTester {