src/capture/AstraCamera.cpp - master - klischa/AstraScanner2

src/capture/AstraCamera.cpp
394 строки14 KB
Arena Agent
fix: neural ICP confirm, cumulative transform direction, CMake cleanup, mesh smoothing, multi-seed BFS
02 авг 2026, 21:44
02 авг 2026, 21:4410986cc
#include "AstraCamera.h"
#include <QThread>
#include <QMutexLocker>
#include <QDebug>
#include <cstdlib>

namespace {
constexpr bool kVerboseCameraLogs = false;
}

AstraCamera::AstraCamera(QObject *parent) : QObject(parent) {}

AstraCamera::~AstraCamera()
{
    shutdown();
}

bool AstraCamera::openColorCamera()
{
    // Astra Pro RGB: 1280x720 @ 30fps MJPG via UVC.
    // We score each candidate camera by how well it matches these specs,
    // and prefer the highest-scoring one to avoid picking a laptop webcam.
    constexpr int kTargetWidth  = 1280;
    constexpr int kTargetHeight = 720;
    constexpr int kTargetFps    = 30;

    std::vector<cv::VideoCaptureAPIs> backends = {
        cv::CAP_DSHOW,
        cv::CAP_ANY,
        cv::CAP_MSMF
    };

    int bestIndex = -1;
    int bestScore = -1;
    cv::VideoCaptureAPIs bestBackend = cv::CAP_DSHOW;
    int bestW = 0, bestH = 0, bestFps = 0, bestFourcc = 0;

    for (int index = 0; index < 10; ++index) {
        for (auto backend : backends) {
            cv::VideoCapture capture(index, backend);
            if (!capture.isOpened()) continue;

            // Try to set Astra Pro parameters
            capture.set(cv::CAP_PROP_FRAME_WIDTH, kTargetWidth);
            capture.set(cv::CAP_PROP_FRAME_HEIGHT, kTargetHeight);
            capture.set(cv::CAP_PROP_FPS, kTargetFps);
            capture.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M','J','P','G'));

            cv::Mat frame;
            if (!capture.read(frame) || frame.empty()) continue;

            // Score this camera
            int score = 0;
            const int w = static_cast<int>(capture.get(cv::CAP_PROP_FRAME_WIDTH));
            const int h = static_cast<int>(capture.get(cv::CAP_PROP_FRAME_HEIGHT));
            const int fps = static_cast<int>(capture.get(cv::CAP_PROP_FPS));

            // Exact resolution match = big bonus
            if (w == kTargetWidth && h == kTargetHeight) score += 100;
            else if (w >= kTargetWidth && h >= kTargetHeight) score += 50;

            // FPS match
            if (fps >= kTargetFps) score += 20;

            // MJPG fourcc = USB camera trait (webcams often default to YUYV)
            const int fourcc = static_cast<int>(capture.get(cv::CAP_PROP_FOURCC));
            if (fourcc == cv::VideoWriter::fourcc('M','J','P','G')) score += 30;

            qInfo() << "[AstraCamera] Candidate" << index << "backend" << backend
                    << w << "x" << h << "@" << fps << "fps fourcc=" << fourcc
                    << "score=" << score;

            if (score > bestScore) {
                bestScore = score;
                bestIndex = index;
                bestBackend = backend;
                bestW = w;
                bestH = h;
                bestFps = fps;
                bestFourcc = fourcc;
            }
            // Release this candidate — we'll re-open the best one cleanly below
            capture.release();
        }
    }

    if (bestIndex >= 0) {
        // Re-open the best candidate directly into m_colorCapture to avoid
        // move-semantics issues with DirectShow COM handles.
        m_colorCapture.open(bestIndex, bestBackend);
        if (m_colorCapture.isOpened()) {
            m_colorCapture.set(cv::CAP_PROP_FRAME_WIDTH, bestW);
            m_colorCapture.set(cv::CAP_PROP_FRAME_HEIGHT, bestH);
            m_colorCapture.set(cv::CAP_PROP_FPS, bestFps);
            m_colorCapture.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M','J','P','G'));
            m_colorIndex = bestIndex;
            qInfo() << "Opened UVC camera index" << bestIndex << "score" << bestScore;
            return true;
        }
    }

    qCritical() << "Could not open UVC camera with any backend";
    return false;
}

void AstraCamera::enableEmulation(const std::string &reason)
{
    qWarning() << "Switching to emulation mode:" << QString::fromStdString(reason);
    m_emulation = true;
    m_initialized = true;
}

bool AstraCamera::initialize()
{
    if (m_emulation) {
        qInfo() << "Emulation mode enabled";
        m_initialized = true;
        return true;
    }

#ifndef ASTRA_HAVE_OPENNI2
    enableEmulation("Built without OpenNI2 SDK — emulation mode");
    return true;
#else
    // Only call shutdown() if OpenNI was previously initialized by *this* process.
    // Calling ::shutdown() without a matching ::initialize() is UB per OpenNI docs
    // and can corrupt state if other OpenNI clients live in the same process.
    static bool s_openniInitialized = false;
    if (s_openniInitialized) {
        openni::OpenNI::shutdown();
        s_openniInitialized = false;
    }

    qDebug() << "Initializing OpenNI...";
    openni::Status rc = openni::OpenNI::initialize();
    if (rc != openni::STATUS_OK) {
        m_lastError = "OpenNI init failed: " + std::string(openni::OpenNI::getExtendedError());
        qCritical() << m_lastError.c_str();
        enableEmulation(m_lastError);
        return true;
    }
    s_openniInitialized = true;

    rc = m_device.open(openni::ANY_DEVICE);
    if (rc != openni::STATUS_OK) {
        m_lastError = "Failed to open OpenNI device: " + std::string(openni::OpenNI::getExtendedError());
        qCritical() << m_lastError.c_str();
        openni::OpenNI::shutdown();
        enableEmulation(m_lastError);
        return true;
    }

    struct OBCameraParams {
        float l_intr_p[4];
        float r_intr_p[4];
        float r2l_r[9];
        float r2l_t[3];
        float k[5];
        int is_mirror;
    };
    OBCameraParams params{};
    int dataSize = sizeof(params);
    rc = m_device.getProperty(openni::OBEXTENSION_ID_CAM_PARAMS, (uint8_t*)&params, &dataSize);
    if (rc == openni::STATUS_OK) {
        QMutexLocker locker(&m_intrinsicsMutex);
        m_fx = params.l_intr_p[0];
        m_fy = params.l_intr_p[1];
        m_cx = params.l_intr_p[2];
        m_cy = params.l_intr_p[3];
        qInfo() << "Intrinsics from device: fx=" << m_fx << "fy=" << m_fy << "cx=" << m_cx << "cy=" << m_cy;
    } else {
        qWarning() << "Failed to get camera intrinsics";
    }

    if (!m_device.hasSensor(openni::SENSOR_DEPTH)) {
        m_lastError = "Depth sensor not available";
        qCritical() << m_lastError.c_str();
        m_device.close();
        openni::OpenNI::shutdown();
        m_colorCapture.release();
        enableEmulation(m_lastError);
        return true;
    }

    m_initialized = true;
    return true;
#endif // ASTRA_HAVE_OPENNI2
}

void AstraCamera::shutdown()
{
    if (m_released) {
        m_initialized = false;
        return;
    }
    stopStreams();
#ifdef ASTRA_HAVE_OPENNI2
    if (m_device.isValid()) m_device.close();
#endif
    if (m_colorCapture.isOpened()) m_colorCapture.release();
    m_initialized = false;
}

bool AstraCamera::startStreams()
{
    if (m_emulation) return true;
    if (!m_initialized) return false;

#ifndef ASTRA_HAVE_OPENNI2
    // Без OpenNI2 ре-инициализация потоков не имеет смысла; оставляем
    // emulation-путь.
    return true;
#else
    if (!m_device.hasSensor(openni::SENSOR_DEPTH)) {
        m_lastError = "Device has no depth sensor";
        return false;
    }

    openni::Status rc = m_depthStream.create(m_device, openni::SENSOR_DEPTH);
    if (rc != openni::STATUS_OK) {
        m_lastError = "Depth stream create failed";
        return false;
    }

    const openni::Array<openni::VideoMode>& modes = m_depthStream.getSensorInfo().getSupportedVideoModes();
    for (int i = 0; i < modes.getSize(); ++i) {
        openni::VideoMode mode = modes[i];
        if (mode.getResolutionX() == 640 && mode.getResolutionY() == 480 &&
            mode.getFps() == 30 && mode.getPixelFormat() == openni::PIXEL_FORMAT_DEPTH_1_MM) {
            m_depthStream.setVideoMode(mode);
            break;
        }
    }

    rc = m_depthStream.start();
    if (rc != openni::STATUS_OK) {
        m_lastError = "Depth stream start failed";
        return false;
    }

    // Register async frame listener — avoids the blocking readFrame() deadlock
    m_depthStream.addNewFrameListener(&m_depthListener);

    if (m_colorEnabled && m_device.isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR)) {
        m_device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
    }

    // Open UVC color camera AFTER depth stream started.
    // OpenNI2 grabs exclusive USB access to the Astra device which can
    // invalidate UVC handles opened before the depth stream.
    if (m_colorEnabled) {
        if (!openColorCamera()) {
            qWarning() << "[AstraCamera] UVC RGB camera unavailable after depth stream start";
            m_colorEnabled = false;
        }
    }

    return true;
#endif // ASTRA_HAVE_OPENNI2
}

void AstraCamera::stopStreams()
{
#ifdef ASTRA_HAVE_OPENNI2
    if (m_released) return;
    if (!m_emulation && m_depthStream.isValid()) {
        m_depthStream.removeNewFrameListener(&m_depthListener);
        m_depthStream.stop();
        m_depthStream.destroy();
    }
#endif
}

void AstraCamera::releaseForShutdown()
{
    m_released = true;
    m_stopRequested = true;
    if (m_colorCapture.isOpened()) m_colorCapture.release();
#ifdef ASTRA_HAVE_OPENNI2
    if (!m_emulation && m_depthStream.isValid()) {
        m_depthStream.removeNewFrameListener(&m_depthListener);
        m_depthListener.clear();
        m_depthStream.stop();
    }
#endif
}

bool AstraCamera::readFrame(cv::Mat &colorMat, cv::Mat &depthMat)
{
    if (m_emulation) {
        generateTestFrames(colorMat, depthMat);
        double minVal, maxVal;
        cv::minMaxLoc(depthMat, &minVal, &maxVal);
        if (kVerboseCameraLogs)
            qDebug() << "[AstraCamera] Emulation frame: depth.size=" << depthMat.cols << "x" << depthMat.rows << "depth.type=" << depthMat.type() 
                     << "min=" << minVal << "max=" << maxVal;
        return true;
    }

#ifndef ASTRA_HAVE_OPENNI2
    // Не должно случаться: без OpenNI2 initialize() уводит в emulation-mode,
    // так что сюда мы попасть не можем. Но на всякий случай — подстраховка.
    generateTestFrames(colorMat, depthMat);
    if (kVerboseCameraLogs)
        qDebug() << "[AstraCamera] No OpenNI2, using emulation frame";
    return true;
#else
    if (m_stopRequested) return false;

    {
        QMutexLocker colorLocker(&m_colorMutex);
        if (m_colorEnabled && m_colorCapture.isOpened()) {
            cv::Mat tempColor;
            if (m_colorCapture.read(tempColor) && !tempColor.empty()) {
                colorMat = tempColor.clone();
            } else {
                if (!m_colorDisconnectWarned) {
                    qWarning() << "RGB frame read failed (read returned"
                               << m_colorCapture.isOpened() << "), using gray fallback";
                    m_colorDisconnectWarned = true;
                }
                colorMat = cv::Mat(480, 640, CV_8UC3, cv::Scalar(200, 200, 200));
            }
        } else {
            if (m_colorEnabled && !m_colorCapture.isOpened() && !m_colorDisconnectWarned) {
                qWarning() << "Color camera disconnected mid-capture, falling back to gray frames";
                m_colorDisconnectWarned = true;
            }
            // Без RGB: создаём серый кадр размером depth-потока (640x480).
            colorMat = cv::Mat(480, 640, CV_8UC3, cv::Scalar(200, 200, 200));
        }
    }

    if (!m_depthStream.isValid()) return false;

    // Use async listener instead of blocking readFrame() — the listener
    // receives frames via callback and never blocks the calling thread.
    if (!m_depthListener.grab(m_depthFrame)) return false;

    const openni::DepthPixel* pDepth = static_cast<const openni::DepthPixel*>(m_depthFrame.getData());
    if (!pDepth) return false;

    const int width  = m_depthFrame.getWidth();
    const int height = m_depthFrame.getHeight();
    if (width <= 0 || height <= 0) {
        qWarning() << "[AstraCamera] Invalid depth frame size:" << width << "x" << height;
        return false;
    }

    const size_t frameBytes = static_cast<size_t>(width) * static_cast<size_t>(height) * sizeof(uint16_t);
    const int dataSize = m_depthFrame.getDataSize();
    if (dataSize < static_cast<int>(frameBytes)) {
        qWarning() << "[AstraCamera] Depth frame data size mismatch:" << dataSize << "vs expected" << frameBytes;
        return false;
    }

    depthMat = cv::Mat(height, width, CV_16UC1);
    if (!depthMat.isContinuous()) {
        qWarning() << "[AstraCamera] depthMat is not continuous – unexpected";
        return false;
    }
    memcpy(depthMat.data, pDepth, frameBytes);

    // Отладка: выведем статистику по глубине
    double minVal, maxVal;
    cv::minMaxLoc(depthMat, &minVal, &maxVal);
    if (kVerboseCameraLogs)
        qDebug() << "[AstraCamera] Depth frame: size=" << depthMat.cols << "x" << depthMat.rows << "type=" << depthMat.type() 
                 << "min=" << minVal << "max=" << maxVal << "mean=" << cv::mean(depthMat).val[0];

    return !depthMat.empty();
#endif // ASTRA_HAVE_OPENNI2
}

bool AstraCamera::getIntrinsics(float &fx, float &fy, float &cx, float &cy) const
{
    QMutexLocker locker(&m_intrinsicsMutex);
    fx = m_fx; fy = m_fy; cx = m_cx; cy = m_cy;
    return true;
}

void AstraCamera::generateTestFrames(cv::Mat &color, cv::Mat &depth)
{
    // Генерируем тестовые кадры напрямую без static-переменных,
    // чтобы обеспечить потокобезопасность. Для эмуляции это достаточно быстро.
    color = cv::Mat(480, 640, CV_8UC3, cv::Scalar(100, 100, 100));
    cv::putText(color, "EMULATION", cv::Point(200, 240),
                cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2);

    depth = cv::Mat(480, 640, CV_16UC1);
    cv::randu(depth, 500, 2000);

    QThread::msleep(33);
}