Add focus length parameter to stereo depth

Author: asahtik

depthai_ros_driver/config/rgbd.yaml

@@ -6,6 +6,8 @@
       i_pipeline_type: rgbd
     stereo:
       i_depth_preset: DEFAULT
+      # Optional disparity-to-depth override in pixels. Leave at default to use calibration.
+      # i_focal_length: 860.0
     # uncomment for better performance in RVC2 PoE devices
     #   i_publish_topic: false
     # rgb:

depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp

@@ -137,6 +137,10 @@ void StereoParamHandler::declareParams(std::shared_ptr<dai::node::StereoDepth> s
     if(declareAndLogParam<bool>("i_set_disparity_to_depth_use_spec_translation", false)) {
         stereo->setDisparityToDepthUseSpecTranslation(true);
     }
+    float focalLength = declareAndLogParam<float>("i_focal_length", 0.0f);
+    if(focalLength > 0.0f) {
+        stereo->setFocalLength(focalLength);
+    }
     //
     stereo->initialConfig->setBilateralFilterSigma(declareAndLogParam<int>("i_bilateral_sigma", 0));
     stereo->initialConfig->setLeftRightCheckThreshold(declareAndLogParam<int>("i_lrc_threshold", 10));