Fixed camera exposure with automatic gain control

src/bitbots_misc/bitbots_bringup/launch/vision.launch

@@ -4,6 +4,8 @@
     <arg name="sim" default="false" description="true: activates simulation time, switches to simulation color settings and deactivates launching of an image provider" />
     <arg name="camera" default="true" description="true: launches an image provider to get images from a camera (unless sim:=true)" />
     <arg name="debug" default="false" description="true: activates publishing of several debug images" />
+    <arg name="fixed_exposure_auto_gain" default="false" description="Use fixed camera exposure with software-controlled automatic gain" />
+    <arg name="camera_exposure" default="20" description="Fixed ZED exposure in the range 0 to 100" />
 
     <!-- Start the vision-->
     <include file="$(find-pkg-share bitbots_vision)/launch/vision.launch">
@@ -18,6 +20,14 @@
                 <arg name="camera_model" value="zedm" />
                 <arg name="publish_urdf" value="false" />
             </include>
+            <node if="$(var fixed_exposure_auto_gain)"
+                  pkg="bitbots_vision"
+                  exec="fixed_exposure_auto_gain"
+                  name="fixed_exposure_auto_gain"
+                  output="screen">
+                <param from="$(find-pkg-share bitbots_vision)/config/fixed_exposure_auto_gain.yaml" />
+                <param name="exposure" value="$(var camera_exposure)" />
+            </node>
         </group>
     </group>
 </launch>

src/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch

@@ -4,6 +4,8 @@
     <arg name="sim" default="false" description="true: activates simulation time, switches to simulation color settings and deactivates launching of an image provider" />
     <arg name="camera" default="true" description="true: launches an image provider to get images from a camera (unless sim:=true)" />
     <arg name="debug" default="false" description="true: activates publishing of several debug images" />
+    <arg name="fixed_exposure_auto_gain" default="false" description="Use fixed camera exposure with software-controlled automatic gain" />
+    <arg name="camera_exposure" default="20" description="Fixed ZED exposure in the range 0 to 100" />
     <arg unless="$(var sim)" name="fieldname" default="labor" description="Loads field settings" />
     <arg if="$(var sim)" name="fieldname" default="hsl_kid" description="Loads field settings" />
 
@@ -21,5 +23,7 @@
         <arg name="sim" value="$(var sim)" />
         <arg name="camera" value="$(var camera)" />
         <arg name="debug" value="$(var debug)" />
+        <arg name="fixed_exposure_auto_gain" value="$(var fixed_exposure_auto_gain)" />
+        <arg name="camera_exposure" value="$(var camera_exposure)" />
     </include>
 </launch>

src/bitbots_vision/CMakeLists.txt

@@ -65,8 +65,10 @@ generate_parameter_library(vision_parameters config/vision_parameters.yaml)
 # ---------------------------------------------------------------------------
 # Processing library (no ONNX dependency) – used by tests and the handler
 # ---------------------------------------------------------------------------
-add_library(bitbots_vision_processing SHARED
-            src/yoeo_processing.cpp src/debug_image.cpp src/model_config.cpp)
+add_library(
+  bitbots_vision_processing SHARED
+  src/yoeo_processing.cpp src/debug_image.cpp src/model_config.cpp
+  src/auto_gain_controller.cpp)
 
 target_include_directories(
   bitbots_vision_processing
@@ -110,6 +112,12 @@ ament_target_dependencies(
   std_msgs
   vision_msgs)
 
+add_executable(fixed_exposure_auto_gain src/fixed_exposure_auto_gain_node.cpp)
+
+target_link_libraries(fixed_exposure_auto_gain bitbots_vision_processing)
+
+ament_target_dependencies(fixed_exposure_auto_gain cv_bridge rclcpp sensor_msgs)
+
 # ---------------------------------------------------------------------------
 # Tests
 # ---------------------------------------------------------------------------
@@ -128,12 +136,15 @@ if(BUILD_TESTING)
   ament_add_gtest(test_processing test/test_processing.cpp)
   target_link_libraries(test_processing bitbots_vision_processing)
 
+  ament_add_gtest(test_auto_gain_controller test/test_auto_gain_controller.cpp)
+  target_link_libraries(test_auto_gain_controller bitbots_vision_processing)
+
   ament_add_gtest(test_yoeo_handler test/test_yoeo_handler.cpp)
   target_link_libraries(test_yoeo_handler bitbots_vision_lib)
 endif()
 
-install(TARGETS vision bitbots_vision_lib bitbots_vision_processing
-        DESTINATION lib/${PROJECT_NAME})
+install(TARGETS vision fixed_exposure_auto_gain bitbots_vision_lib
+                bitbots_vision_processing DESTINATION lib/${PROJECT_NAME})
 install(DIRECTORY include/ DESTINATION include)
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

src/bitbots_vision/README.md

@@ -1,5 +1,4 @@
-Bit-Bots Vision
-===============
+# Bit-Bots Vision
 
 [![CodeFactor](https://www.codefactor.io/repository/github/bit-bots/bitbots_vision/badge)](https://www.codefactor.io/repository/github/bit-bots/bitbots_vision)
   
@@ -10,11 +9,27 @@ This is the vision ROS package of the Hamburg Bit-Bots.
 
 The vision is able to detect balls, lines, the field itself, the field boundary, goal posts, teammates, enemies and other obstacles.
 
+## Fixed exposure with automatic gain
+
+The ZED Mini couples its hardware auto-exposure and auto-gain controls. The
+bringup package therefore provides an optional software gain controller that
+keeps exposure fixed and adjusts `video.gain` from the mean image brightness.
+
+```bash
+pixi run -e default ros2 launch bitbots_bringup vision_standalone.launch \
+  fixed_exposure_auto_gain:=true camera_exposure:=20
+```
+
+The controller defaults are in
+`config/fixed_exposure_auto_gain.yaml`. In particular,
+`target_brightness`, `brightness_deadband`, and `gain_kp` control its response.
+
 For further information and getting started, see the documentation of this package at our website: [docs.bit-bots.de](https://docs.bit-bots.de/package/bitbots_vision/latest/index.html)
 
 An earlier version of this pipeline is presented in our paper [An Open Source Vision Pipeline Approach for RoboCup Humanoid Soccer](https://robocup.informatik.uni-hamburg.de/wp-content/uploads/2019/06/vision_paper.pdf).
 When you use this pipeline or parts of it, please cite it.
-```
+
+```bibtex
 @inproceedings{vision2019,
 author={Fiedler, Niklas and Brandt, Hendrik and Gutsche, Jan and Vahl, Florian and Hagge, Jonas and Bestmann, Marc},
 year={2019},

src/bitbots_vision/config/fixed_exposure_auto_gain.yaml

@@ -0,0 +1,12 @@
+fixed_exposure_auto_gain:
+  ros__parameters:
+    image_topic: "zed/zed_node/rgb/image_rect_color"
+    camera_node: "/zed/zed_node"
+    initial_gain: 40
+    update_rate: 2.0
+    target_brightness: 110.0
+    brightness_deadband: 8.0
+    gain_kp: 0.08
+    max_gain_step: 4
+    min_gain: 0
+    max_gain: 100

src/bitbots_vision/include/bitbots_vision/auto_gain_controller.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <optional>
+
+namespace bitbots_vision {
+
+struct AutoGainConfig {
+  double target_brightness{110.0};
+  double brightness_deadband{8.0};
+  double gain_kp{0.08};
+  int max_gain_step{4};
+  int min_gain{0};
+  int max_gain{100};
+};
+
+class AutoGainController {
+ public:
+  explicit AutoGainController(AutoGainConfig config);
+
+  /// Returns a new gain when the brightness error requires an adjustment.
+  std::optional<int> update(double brightness, int current_gain) const;
+
+ private:
+  AutoGainConfig config_;
+};
+
+}  // namespace bitbots_vision

src/bitbots_vision/src/auto_gain_controller.cpp

@@ -0,0 +1,37 @@
+#include <algorithm>
+#include <bitbots_vision/auto_gain_controller.hpp>
+#include <cmath>
+#include <stdexcept>
+
+namespace bitbots_vision {
+
+AutoGainController::AutoGainController(AutoGainConfig config) : config_(config) {
+  if (config_.target_brightness < 0.0 || config_.target_brightness > 255.0) {
+    throw std::invalid_argument("target brightness must be between 0 and 255");
+  }
+  if (config_.brightness_deadband < 0.0 || config_.gain_kp <= 0.0 || config_.max_gain_step <= 0 ||
+      config_.min_gain < 0 || config_.max_gain > 100 || config_.min_gain > config_.max_gain) {
+    throw std::invalid_argument("invalid auto gain configuration");
+  }
+}
+
+std::optional<int> AutoGainController::update(double brightness, int current_gain) const {
+  const double error = config_.target_brightness - brightness;
+  if (std::abs(error) <= config_.brightness_deadband) {
+    return std::nullopt;
+  }
+
+  int step = static_cast<int>(std::round(config_.gain_kp * error));
+  if (step == 0) {
+    step = error > 0.0 ? 1 : -1;
+  }
+  step = std::clamp(step, -config_.max_gain_step, config_.max_gain_step);
+
+  const int new_gain = std::clamp(current_gain + step, config_.min_gain, config_.max_gain);
+  if (new_gain == current_gain) {
+    return std::nullopt;
+  }
+  return new_gain;
+}
+
+}  // namespace bitbots_vision

src/bitbots_vision/src/fixed_exposure_auto_gain_node.cpp

@@ -0,0 +1,141 @@
+#include <bitbots_vision/auto_gain_controller.hpp>
+#include <chrono>
+#include <cv_bridge/cv_bridge.hpp>
+#include <functional>
+#include <memory>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
+#include <rclcpp/parameter_client.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace bitbots_vision {
+
+class FixedExposureAutoGainNode : public rclcpp::Node {
+ public:
+  FixedExposureAutoGainNode() : Node("fixed_exposure_auto_gain") {
+    const auto image_topic = declare_parameter<std::string>("image_topic", "zed/zed_node/rgb/image_rect_color");
+    const auto camera_node = declare_parameter<std::string>("camera_node", "/zed/zed_node");
+    exposure_ = static_cast<int>(declare_parameter<int>("exposure", 20));
+    current_gain_ = static_cast<int>(declare_parameter<int>("initial_gain", 40));
+    const double update_rate = declare_parameter<double>("update_rate", 2.0);
+    if (update_rate <= 0.0) {
+      throw std::invalid_argument("update rate must be positive");
+    }
+    update_period_ = rclcpp::Duration::from_seconds(1.0 / update_rate);
+
+    AutoGainConfig config;
+    config.target_brightness = declare_parameter<double>("target_brightness", 110.0);
+    config.brightness_deadband = declare_parameter<double>("brightness_deadband", 8.0);
+    config.gain_kp = declare_parameter<double>("gain_kp", 0.08);
+    config.max_gain_step = static_cast<int>(declare_parameter<int>("max_gain_step", 4));
+    config.min_gain = static_cast<int>(declare_parameter<int>("min_gain", 0));
+    config.max_gain = static_cast<int>(declare_parameter<int>("max_gain", 100));
+    controller_ = std::make_unique<AutoGainController>(config);
+
+    if (exposure_ < 0 || exposure_ > 100 || current_gain_ < config.min_gain || current_gain_ > config.max_gain) {
+      throw std::invalid_argument("exposure or initial gain is outside the configured range");
+    }
+
+    camera_parameters_ = std::make_shared<rclcpp::AsyncParametersClient>(this, camera_node);
+    setup_timer_ =
+        create_wall_timer(std::chrono::seconds(1), std::bind(&FixedExposureAutoGainNode::configure_camera, this));
+
+    image_sub_ = create_subscription<sensor_msgs::msg::Image>(
+        image_topic, rclcpp::SensorDataQoS(),
+        std::bind(&FixedExposureAutoGainNode::image_callback, this, std::placeholders::_1));
+  }
+
+ private:
+  void configure_camera() {
+    if (!camera_parameters_->service_is_ready() || parameter_update_pending_) {
+      return;
+    }
+
+    parameter_update_pending_ = true;
+    const std::vector<rclcpp::Parameter> parameters{
+        rclcpp::Parameter("video.auto_exposure_gain", false),
+        rclcpp::Parameter("video.exposure", exposure_),
+        rclcpp::Parameter("video.gain", current_gain_),
+    };
+    camera_parameters_->set_parameters(parameters, [this](const auto future) {
+      parameter_update_pending_ = false;
+      const auto results = future.get();
+      if (results.size() != 3 || !results[0].successful || !results[1].successful || !results[2].successful) {
+        RCLCPP_ERROR(get_logger(), "Failed to configure fixed exposure and manual gain");
+        return;
+      }
+
+      camera_configured_ = true;
+      setup_timer_->cancel();
+      RCLCPP_INFO(get_logger(), "Fixed exposure at %d; software auto-gain started at %d", exposure_, current_gain_);
+    });
+  }
+
+  void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg) {
+    const auto now = get_clock()->now();
+    if (!camera_configured_ || parameter_update_pending_ || now - last_update_ < update_period_) {
+      return;
+    }
+    last_update_ = now;
+
+    cv::Mat gray;
+    try {
+      gray = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8)->image;
+    } catch (const cv_bridge::Exception& error) {
+      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "Cannot measure image brightness: %s", error.what());
+      return;
+    }
+
+    cv::Mat sampled;
+    cv::resize(gray, sampled, cv::Size(), 0.125, 0.125, cv::INTER_AREA);
+    const double brightness = cv::mean(sampled)[0];
+    const auto requested_gain = controller_->update(brightness, current_gain_);
+    if (!requested_gain.has_value()) {
+      return;
+    }
+
+    parameter_update_pending_ = true;
+    const int new_gain = requested_gain.value();
+    camera_parameters_->set_parameters(
+        {rclcpp::Parameter("video.gain", new_gain)}, [this, new_gain, brightness](const auto future) {
+          parameter_update_pending_ = false;
+          const auto results = future.get();
+          if (results.size() != 1 || !results[0].successful) {
+            RCLCPP_WARN(get_logger(), "Failed to set camera gain to %d", new_gain);
+            return;
+          }
+
+          current_gain_ = new_gain;
+          RCLCPP_DEBUG(get_logger(), "Brightness %.1f, camera gain %d", brightness, current_gain_);
+        });
+  }
+
+  int exposure_;
+  int current_gain_;
+  rclcpp::Duration update_period_{0, 0};
+  rclcpp::Time last_update_{0, 0, RCL_ROS_TIME};
+  bool camera_configured_{false};
+  bool parameter_update_pending_{false};
+  std::unique_ptr<AutoGainController> controller_;
+  std::shared_ptr<rclcpp::AsyncParametersClient> camera_parameters_;
+  rclcpp::TimerBase::SharedPtr setup_timer_;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+};
+
+}  // namespace bitbots_vision
+
+int main(int argc, char** argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<bitbots_vision::FixedExposureAutoGainNode>();
+  rclcpp::experimental::executors::EventsExecutor executor;
+  executor.add_node(node);
+  executor.spin();
+  rclcpp::shutdown();
+  return 0;
+}

src/bitbots_vision/test/test_auto_gain_controller.cpp

@@ -0,0 +1,46 @@
+#include <gtest/gtest.h>
+
+#include <bitbots_vision/auto_gain_controller.hpp>
+#include <stdexcept>
+
+using bitbots_vision::AutoGainConfig;
+using bitbots_vision::AutoGainController;
+
+TEST(AutoGainController, HoldsGainInsideDeadband) {
+  AutoGainController controller(AutoGainConfig{});
+  EXPECT_FALSE(controller.update(105.0, 40).has_value());
+  EXPECT_FALSE(controller.update(118.0, 40).has_value());
+}
+
+TEST(AutoGainController, RaisesGainForDarkImage) {
+  AutoGainController controller(AutoGainConfig{});
+  EXPECT_EQ(controller.update(50.0, 40), 44);
+}
+
+TEST(AutoGainController, LowersGainForBrightImage) {
+  AutoGainController controller(AutoGainConfig{});
+  EXPECT_EQ(controller.update(180.0, 40), 36);
+}
+
+TEST(AutoGainController, UsesAtLeastOneGainStepOutsideDeadband) {
+  AutoGainConfig config;
+  config.brightness_deadband = 0.0;
+  config.gain_kp = 0.001;
+  AutoGainController controller(config);
+
+  EXPECT_EQ(controller.update(109.0, 40), 41);
+  EXPECT_EQ(controller.update(111.0, 40), 39);
+}
+
+TEST(AutoGainController, RespectsGainLimits) {
+  AutoGainController controller(AutoGainConfig{});
+  EXPECT_FALSE(controller.update(0.0, 100).has_value());
+  EXPECT_FALSE(controller.update(255.0, 0).has_value());
+}
+
+TEST(AutoGainController, RejectsInvalidConfiguration) {
+  AutoGainConfig config;
+  config.min_gain = 80;
+  config.max_gain = 20;
+  EXPECT_THROW(AutoGainController controller(config), std::invalid_argument);
+}