luxonis / depthai-core

DepthAI C++ Library
MIT License
235 stars 127 forks source link

[BUG] {Segfault using the same camera stream for two different xlink outs} #1052

Closed onthegrid007 closed 3 months ago

onthegrid007 commented 4 months ago

Hello, so I needed to modify the spatial location calculator program because I need a stream of one of the left or right stereo cameras for a separate april tag detection algorithm. When running this program by default when the camera is turned on and reconnected I get a segfault, so running it under lldb the program runs fine with no segfault but the stereo display frame is intermittent and wither shows white or flashes a proper depth screaming when it wants.

image

I am using this code to get the stream output and I don't know If I can use the same dai::node::MonoCamera for two different dai::node::XLinkOut's...

// Create pipeline
    dai::Pipeline pipeline;

    // Define sources and outputs
    auto monoLeft{pipeline.create<dai::node::MonoCamera>()};
    auto monoRight{pipeline.create<dai::node::MonoCamera>()};
    auto stereo{pipeline.create<dai::node::StereoDepth>()};
    auto spatialDataCalculator{pipeline.create<dai::node::SpatialLocationCalculator>()};

    auto xoutMono{pipeline.create<dai::node::XLinkOut>()};
    auto xoutDepth{pipeline.create<dai::node::XLinkOut>()};
    auto xoutSpatialData{pipeline.create<dai::node::XLinkOut>()};
    auto xinSpatialCalcConfig{pipeline.create<dai::node::XLinkIn>()};

    xoutDepth->setStreamName("depth");
    xoutMono->setStreamName("apriltagdetection");
    xoutSpatialData->setStreamName("spatialData");
    xinSpatialCalcConfig->setStreamName("spatialCalcConfig");

    // Properties
    monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
    monoLeft->setCamera("left");
    monoLeft->setFps(120);
    monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
    monoRight->setCamera("right");
    monoRight->setFps(120);

    bool lrcheck = false;
    bool subpixel = false;

    stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY);
    stereo->setLeftRightCheck(lrcheck);
    stereo->setSubpixel(subpixel);

    // Config
    dai::Point2f topLeft(0.4f, 0.4f);
    dai::Point2f bottomRight(0.6f, 0.6f);

    dai::SpatialLocationCalculatorConfigData config;
    config.depthThresholds.lowerThreshold = 100;
    config.depthThresholds.upperThreshold = 10000;
    auto calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEDIAN;
    config.calculationAlgorithm = calculationAlgorithm;
    config.roi = dai::Rect(topLeft, bottomRight);

    spatialDataCalculator->inputConfig.setWaitForMessage(false);
    spatialDataCalculator->initialConfig.addROI(config);

    // Linking
    monoLeft->out.link(stereo->left);
    monoRight->out.link(stereo->right);
    monoLeft->out.link(xoutMono->input);

    spatialDataCalculator->passthroughDepth.link(xoutDepth->input);
    stereo->depth.link(spatialDataCalculator->inputDepth);

    spatialDataCalculator->out.link(xoutSpatialData->input);
    xinSpatialCalcConfig->out.link(spatialDataCalculator->inputConfig);

    // Connect to device and start pipeline
    dai::Device device(pipeline);

    // Output queue will be used to get the depth frames from the outputs defined above
    auto depthQueue = device.getOutputQueue("depth", 8, false);
    auto spatialCalcQueue = device.getOutputQueue("spatialData", 8, false);
    auto spatialCalcConfigInQueue = device.getInputQueue("spatialCalcConfig");
    auto apriltagdetectionQueue{device.getOutputQueue("apriltagdetection", 4, false)};

    const cv::Scalar color{255, 255, 255};

    Mat apriltagFrame, depthFrame, depthFrameColor;
    while(true) {
        apriltagFrame = apriltagdetectionQueue->get<dai::ImgFrame>()->getCvFrame();
        depthFrame = depthQueue->get<dai::ImgFrame>()->getFrame();

        imshow("apriltagRawFrame", apriltagFrame);

        cv::normalize(depthFrame, depthFrameColor, 255, 0, cv::NORM_INF, CV_8UC1);
        cv::equalizeHist(depthFrameColor, depthFrameColor);
        cv::applyColorMap(depthFrameColor, depthFrameColor, cv::COLORMAP_HOT);

        const auto spatialData{spatialCalcQueue->get<dai::SpatialLocationCalculatorData>()->getSpatialLocations()};
        for(auto depthData : spatialData) {
            auto roi{depthData.config.roi.denormalize(depthFrameColor.cols, depthFrameColor.rows)};
            auto [xmin, ymin] = roi.topLeft();
            auto [xmax, ymax] = roi.bottomRight();

            auto depthMin = depthData.depthMin;
            auto depthMax = depthData.depthMax;

            cv::rectangle(depthFrameColor, cv::Rect(cv::Point(xmin, ymin), cv::Point(xmax, ymax)), color, cv::FONT_HERSHEY_SIMPLEX);
            std::stringstream depthX;
            depthX << "X: " << (int)depthData.spatialCoordinates.x << " mm";
            cv::putText(depthFrameColor, depthX.str(), cv::Point(xmin + 10, ymin + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, color);
            std::stringstream depthY;
            depthY << "Y: " << (int)depthData.spatialCoordinates.y << " mm";
            cv::putText(depthFrameColor, depthY.str(), cv::Point(xmin + 10, ymin + 35), cv::FONT_HERSHEY_TRIPLEX, 0.5, color);
            std::stringstream depthZ;
            depthZ << "Z: " << (int)depthData.spatialCoordinates.z << " mm";
            cv::putText(depthFrameColor, depthZ.str(), cv::Point(xmin + 10, ymin + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, color);
        }
        // Show the frame
        cv::imshow("depth", depthFrameColor);

        int key = cv::waitKey(1);
        switch(key) {
            case 'q':
                return 0;
            // case 'd':
            //     if(bottomRight.x + stepSize <= 1) {
            //         topLeft.x += stepSize;
            //         bottomRight.x += stepSize;
            //         newConfig = true;
            //     }
            //     break;
            case '1':
                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEAN;
                newConfig = true;
                std::cout << "Switching calculation algorithm to MEAN!" << std::endl;
                break;
            case '2':
                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MIN;
                newConfig = true;
                std::cout << "Switching calculation algorithm to MIN!" << std::endl;
                break;
            case '3':
                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MAX;
                newConfig = true;
                std::cout << "Switching calculation algorithm to MAX!" << std::endl;
                break;
            case '4':
                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MODE;
                newConfig = true;
                std::cout << "Switching calculation algorithm to MODE!" << std::endl;
                break;
            case '5':
                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEDIAN;
                newConfig = true;
                std::cout << "Switching calculation algorithm to MEDIAN!" << std::endl;
                break;
            default:
                break;
        }

        if(newConfig) {
            config.roi = dai::Rect(topLeft, bottomRight);
            config.calculationAlgorithm = calculationAlgorithm;
            dai::SpatialLocationCalculatorConfig cfg;
            cfg.addROI(config);
            spatialCalcConfigInQueue->send(cfg);
            newConfig = false;
        }
    }

Any help would be appreciated

onthegrid007 commented 3 months ago

Pipeline was not setup correctly, fixed now...