Fix: Early return when no detections in SpatialLocationCalculator (#1824)

* Fix early return

* Move segmask passthrough to SpatialLocationCalculator instead

* Bump FW

* Bump FW

Author: aljazkonec1

cmake/Depthai/DepthaiDeviceRVC4Config.cmake

@@ -3,4 +3,4 @@
 set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot")
 
 # "version if applicable"
-set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+ffaaa351f971c291311f8ab5d8d0753f327799b3")
+set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+44b5f9a40174dac9670e4f6f411e9b4daf6e6f76")

src/pipeline/node/SpatialLocationCalculator.cpp

@@ -100,6 +100,13 @@ void SpatialLocationCalculator::run() {
             auto stop = high_resolution_clock::now();
             auto timeToComputeSpatialDetections = duration_cast<microseconds>(stop - start);
             logger->trace("Time to compute spatial detections: {} us", timeToComputeSpatialDetections.count());
+
+            if(calculationConfig->segmentationPassthrough && !imgDetections->getData().empty()) {
+                outputSpatialImgDetections->data = imgDetections->data;
+                outputSpatialImgDetections->segmentationMaskWidth = imgDetections->getSegmentationMaskWidth();
+                outputSpatialImgDetections->segmentationMaskHeight = imgDetections->getSegmentationMaskHeight();
+            }
+
             outputSpatialImgDetections->setSequenceNum(imgDetections->getSequenceNum());
             outputSpatialImgDetections->setTimestampDevice(imgDetections->getTimestampDevice());
             outputSpatialImgDetections->setTimestamp(imgDetections->getTimestamp());

src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp

@@ -350,7 +350,6 @@ dai::SpatialImgDetection computeSpatialDetection(const dai::ImgFrame& depthFrame
     if(!areAligned) centerPoint = depthTransformation->remapPointTo(detectionsTransformation, centerPoint);
     dai::Point3f spatialCoordinates =
         calculateSpatialCoordinates(depthStats.calculateDepth(calculationAlgorithm), detectionsTransformation.getIntrinsicMatrix(), centerPoint);
-    logger->trace("Calculated spatial coordinates: {} {} {}", spatialCoordinates.x, spatialCoordinates.y, spatialCoordinates.z);
 
     dai::SpatialImgDetection spatialDetection = createSpatialDetection(detection, spatialCoordinates);
 
@@ -418,9 +417,6 @@ void computeSpatialDetections(const dai::ImgFrame& depthFrame,
     }
 
     std::vector<dai::ImgDetection> imgDetectionsVector = imgDetections.detections;
-    if(imgDetectionsVector.size() == 0) {
-        return;
-    }
     spatialDetections.detections.resize(imgDetectionsVector.size());
 
     const SpatialLocationCalculatorAlgorithm calculationAlgorithm = config.globalCalculationAlgorithm;
@@ -432,7 +428,11 @@ void computeSpatialDetections(const dai::ImgFrame& depthFrame,
     const std::size_t segmentationMaskHeight = imgDetections.getSegmentationMaskHeight();
 
     span<const std::uint8_t> maskSpan = imgDetections.getData();
-    const bool passthroughSegmentation = config.segmentationPassthrough && !maskSpan.empty();
+
+    if(imgDetectionsVector.size() == 0) {
+        return;
+    }
+
     if(!config.useSegmentation) {  // ignore segmentation mask even if provided
         maskSpan = span<const std::uint8_t>{};
     }
@@ -458,12 +458,6 @@ void computeSpatialDetections(const dai::ImgFrame& depthFrame,
                                                                                logger);
         spatialDetections.detections[i] = spatialImgDetection;
     }
-
-    if(passthroughSegmentation) {
-        spatialDetections.data = imgDetections.data;
-        spatialDetections.segmentationMaskWidth = imgDetections.getSegmentationMaskWidth();
-        spatialDetections.segmentationMaskHeight = imgDetections.getSegmentationMaskHeight();
-    }
 }
 
 }  // namespace SpatialUtils

tests/src/ondevice_tests/pipeline/node/spatial_location_calculator_test.cpp

@@ -604,6 +604,49 @@ TEST_CASE("Segmentation passthrough can be toggled") {
     }
 }
 
+TEST_CASE("Segmentation passthrough preserves mask when detections are empty") {
+    constexpr unsigned width = 320;
+    constexpr unsigned height = 240;
+    const std::array<std::array<float, 3>, 3> intrinsics = {{
+        {{5.0F, 0.0F, 1.0F}},
+        {{0.0F, 5.0F, 1.0F}},
+        {{0.0F, 0.0F, 1.0F}},
+    }};
+
+    cv::Mat depthMat(height, width, CV_16UC1, cv::Scalar(2000));
+    auto depthFrame = createDepthFrame(depthMat, intrinsics);
+
+    std::vector<std::uint8_t> mask(width * height, 255);
+    for(int y = 20; y < 60; ++y) {
+        for(int x = 30; x < 90; ++x) {
+            mask.at(y * static_cast<int>(width) + x) = 0;
+        }
+    }
+
+    dai::SpatialLocationCalculatorConfig initialConfig;
+    initialConfig.setCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm::AVERAGE);
+    initialConfig.setDepthThresholds(0, 10000);
+    initialConfig.setUseSegmentation(true);
+    initialConfig.setSegmentationPassthrough(true);
+    initialConfig.setCalculateSpatialKeypoints(false);
+
+    auto detectionMsg = std::make_shared<dai::ImgDetections>();
+    detectionMsg->setSegmentationMask(mask, width, height);
+    detectionMsg->transformation = depthFrame->transformation;
+    detectionMsg->setTimestamp(depthFrame->getTimestamp());
+    detectionMsg->setSequenceNum(depthFrame->getSequenceNum());
+    auto [unusedLegacy, unusedPassthrough, spatialDetections] = processDepthFrame(initialConfig, depthFrame, std::nullopt, detectionMsg);
+    static_cast<void>(unusedLegacy);
+    static_cast<void>(unusedPassthrough);
+
+    CHECK(spatialDetections.detections.empty());
+    const auto outMask = spatialDetections.getMaskData();
+    REQUIRE(outMask.has_value());
+    CHECK(*outMask == mask);
+    CHECK(spatialDetections.getSegmentationMaskWidth() == width);
+    CHECK(spatialDetections.getSegmentationMaskHeight() == height);
+}
+
 TEST_CASE("Spatial detections remap depth to detection transformations") {
     using std::chrono::steady_clock;
     const auto RESET_REMOTE_TIMEOUT_MS = std::chrono::milliseconds(2000);