From 46f025327b1dd8ca284360d9f19158b3c10c1725 Mon Sep 17 00:00:00 2001 From: Ben <987807597@qq.com> Date: Wed, 26 Aug 2026 10:19:47 +0800 Subject: [PATCH] fix: capture full frame with ROI restoration --- src/VizumCamera.cpp | 190 ++++++++++++++++++++++++++++++++ src/VizumCamera.h | 15 ++- src/core/StructuredLightTypes.h | 4 + 3 files changed, 206 insertions(+), 3 deletions(-) diff --git a/src/VizumCamera.cpp b/src/VizumCamera.cpp index cd0fce1..0269cd5 100755 --- a/src/VizumCamera.cpp +++ b/src/VizumCamera.cpp @@ -106,6 +106,100 @@ struct VizumCamera::Impl { #ifdef ENABLE_VIZUM_CAMERA namespace { +class DetectRoiRestoreGuard +{ +public: + DetectRoiRestoreGuard(VZNLHANDLE device, + const SVzNLROIRect& leftROI, + const SVzNLROIRect& rightROI) + : m_device(device), m_leftROI(leftROI), m_rightROI(rightROI) + { + } + + bool RestoreNow() + { + if (m_restored) + return true; + + const int ret = VzNL_ConfigDetectROI(m_device, &m_leftROI, &m_rightROI); + if (ret != 0) { + char errorMsg[256] = {0}; + VzNL_GetErrorInfo(ret, errorMsg); + LOG_ERROR("[VizumCamera] Failed to restore detection ROI: {} (error code: {})", + errorMsg, ret); + return false; + } + + m_restored = true; + LOG_INFO("[VizumCamera] Detection ROI restored: left=[{}, {}, {}, {}], " + "right=[{}, {}, {}, {}]", + m_leftROI.left, m_leftROI.right, m_leftROI.top, m_leftROI.bottom, + m_rightROI.left, m_rightROI.right, m_rightROI.top, m_rightROI.bottom); + return true; + } + + ~DetectRoiRestoreGuard() + { + if (!m_restored && !RestoreNow()) + LOG_ERROR("[VizumCamera] Emergency detection ROI restoration failed"); + } + + DetectRoiRestoreGuard(const DetectRoiRestoreGuard&) = delete; + DetectRoiRestoreGuard& operator=(const DetectRoiRestoreGuard&) = delete; + +private: + VZNLHANDLE m_device = nullptr; + SVzNLROIRect m_leftROI{}; + SVzNLROIRect m_rightROI{}; + bool m_restored = false; +}; + +bool NormalizeToDeviceFrame(SLImage& image, uint32_t frameWidth, uint32_t frameHeight) +{ + if (image.empty() || frameWidth == 0 || frameHeight == 0 + || image.originX < 0 || image.originY < 0 || image.channels <= 0) { + return false; + } + + const uint64_t right = static_cast(image.originX) + image.width; + const uint64_t bottom = static_cast(image.originY) + image.height; + if (right > frameWidth || bottom > frameHeight) + return false; + + if (image.originX == 0 && image.originY == 0 + && image.width == frameWidth && image.height == frameHeight) { + return true; + } + + const size_t bytesPerChannel = image.bitDepth > 8 ? 2u : 1u; + const size_t pixelBytes = static_cast(image.channels) * bytesPerChannel; + const size_t sourceRowBytes = static_cast(image.width) * pixelBytes; + if (image.data.size() < sourceRowBytes * image.height) + return false; + + SLImage fullImage; + fullImage.width = frameWidth; + fullImage.height = frameHeight; + fullImage.originX = 0; + fullImage.originY = 0; + fullImage.channels = image.channels; + fullImage.bitDepth = image.bitDepth; + fullImage.data.assign(static_cast(frameWidth) * frameHeight * pixelBytes, 0); + + const size_t targetRowBytes = static_cast(frameWidth) * pixelBytes; + const size_t targetXBytes = static_cast(image.originX) * pixelBytes; + for (uint32_t row = 0; row < image.height; ++row) { + const size_t sourceOffset = static_cast(row) * sourceRowBytes; + const size_t targetOffset = (static_cast(image.originY) + row) * targetRowBytes + + targetXBytes; + std::memcpy(fullImage.data.data() + targetOffset, + image.data.data() + sourceOffset, sourceRowBytes); + } + + image = std::move(fullImage); + return true; +} + const char* DeviceStatusName(int status) { switch (static_cast(status)) { @@ -826,6 +920,8 @@ bool VizumCamera::ConvertVzImageToSLImage(void* vzImagePtr, SLImage& slImage) slImage.width = vzImage->nWidth; slImage.height = vzImage->nHeight; + slImage.originX = vzImage->ptOriPos.x; + slImage.originY = vzImage->ptOriPos.y; slImage.channels = vzImage->nChannels; slImage.bitDepth = vzImage->byBitDepth; @@ -864,6 +960,100 @@ bool VizumCamera::GetRightImage(SLImage& image) return !image.empty(); } +bool VizumCamera::CaptureFullFrameLeftImage(SLImage& image, std::string& frameId) +{ + image.clear(); + frameId.clear(); +#ifdef ENABLE_VIZUM_CAMERA + const std::lock_guard deviceLock(m_deviceAccessMutex); + if (!m_isConnected || !m_deviceHandle) { + LOG_ERROR("[VizumCamera] Full-frame capture failed: device not connected"); + return false; + } + + SVzNLROIRect oldLeftROI{}; + SVzNLROIRect oldRightROI{}; + int ret = VzNL_GetConfigDetectROI(m_deviceHandle, &oldLeftROI, &oldRightROI); + if (ret != 0) { + char errorMsg[256] = {0}; + VzNL_GetErrorInfo(ret, errorMsg); + LOG_ERROR("[VizumCamera] Full-frame capture cannot read detection ROI: {} " + "(error code: {})", errorMsg, ret); + return false; + } + + SVzVideoResolution resolution{}; + ret = VzNL_GetResolution(m_deviceHandle, &resolution); + if (ret != 0 || resolution.nFrameWidth == 0 || resolution.nFrameHeight == 0) { + char errorMsg[256] = {0}; + VzNL_GetErrorInfo(ret, errorMsg); + LOG_ERROR("[VizumCamera] Full-frame capture cannot read device resolution: {} " + "(error code: {}, size={}x{})", errorMsg, ret, + resolution.nFrameWidth, resolution.nFrameHeight); + return false; + } + + DetectRoiRestoreGuard restoreGuard(m_deviceHandle, oldLeftROI, oldRightROI); + const SVzNLROIRect fullLeftROI{0, static_cast(resolution.nFrameWidth), + 0, static_cast(resolution.nFrameHeight)}; + const SVzNLROIRect fullRightROI = fullLeftROI; + ret = VzNL_ConfigDetectROI(m_deviceHandle, &fullLeftROI, &fullRightROI); + if (ret != 0) { + char errorMsg[256] = {0}; + VzNL_GetErrorInfo(ret, errorMsg); + LOG_ERROR("[VizumCamera] Failed to apply temporary full-frame ROI: {} " + "(error code: {}, size={}x{})", errorMsg, ret, + resolution.nFrameWidth, resolution.nFrameHeight); + return false; + } + LOG_INFO("[VizumCamera] Temporary full-frame ROI applied: {}x{}; CalibROI unchanged", + resolution.nFrameWidth, resolution.nFrameHeight); + + SVzNLImageData* leftImage = nullptr; + SVzNLImageData* rightImage = nullptr; + ret = VzNL_GetEyeImage(m_deviceHandle, &leftImage, &rightImage, 3000); + bool converted = false; + if (ret == 0 && leftImage) { + converted = ConvertVzImageToSLImage(leftImage, image); + LOG_INFO("[VizumCamera] Full-frame SDK image received: size={}x{}, origin=({}, {})", + leftImage->nWidth, leftImage->nHeight, + leftImage->ptOriPos.x, leftImage->ptOriPos.y); + } else { + char errorMsg[256] = {0}; + VzNL_GetErrorInfo(ret, errorMsg); + LOG_ERROR("[VizumCamera] Full-frame image acquisition failed: {} (error code: {})", + errorMsg, ret); + } + if (leftImage) + VzNL_ReleaseImage(&leftImage); + if (rightImage) + VzNL_ReleaseImage(&rightImage); + + const bool normalized = converted + && NormalizeToDeviceFrame(image, resolution.nFrameWidth, resolution.nFrameHeight); + if (converted && !normalized) { + LOG_ERROR("[VizumCamera] SDK full-frame image cannot be placed in device frame: " + "image={}x{} origin=({}, {}), device={}x{}", + image.width, image.height, image.originX, image.originY, + resolution.nFrameWidth, resolution.nFrameHeight); + } + + const bool restored = restoreGuard.RestoreNow(); + if (!normalized || !restored) { + image.clear(); + return false; + } + + const uint64_t id = ++m_frameCounter; + frameId = "VZ_FULL_" + std::to_string(id); + LOG_INFO("[VizumCamera] Full-frame capture completed and ROI restored: frameId={}, " + "output={}x{} origin=(0, 0)", frameId, image.width, image.height); + return true; +#else + return false; +#endif +} + bool VizumCamera::CapturePointCloudWithAutoDetect(PointCloud& pointCloud, int timeoutMs) { LOG_INFO("[VizumCamera] CapturePointCloudWithAutoDetect called, timeoutMs={}", timeoutMs); diff --git a/src/VizumCamera.h b/src/VizumCamera.h index 93f8a0e..18028fd 100755 --- a/src/VizumCamera.h +++ b/src/VizumCamera.h @@ -125,9 +125,18 @@ public: * @param[out] image 输出图像 * @return 是否成功 */ - bool GetRightImage(SLImage& image); - - /** + bool GetRightImage(SLImage& image); + + /** + * @brief 采集全幅左目图像并在返回前恢复原检测 ROI + * @param[out] image 原点归一到(0,0)的设备全分辨率图像 + * @param[out] frameId 本次采集帧编号 + * @return 采集、全幅归一化及左右检测 ROI 恢复是否全部成功 + * @note 保持当前 CalibROI 状态不变；任何失败路径都会尝试恢复原左右 ROI + */ + bool CaptureFullFrameLeftImage(SLImage& image, std::string& frameId); + + /** * @brief 启用/禁用矫正 ROI 图像输出(VzNL_EnableCalibROI) * @param enable true=启用;启用后 GetLeftImage/GetRightImage 返回 SDK 矫正图 * @return 是否成功 diff --git a/src/core/StructuredLightTypes.h b/src/core/StructuredLightTypes.h index 17aeae9..55d0605 100755 --- a/src/core/StructuredLightTypes.h +++ b/src/core/StructuredLightTypes.h @@ -132,6 +132,8 @@ struct SLImage { uint32_t height = 0; int channels = 3; int bitDepth = 8; + int originX = 0; // SDK ROI image top-left in the full sensor frame + int originY = 0; size_t size() const { return data.size(); } bool empty() const { return data.empty(); } @@ -142,6 +144,8 @@ struct SLImage { data.shrink_to_fit(); width = 0; height = 0; + originX = 0; + originY = 0; } };