Humble low bandwidth stereo fix (#624)

luxonis · Dec 3, 2024 · da0463f · da0463f
1 parent 7589290
commit da0463f
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 6 deletions.
diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt
@@ -5,7 +5,7 @@ set(CMAKE_BUILD_SHARED_LIBS ON)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+    add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-declarations -Wno-maybe-uninitialized)
 endif()
 # Default to C99
 if(NOT CMAKE_C_STANDARD)

diff --git a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp
@@ -97,7 +97,7 @@ void ImagePublisher::createImageConverter(std::shared_ptr<dai::Device> device) {
     if(convConfig.alphaScalingEnabled) {
         converter->setAlphaScaling(convConfig.alphaScaling);
     }
-    if(convConfig.outputDisparity) {
+    if(!convConfig.outputDisparity) {
         auto calHandler = device->readCalibration();
         double baseline = calHandler.getBaselineDistance(pubConfig.leftSocket, pubConfig.rightSocket, false);
         if(convConfig.reverseSocketOrder) {

diff --git a/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp b/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp
@@ -80,8 +80,9 @@ void Stereo::setNames() {
 
 void Stereo::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
     bool outputDisparity = ph->getParam<bool>("i_output_disparity");
+    bool lowBandwidth = ph->getParam<bool>("i_low_bandwidth");
     std::function<void(dai::Node::Input)> stereoLinkChoice;
-    if(outputDisparity) {
+    if(outputDisparity || lowBandwidth) {
         stereoLinkChoice = [&](auto input) { stereoCamNode->disparity.link(input); };
     } else {
         stereoLinkChoice = [&](auto input) { stereoCamNode->depth.link(input); };
@@ -92,7 +93,7 @@ void Stereo::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
         encConf.bitrate = ph->getParam<int>("i_low_bandwidth_bitrate");
         encConf.frameFreq = ph->getParam<int>("i_low_bandwidth_frame_freq");
         encConf.quality = ph->getParam<int>("i_low_bandwidth_quality");
-        encConf.enabled = ph->getParam<bool>("i_low_bandwidth");
+        encConf.enabled = lowBandwidth;
 
         stereoPub = setupOutput(pipeline, stereoQName, stereoLinkChoice, ph->getParam<bool>("i_synced"), encConf);
     }

diff --git a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp
@@ -54,7 +54,7 @@ void StereoParamHandler::updateSocketsFromParams(dai::CameraBoardSocket& left, d
 
 void StereoParamHandler::declareParams(std::shared_ptr<dai::node::StereoDepth> stereo) {
     declareAndLogParam<int>("i_max_q_size", 30);
-    declareAndLogParam<bool>("i_low_bandwidth", false);
+    bool lowBandwidth = declareAndLogParam<bool>("i_low_bandwidth", false);
     declareAndLogParam<int>("i_low_bandwidth_quality", 50);
     declareAndLogParam<int>("i_low_bandwidth_profile", 4);
     declareAndLogParam<int>("i_low_bandwidth_frame_freq", 30);
@@ -136,10 +136,13 @@ void StereoParamHandler::declareParams(std::shared_ptr<dai::node::StereoDepth> s
     stereo->initialConfig.setLeftRightCheckThreshold(declareAndLogParam<int>("i_lrc_threshold", 10));
     stereo->initialConfig.setMedianFilter(static_cast<dai::MedianFilter>(declareAndLogParam<int>("i_depth_filter_size", 5)));
     stereo->initialConfig.setConfidenceThreshold(declareAndLogParam<int>("i_stereo_conf_threshold", 240));
-    if(declareAndLogParam<bool>("i_subpixel", true)) {
+    if(declareAndLogParam<bool>("i_subpixel", true) && !lowBandwidth) {
         stereo->initialConfig.setSubpixel(true);
         stereo->initialConfig.setSubpixelFractionalBits(declareAndLogParam<int>("i_subpixel_fractional_bits", 3));
     }
+    else if(lowBandwidth) {
+        RCLCPP_INFO(getROSNode()->get_logger(), "Subpixel disabled due to low bandwidth mode");
+    }
     stereo->setRectifyEdgeFillColor(declareAndLogParam<int>("i_rectify_edge_fill_color", 0));
     if(declareAndLogParam<bool>("i_enable_alpha_scaling", false)) {
         stereo->setAlphaScaling(declareAndLogParam<float>("i_alpha_scaling", 0.0));