Skip to content

Commit

Permalink
Merge pull request RobotLocomotion#8852 from EricCousineau-TRI/issue/…
Browse files Browse the repository at this point in the history 
…8850_pr1

rgbd_camera: Teach RgbdCamera to take different image sizes
  • Loading branch information
@EricCousineau-TRI
EricCousineau-TRI authored May 24, 2018
2 parents 9b8f7c1 + 9d1b5b8 commit e389a25
Show file tree
Hide file tree
Showing 6 changed files with 135 additions and 83 deletions.
4 changes: 3 additions & 1 deletion bindings/pydrake/systems/sensors_py.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,11 +191,13 @@ PYBIND11_MODULE(sensors, m) {
.def(
py::init<
string, const RigidBodyTree<T>&, const RigidBodyFrame<T>&,
double, double, double, bool>(),
double, double, double, bool, int, int>(),
py::arg("name"), py::arg("tree"), py::arg("frame"),
py::arg("z_near") = 0.5, py::arg("z_far") = 5.0,
py::arg("fov_y") = M_PI_4,
py::arg("show_window") = bool{RenderingConfig::kDefaultShowWindow},
py::arg("width") = int{RenderingConfig::kDefaultWidth},
py::arg("height") = int{RenderingConfig::kDefaultHeight},
// Keep alive, reference: `this` keeps `RigidBodyTree` alive.
py::keep_alive<1, 3>())
.def("color_camera_info", &RgbdCamera::color_camera_info,
Expand Down
16 changes: 13 additions & 3 deletions bindings/pydrake/systems/test/sensors_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,13 +206,23 @@ def test_rgbd_camera(self):
frame = RigidBodyFrame("rgbd camera frame", tree.FindBody("link"))
tree.addFrame(frame)

# Use HDTV size.
width = 1280
height = 720

camera = mut.RgbdCamera(
name="camera", tree=tree, frame=frame,
z_near=0.5, z_far=5.0,
fov_y=np.pi / 4, show_window=False)
fov_y=np.pi / 4, show_window=False,
width=width, height=height)

def check_info(camera_info):
self.assertIsInstance(camera_info, mut.CameraInfo)
self.assertEqual(camera_info.width(), width)
self.assertEqual(camera_info.height(), height)

self.assertIsInstance(camera.color_camera_info(), mut.CameraInfo)
self.assertIsInstance(camera.depth_camera_info(), mut.CameraInfo)
check_info(camera.color_camera_info())
check_info(camera.depth_camera_info())
self.assertIsInstance(camera.color_camera_optical_pose(), Isometry3)
self.assertIsInstance(camera.depth_camera_optical_pose(), Isometry3)
self.assertTrue(camera.tree() is tree)
Expand Down
48 changes: 24 additions & 24 deletions systems/sensors/rgbd_camera.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,14 +17,6 @@ namespace drake {
namespace systems {
namespace sensors {

namespace {

// TODO(kunimatsu-tri) Add support for the arbitrary image size
constexpr int kImageWidth = 640; // In pixels
constexpr int kImageHeight = 480; // In pixels

} // namespace

// Note that if `depth_image` holds any pixels that have NaN, the converted
// points will aslo become NaN.
void RgbdCamera::ConvertDepthImageToPointCloud(const ImageDepth32F& depth_image,
Expand Down Expand Up @@ -61,17 +53,18 @@ RgbdCamera::RgbdCamera(const std::string& name,
const RigidBodyTree<double>& tree,
const Eigen::Vector3d& position,
const Eigen::Vector3d& orientation, double z_near,
double z_far, double fov_y, bool show_window)
double z_far, double fov_y, bool show_window,
int width, int height)
: tree_(tree),
frame_(RigidBodyFrame<double>()),
camera_fixed_(true),
color_camera_info_(kImageWidth, kImageHeight, fov_y),
depth_camera_info_(kImageWidth, kImageHeight, fov_y),
color_camera_info_(width, height, fov_y),
depth_camera_info_(width, height, fov_y),
X_WB_initial_(
Eigen::Translation3d(position[0], position[1], position[2]) *
Eigen::Isometry3d(math::rpy2rotmat(orientation))),
renderer_(new RgbdRendererVTK(
RenderingConfig{kImageWidth, kImageHeight, fov_y, z_near, z_far,
RenderingConfig{width, height, fov_y, z_near, z_far,
show_window},
Eigen::Translation3d(position[0], position[1], position[2]) *
Eigen::Isometry3d(math::rpy2rotmat(orientation)) * X_BC_)) {
Expand All @@ -81,14 +74,15 @@ RgbdCamera::RgbdCamera(const std::string& name,
RgbdCamera::RgbdCamera(const std::string& name,
const RigidBodyTree<double>& tree,
const RigidBodyFrame<double>& frame, double z_near,
double z_far, double fov_y, bool show_window)
double z_far, double fov_y, bool show_window,
int width, int height)
: tree_(tree),
frame_(frame),
camera_fixed_(false),
color_camera_info_(kImageWidth, kImageHeight, fov_y),
depth_camera_info_(kImageWidth, kImageHeight, fov_y),
color_camera_info_(width, height, fov_y),
depth_camera_info_(width, height, fov_y),
renderer_(
new RgbdRendererVTK(RenderingConfig{kImageWidth, kImageHeight, fov_y,
new RgbdRendererVTK(RenderingConfig{width, height, fov_y,
z_near, z_far, show_window},
Eigen::Isometry3d::Identity())) {
Init(name);
Expand All @@ -101,15 +95,18 @@ void RgbdCamera::Init(const std::string& name) {

state_input_port_ = &this->DeclareInputPort(systems::kVectorValued, kVecNum);

ImageRgba8U color_image(kImageWidth, kImageHeight);
ImageRgba8U color_image(
color_camera_info_.width(), color_camera_info_.height());
color_image_port_ = &this->DeclareAbstractOutputPort(
sensors::ImageRgba8U(color_image), &RgbdCamera::OutputColorImage);

ImageDepth32F depth_image(kImageWidth, kImageHeight);
ImageDepth32F depth_image(
depth_camera_info_.width(), depth_camera_info_.height());
depth_image_port_ = &this->DeclareAbstractOutputPort(
sensors::ImageDepth32F(depth_image), &RgbdCamera::OutputDepthImage);

ImageLabel16I label_image(kImageWidth, kImageHeight);
ImageLabel16I label_image(
color_camera_info_.width(), color_camera_info_.height());
label_image_port_ = &this->DeclareAbstractOutputPort(
sensors::ImageLabel16I(label_image), &RgbdCamera::OutputLabelImage);

Expand Down Expand Up @@ -242,23 +239,25 @@ void RgbdCamera::OutputLabelImage(const Context<double>& context,
RgbdCameraDiscrete::RgbdCameraDiscrete(
std::unique_ptr<RgbdCamera> camera, double period, bool render_label_image)
: camera_(camera.get()), period_(period) {
constexpr int width = kImageWidth;
constexpr int height = kImageHeight;
const auto& color_camera_info = camera->color_camera_info();
const auto& depth_camera_info = camera->depth_camera_info();

DiagramBuilder<double> builder;
builder.AddSystem(std::move(camera));
input_port_state_ = builder.ExportInput(camera_->state_input_port());

// Color image.
const Value<ImageRgba8U> image_color(width, height);
const Value<ImageRgba8U> image_color(
color_camera_info.width(), color_camera_info.height());
const auto* const zoh_color =
builder.AddSystem<ZeroOrderHold>(period_, image_color);
builder.Connect(camera_->color_image_output_port(),
zoh_color->get_input_port());
output_port_color_image_ = builder.ExportOutput(zoh_color->get_output_port());

// Depth image.
const Value<ImageDepth32F> image_depth(width, height);
const Value<ImageDepth32F> image_depth(
depth_camera_info.width(), depth_camera_info.height());
const auto* const zoh_depth =
builder.AddSystem<ZeroOrderHold>(period_, image_depth);
builder.Connect(camera_->depth_image_output_port(),
Expand All @@ -267,7 +266,8 @@ RgbdCameraDiscrete::RgbdCameraDiscrete(

// Label image.
if (render_label_image) {
const Value<ImageLabel16I> image_label(width, height);
const Value<ImageLabel16I> image_label(
color_camera_info.width(), color_camera_info.height());
const auto* const zoh_label =
builder.AddSystem<ZeroOrderHold>(period_, image_label);
builder.Connect(camera_->label_image_output_port(),
Expand Down
8 changes: 6 additions & 2 deletions systems/sensors/rgbd_camera.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,9 @@ class RgbdCamera final : public LeafSystem<double> {
double z_near = 0.5,
double z_far = 5.0,
double fov_y = M_PI_4,
bool show_window = RenderingConfig::kDefaultShowWindow);
bool show_window = RenderingConfig::kDefaultShowWindow,
int width = RenderingConfig::kDefaultWidth,
int height = RenderingConfig::kDefaultHeight);

/// A constructor for %RgbdCamera that defines `B` using a RigidBodyFrame.
/// The pose of %RgbdCamera is fixed to a user-defined frame and will be
Expand Down Expand Up @@ -167,7 +169,9 @@ class RgbdCamera final : public LeafSystem<double> {
double z_near = 0.5,
double z_far = 5.0,
double fov_y = M_PI_4,
bool show_window = RenderingConfig::kDefaultShowWindow);
bool show_window = RenderingConfig::kDefaultShowWindow,
int width = RenderingConfig::kDefaultWidth,
int height = RenderingConfig::kDefaultHeight);

~RgbdCamera() = default;

Expand Down
8 changes: 6 additions & 2 deletions systems/sensors/rgbd_renderer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,13 @@ struct RenderingConfig {
// TODO(eric.cousineau): Define all default values be defined here to
// minimize duplication.
/// The width of the image to be rendered in pixels.
const int width;
const int width{kDefaultWidth};
/// Default value for `width`.
static constexpr int kDefaultWidth{640};
/// The height of the image to be rendered in pixels.
const int height;
const int height{kDefaultHeight};
/// Default value for `height`.
static constexpr int kDefaultHeight{480};
/// The renderer's camera vertical field of view in radians.
const double fov_y;
/// The minimum depth RgbdRenderer can output. Note that this is different
Expand Down
Loading

0 comments on commit e389a25

Please sign in to comment.