Added rendered_image_is_static option.

Author: peci1

README.md

@@ -72,6 +72,11 @@ viewpoint, useful for self-filtering, occlusion detection, and robot awareness a
                                                   values to decrease the resolution of the rendered image.
 - `~upscaling_interpolation` (string, default: "INTER_LINEAR"): OpenCV interpolation method for upscaling when the
                                                                 rendered image is smaller than camera info size.
+- `~rendered_image_is_static` (bool, default: false): Enable caching of rendered images for identical camera geometry.
+                                                      When true, subsequent camera infos with identical camera geometry
+                                                      will reuse the last rendered image without triggering rendering or
+                                                      TF lookups, but output images will have correct timestamps. Use
+                                                      this when the visible parts of the robot do not move.
 
 #### Lens Distortion
 

include/robot_model_renderer/RobotModelRenderer.hpp

@@ -94,6 +94,7 @@ struct RobotModelRendererConfig
   cv::Mat staticMaskImage;
   std::string staticMaskImageEncoding;  //!< Encoding from sensor_msgs/image_encodings.h . If empty, BGR(A) is assumed.
   bool staticMaskIsBackground {true};  //!< If false, the static mask image will be drawn over the rendered image.
+  bool renderedImageIsStatic {false};  //!< If true, cache rendered images for identical camera geometry.
 };
 
 struct RenderErrors
@@ -173,6 +174,8 @@ class RobotModelRenderer : public cras::HasLogger
   OgreStaticImage staticImagePass_;
 
   int cvImageType;
+
+  cv::Mat cached_image_;
 };
 
 }

include/robot_model_renderer/RosCameraRobotModelRenderer.hpp

@@ -73,6 +73,7 @@ struct RosCameraRobotModelRendererConfig
 
   sensor_msgs::Image staticMaskImage;
   bool staticMaskIsBackground {true};  //!< If false, the static mask image will be drawn over the rendered image.
+  bool renderedImageIsStatic {false};  //!< If true, cache rendered images for identical camera geometry.
 };
 
 /**

include/robot_model_renderer/c_api.h

@@ -83,6 +83,7 @@ struct robot_model_renderer_RobotModelRendererConfig
   int upscalingInterpolation;
   double renderImageScale;
   size_t maxRenderImageSize;
+  bool renderedImageIsStatic;
 
   size_t staticMaskImageWidth;
   size_t staticMaskImageHeight;

nodelets/robot_model_renderer_nodelet.cpp

@@ -331,6 +331,7 @@ class RobotModelRendererNodelet : public cras::Nodelet
     config.upscalingInterpolation = params->getParam("upscaling_interpolation", config.upscalingInterpolation);
     config.renderImageScale = params->getParam("render_image_scale", config.renderImageScale);
     config.maxRenderImageSize = params->getParam("max_render_image_size", config.maxRenderImageSize, "px (0 = none)");
+    config.renderedImageIsStatic = params->getParam("rendered_image_is_static", config.renderedImageIsStatic);
 
     const auto staticMaskImageFile = params->getParam("static_mask_image_file", "");
     const auto staticMaskImageFileEncoding = params->getParam("static_mask_image_encoding", "");

src/RobotModelRenderer.cpp

@@ -334,6 +334,13 @@ bool RobotModelRenderer::updateCameraInfo(const robot_model_renderer::PinholeCam
 
   CRAS_DEBUG_NAMED("camera_info", "Updating new camera info");
 
+  // Invalidate cache if camera geometry changes and static rendering is enabled
+  if (this->config.renderedImageIsStatic && this->cached_image_.total() > 0)
+  {
+    this->cached_image_ = cv::Mat();
+    CRAS_DEBUG_NAMED("camera_info", "Invalidating cached image due to camera geometry change");
+  }
+
   const auto prevOrigCamMsg = this->origCameraModel.cameraInfo();
   this->origCameraModel = model;
 
@@ -488,33 +495,47 @@ bool RobotModelRenderer::updateCameraInfo(const robot_model_renderer::PinholeCam
 
 cras::expected<cv::Mat, std::string> RobotModelRenderer::render(const ros::Time& time, RenderErrors& errors)
 {
+  // Check if we can use cached rendering for static images
+  if (this->config.renderedImageIsStatic && this->cached_image_.total() > 0)
+    return this->cached_image_.clone();
+
   auto renderResult = this->renderInner(time, errors);
   if (!renderResult.has_value())
     return renderResult;
 
-  if (!this->hasOverlays())
-    return *renderResult;
+  cv::Mat finalImage;
 
-  cv::Mat overlaidImg(renderResult->rows, renderResult->cols, this->cvImageType);
+  if (!this->hasOverlays())
+  {
+    finalImage = *renderResult;
+  }
+  else
   {
-    auto renderedImg = renderResult->isContinuous() ? *renderResult : renderResult->clone();
+    finalImage = cv::Mat(renderResult->rows, renderResult->cols, this->cvImageType);
+    {
+      auto renderedImg = renderResult->isContinuous() ? *renderResult : renderResult->clone();
 
-    auto renderLock {render_system_.lock()};
+      auto renderLock {render_system_.lock()};
 
-    // Draw the previously rendered scene onto overlay_scene_tex_ .
-    {
-      const Ogre::PixelBox renderPb(renderedImg.cols, renderedImg.rows, 1, this->config.pixelFormat, renderedImg.data);
-      overlay_scene_tex_->getBuffer()->blitFromMemory(renderPb);
-    }
+      // Draw the previously rendered scene onto overlay_scene_tex_ .
+      {
+        const Ogre::PixelBox renderPb(renderedImg.cols, renderedImg.rows, 1, this->config.pixelFormat,
+          renderedImg.data);
+        overlay_scene_tex_->getBuffer()->blitFromMemory(renderPb);
+      }
 
-    overlay_rt_->update();
+      overlay_rt_->update();
 
-    const Ogre::PixelBox pb(overlay_tex_->getWidth(), overlay_tex_->getHeight(), 1, this->config.pixelFormat,
-      overlaidImg.data);
-    overlay_rt_->copyContentsToMemory(pb);
+      const Ogre::PixelBox pb(overlay_tex_->getWidth(), overlay_tex_->getHeight(), 1, this->config.pixelFormat,
+        finalImage.data);
+      overlay_rt_->copyContentsToMemory(pb);
+    }
   }
 
-  return overlaidImg;
+  if (this->config.renderedImageIsStatic)
+    this->cached_image_ = finalImage.clone();
+
+  return finalImage;
 }
 
 cras::expected<cv::Mat, std::string> RobotModelRenderer::renderInner(const ros::Time& time, RenderErrors& errors)
@@ -662,6 +683,8 @@ void RobotModelRenderer::reset()
   {
     Ogre::MaterialManager::getSingleton().remove(overlay_material_->getName());
   }
+
+  cached_image_ = cv::Mat();
 }
 
 }

src/RosCameraRobotModelRenderer.cpp

@@ -19,6 +19,7 @@
 #include <robot_model_renderer/pinhole_camera.hpp>
 #include <robot_model_renderer/RobotModelRenderer.hpp>
 #include <robot_model_renderer/RosCameraRobotModelRenderer.hpp>
+#include <robot_model_renderer/utils/sensor_msgs.hpp>
 #include <robot_model_renderer/utils/sensor_msgs_ogre.hpp>
 #include <robot_model_renderer/utils/validate_floats.hpp>
 #include <tf2_ros/buffer.h>
@@ -71,6 +72,7 @@ RosCameraRobotModelRenderer::RosCameraRobotModelRenderer(
     robotConfig.staticMaskImage = cv_bridge::toCvCopy(config.staticMaskImage)->image;
   robotConfig.staticMaskImageEncoding = config.staticMaskImage.encoding;
   robotConfig.staticMaskIsBackground = config.staticMaskIsBackground;
+  robotConfig.renderedImageIsStatic = config.renderedImageIsStatic;
 
   this->linkUpdater = std::make_unique<TFROSLinkUpdater>(this->log, tf, "", "", config.tfTimeout);
   this->renderer = std::make_unique<RobotModelRenderer>(this->log, model, this->linkUpdater.get(),

src/c_api.cpp

@@ -83,6 +83,7 @@ inline robot_model_renderer::RobotModelRendererConfig convertConfig(
   cpp.upscalingInterpolation = static_cast<cv::InterpolationFlags>(c.upscalingInterpolation);
   cpp.renderImageScale = c.renderImageScale;
   cpp.maxRenderImageSize = c.maxRenderImageSize;
+  cpp.renderedImageIsStatic = c.renderedImageIsStatic;
 
   if (c.staticMaskImageWidth > 0 && c.staticMaskImageHeight > 0 && c.staticMaskImage != nullptr)
   {
@@ -142,6 +143,7 @@ robot_model_renderer_RobotModelRendererConfig robot_model_renderer_createDefault
   c.upscalingInterpolation = cpp.upscalingInterpolation;
   c.renderImageScale = cpp.renderImageScale;
   c.maxRenderImageSize = cpp.maxRenderImageSize;
+  c.renderedImageIsStatic = cpp.renderedImageIsStatic;
 
   c.staticMaskImageWidth = 0u;
   c.staticMaskImageHeight = 0u;

src/robot_model_renderer/types.py

@@ -179,6 +179,7 @@ class _RobotModelRendererConfig(Structure):
         ("upscalingInterpolation", c_int),
         ("renderImageScale", c_double),
         ("maxRenderImageSize", c_size_t),
+        ("renderedImageIsStatic", c_bool),
         ("staticMaskImageWidth", c_size_t),
         ("staticMaskImageHeight", c_size_t),
         ("staticMaskImageStep", c_size_t),
@@ -230,6 +231,7 @@ def __init__(self):
         self.upscalingInterpolation = conf.upscalingInterpolation
         self.renderImageScale = conf.renderImageScale
         self.maxRenderImageSize = conf.maxRenderImageSize
+        self.renderedImageIsStatic = conf.renderedImageIsStatic
         self.staticMaskImage = None  # An OpenCV-compatible Numpy array
         self.staticMaskImageEncoding = ""
         self.staticMaskIsBackground = conf.staticMaskIsBackground