Consider region of interest in CameraDisplay

This was already implemented in Rviz1
ros2 · Jun 3, 2022 · 942aeb7 · 942aeb7
1 parent 7b4ba85
commit 942aeb7
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 2 deletions.
diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp
@@ -174,6 +174,10 @@ private Q_SLOTS:
     ImageDimensions dimensions,
     const Ogre::Vector2 & zoom) const;
 
+  Ogre::Vector4 calculateScreenCorners(
+    sensor_msgs::msg::CameraInfo::ConstSharedPtr info,
+    const Ogre::Vector2 & zoom) const;
+
   Ogre::SceneNode * background_scene_node_;
   Ogre::SceneNode * overlay_scene_node_;
 

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp
@@ -500,8 +500,9 @@ bool CameraDisplay::updateCamera()
   setStatus(StatusLevel::Ok, CAM_INFO_STATUS, "OK");
 
   // adjust the image rectangles to fit the zoom & aspect ratio
-  background_screen_rect_->setCorners(-1.0f * zoom.x, 1.0f * zoom.y, 1.0f * zoom.x, -1.0f * zoom.y);
-  overlay_screen_rect_->setCorners(-1.0f * zoom.x, 1.0f * zoom.y, 1.0f * zoom.x, -1.0f * zoom.y);
+  Ogre::Vector4 corners = calculateScreenCorners(info, zoom);
+  background_screen_rect_->setCorners(corners.x, corners.y, corners.z, corners.w);
+  overlay_screen_rect_->setCorners(corners.x, corners.y, corners.z, corners.w);
 
   Ogre::AxisAlignedBox aabInf;
   aabInf.setInfinite();
@@ -620,6 +621,31 @@ Ogre::Matrix4 CameraDisplay::calculateProjectionMatrix(
   return proj_matrix;
 }
 
+Ogre::Vector4 CameraDisplay::calculateScreenCorners(
+  const sensor_msgs::msg::CameraInfo::ConstSharedPtr info,
+  const Ogre::Vector2 & zoom) const
+{
+  float x_corner_start, y_corner_start, x_corner_end, y_corner_end;
+
+  if (info->roi.height != 0 || info->roi.width != 0)
+  {
+    // corners are computed according to roi
+    x_corner_start = (2.0 * info->roi.x_offset / info->width - 1.0) * zoom.x;
+    y_corner_start = (-2.0 * info->roi.y_offset / info->height + 1.0) * zoom.y;
+    x_corner_end = x_corner_start + (2.0 * info->roi.width / info->width) * zoom.x;
+    y_corner_end = y_corner_start - (2.0 * info->roi.height / info->height) * zoom.y;
+  }
+  else
+  {
+    x_corner_start = -1.0f * zoom.x;
+    y_corner_start = 1.0f * zoom.y;
+    x_corner_end = 1.0f * zoom.x;
+    y_corner_end = -1.0f * zoom.y;
+  }
+
+  return {x_corner_start, y_corner_start, x_corner_end, y_corner_end};
+}
+
 void CameraDisplay::processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg)
 {
   texture_->addMessage(msg);