1897 lines
66 KiB
C++
Executable File
1897 lines
66 KiB
C++
Executable File
#include "VizumCamera.h"
|
||
#include "utils/Logger.h"
|
||
#include <iostream>
|
||
#include <cstring>
|
||
#include <chrono>
|
||
#include <thread>
|
||
#include <algorithm>
|
||
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
#include "VZNL_Common.h"
|
||
#include "VZNL_Types.h"
|
||
#include "VZNL_DetectLaser.h"
|
||
#include "VZNL_DetectConfig.h"
|
||
#include "VZNL_EyeConfig.h"
|
||
#include "VZNL_SwingMotor.h"
|
||
#endif
|
||
|
||
/**
|
||
* @file VizumCamera.cpp
|
||
* @brief 伟景智能 Vizum 结构光相机封装实现
|
||
*/
|
||
|
||
namespace StructuredLight {
|
||
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
struct VizumCameraCallbackHooks {
|
||
static void AutoOutputLaserLine(int dataType, void* laserLinePoint, VizumCamera* camera)
|
||
{
|
||
if (!camera) {
|
||
return;
|
||
}
|
||
camera->HandleAutoOutputLaserLine(dataType, laserLinePoint);
|
||
}
|
||
|
||
static void DeviceStatusChange(int status, VizumCamera* camera)
|
||
{
|
||
if (!camera) {
|
||
return;
|
||
}
|
||
camera->HandleDeviceStatusChange(status);
|
||
}
|
||
};
|
||
#endif
|
||
|
||
// 静态成员初始化
|
||
std::atomic<bool> VizumCamera::s_sdkInitialized(false);
|
||
std::mutex VizumCamera::s_sdkMutex;
|
||
|
||
struct VizumCamera::Impl {
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
VZNLHANDLE deviceHandle = nullptr;
|
||
SVzNLEyeCBInfo vzDeviceInfo{};
|
||
#else
|
||
void* deviceHandle = nullptr;
|
||
#endif
|
||
std::string ipAddress;
|
||
SLDeviceInfo deviceInfo;
|
||
std::atomic<bool> isConnected{false};
|
||
std::atomic<bool> isAsyncCapturing{false};
|
||
FrameCallback frameCallback = nullptr;
|
||
CaptureType captureType = CaptureType::All;
|
||
mutable std::mutex mutex;
|
||
mutable std::mutex deviceAccessMutex;
|
||
std::atomic<uint64_t> frameCounter{0};
|
||
|
||
struct CachedFrameData {
|
||
std::string frameId;
|
||
SLImage leftImage;
|
||
SLImage rightImage;
|
||
PointCloud pointCloud;
|
||
DepthMap depthMap;
|
||
uint64_t timestamp = 0;
|
||
} cachedFrame;
|
||
|
||
std::vector<std::vector<Point3D>> accumulatedScanLines;
|
||
std::vector<std::vector<PixelLR>> accumulatedScanLines2D; // 与扫描线一一对应的双目2D坐标
|
||
std::vector<ScanLineInfo> accumulatedLineMeta; // 每线 encodeNo/timestamp
|
||
std::mutex scanLineMutex;
|
||
std::atomic<bool> autoDetectRunning{false};
|
||
std::atomic<bool> isPaused{true};
|
||
std::atomic<int> scanCompletionStatus{0};
|
||
PointCloud accumulatedCloud;
|
||
};
|
||
|
||
#define m_deviceHandle m_impl->deviceHandle
|
||
#define m_vzDeviceInfo m_impl->vzDeviceInfo
|
||
#define m_ipAddress m_impl->ipAddress
|
||
#define m_deviceInfo m_impl->deviceInfo
|
||
#define m_isConnected m_impl->isConnected
|
||
#define m_isAsyncCapturing m_impl->isAsyncCapturing
|
||
#define m_frameCallback m_impl->frameCallback
|
||
#define m_captureType m_impl->captureType
|
||
#define m_mutex m_impl->mutex
|
||
#define m_deviceAccessMutex m_impl->deviceAccessMutex
|
||
#define m_frameCounter m_impl->frameCounter
|
||
#define m_cachedFrame m_impl->cachedFrame
|
||
#define m_accumulatedScanLines m_impl->accumulatedScanLines
|
||
#define m_accumulatedScanLines2D m_impl->accumulatedScanLines2D
|
||
#define m_accumulatedLineMeta m_impl->accumulatedLineMeta
|
||
#define m_scanLineMutex m_impl->scanLineMutex
|
||
#define m_autoDetectRunning m_impl->autoDetectRunning
|
||
#define m_isPaused m_impl->isPaused
|
||
#define m_scanCompletionStatus m_impl->scanCompletionStatus
|
||
#define m_accumulatedCloud m_impl->accumulatedCloud
|
||
|
||
#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<uint64_t>(image.originX) + image.width;
|
||
const uint64_t bottom = static_cast<uint64_t>(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<size_t>(image.channels) * bytesPerChannel;
|
||
const size_t sourceRowBytes = static_cast<size_t>(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<size_t>(frameWidth) * frameHeight * pixelBytes, 0);
|
||
|
||
const size_t targetRowBytes = static_cast<size_t>(frameWidth) * pixelBytes;
|
||
const size_t targetXBytes = static_cast<size_t>(image.originX) * pixelBytes;
|
||
for (uint32_t row = 0; row < image.height; ++row) {
|
||
const size_t sourceOffset = static_cast<size_t>(row) * sourceRowBytes;
|
||
const size_t targetOffset = (static_cast<size_t>(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<EVzDeviceWorkStatus>(status)) {
|
||
case keDeviceWorkStatusUnknown: return "Unknown";
|
||
case keDeviceWorkStatus_Eye_Comming: return "Eye_Comming";
|
||
case keDeviceWorkStatus_Eye_Reconnect: return "Eye_Reconnect";
|
||
case keDeviceWorkStatus_Working: return "Working";
|
||
case keDeviceWorkStatus_Offline: return "Offline";
|
||
case keDeviceWorkStatus_Device_Swing_Finish: return "Device_Swing_Finish";
|
||
case keDeviceWorkStatus_Device_Auto_Stop: return "Device_Auto_Stop";
|
||
case keDeviceWorkStatus_Device_Swing_Running: return "Device_Swing_Running";
|
||
default: return "Other";
|
||
}
|
||
}
|
||
|
||
void AutoOutputLaserLineExCB(EVzResultDataType eDataType,
|
||
SVzLaserLineData* pLaserLinePoint,
|
||
void* pParam)
|
||
{
|
||
VizumCameraCallbackHooks::AutoOutputLaserLine(
|
||
static_cast<int>(eDataType), pLaserLinePoint, static_cast<VizumCamera*>(pParam));
|
||
}
|
||
|
||
void DeviceStatusCB(EVzDeviceWorkStatus eStatus, void* pParam)
|
||
{
|
||
VizumCameraCallbackHooks::DeviceStatusChange(
|
||
static_cast<int>(eStatus), static_cast<VizumCamera*>(pParam));
|
||
}
|
||
|
||
} // namespace
|
||
#endif
|
||
|
||
void VizumCamera::HandleAutoOutputLaserLine(int dataType, void* laserLinePoint)
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
static bool firstCallback = true;
|
||
if (firstCallback) {
|
||
firstCallback = false;
|
||
LOG_INFO("[VizumCamera] Received first callback, eDataType={}", dataType);
|
||
}
|
||
|
||
if (dataType == static_cast<int>(keResultDataType_PointXYZRGBA)) {
|
||
ConvertLaserLineDataToPointCloud(laserLinePoint);
|
||
} else {
|
||
LOG_WARN("[VizumCamera] Callback received with unexpected data type: {}", dataType);
|
||
}
|
||
#else
|
||
(void)dataType;
|
||
(void)laserLinePoint;
|
||
#endif
|
||
}
|
||
|
||
void VizumCamera::HandleDeviceStatusChange(int status)
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
const char* statusName = DeviceStatusName(status);
|
||
LOG_INFO("[VizumCamera] Device status changed: {} ({})", statusName, status);
|
||
|
||
const bool completesScan =
|
||
status == static_cast<int>(keDeviceWorkStatus_Device_Swing_Finish)
|
||
|| status == static_cast<int>(keDeviceWorkStatus_Device_Auto_Stop);
|
||
if (completesScan) {
|
||
if (!m_autoDetectRunning.load()) {
|
||
LOG_INFO("[VizumCamera] Completion status {} ({}) received outside an active "
|
||
"scan wait; ignored",
|
||
statusName, status);
|
||
return;
|
||
}
|
||
size_t scanLines = 0;
|
||
size_t validPoints = 0;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanLineMutex);
|
||
scanLines = m_accumulatedScanLines.size();
|
||
validPoints = m_accumulatedCloud.points.size();
|
||
}
|
||
m_scanCompletionStatus = status;
|
||
LOG_INFO("[VizumCamera] Scan completion status received: {} ({}); "
|
||
"releasing scan wait, scan lines={}, valid points={}",
|
||
statusName, status, scanLines, validPoints);
|
||
m_isPaused = true;
|
||
return;
|
||
}
|
||
|
||
if (status == static_cast<int>(keDeviceWorkStatus_Offline)) {
|
||
LOG_WARN("[VizumCamera] Camera went offline");
|
||
} else if (status == static_cast<int>(keDeviceWorkStatus_Device_Swing_Running)) {
|
||
LOG_INFO("[VizumCamera] Camera swing motor started running");
|
||
} else if (status == static_cast<int>(keDeviceWorkStatus_Eye_Reconnect)) {
|
||
LOG_INFO("[VizumCamera] Camera reconnected");
|
||
}
|
||
#else
|
||
(void)status;
|
||
#endif
|
||
}
|
||
|
||
void VizumCamera::ConvertLaserLineDataToPointCloud(void* laserLinePoint)
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
SVzLaserLineData* pLaserLinePoint = static_cast<SVzLaserLineData*>(laserLinePoint);
|
||
if (pLaserLinePoint == nullptr) {
|
||
return;
|
||
}
|
||
|
||
int nPoints = pLaserLinePoint->nPointCount;
|
||
|
||
// 检查点数量和指针有效性
|
||
if (nPoints <= 0 || pLaserLinePoint->p3DPoint == nullptr) {
|
||
return;
|
||
}
|
||
|
||
// 将 p3DPoint 转换为 SVzNLPointXYZRGBA* 类型
|
||
SVzNLPointXYZRGBA* pPoints = static_cast<SVzNLPointXYZRGBA*>(pLaserLinePoint->p3DPoint);
|
||
|
||
// keResultDataType_PointXYZRGBA 对应的 2D 数据类型为 SVzNL2DLRPoint(左右目像素坐标)
|
||
SVzNL2DLRPoint* pPoints2D = static_cast<SVzNL2DLRPoint*>(pLaserLinePoint->p2DPoint);
|
||
|
||
// 使用锁保护对累积点云的访问
|
||
std::lock_guard<std::mutex> lock(m_scanLineMutex);
|
||
|
||
// 预先分配内存
|
||
m_accumulatedCloud.points.reserve(m_accumulatedCloud.points.size() + nPoints);
|
||
|
||
// 同时保存扫描线数据
|
||
std::vector<Point3D> scanLine;
|
||
scanLine.reserve(nPoints);
|
||
std::vector<PixelLR> scanLine2D;
|
||
scanLine2D.reserve(nPoints);
|
||
|
||
int validCount = 0;
|
||
for (int i = 0; i < nPoints; ++i) {
|
||
SVzNLPointXYZRGBA& point = pPoints[i];
|
||
|
||
// 只将有效点添加到显示用的点云
|
||
if (point.x != 0.0f || point.y != 0.0f || point.z != 0.0f) {
|
||
Point3D pt;
|
||
pt.x = point.x;
|
||
pt.y = point.y;
|
||
pt.z = point.z;
|
||
m_accumulatedCloud.points.push_back(pt);
|
||
validCount++;
|
||
}
|
||
|
||
// 保存所有点到扫描线数据（包括无效点），保持网格格式
|
||
Point3D scanPt;
|
||
scanPt.x = point.x;
|
||
scanPt.y = point.y;
|
||
scanPt.z = point.z;
|
||
scanLine.push_back(scanPt);
|
||
|
||
// 同步保存双目 2D 像素坐标(无 2D 数据时补零占位,保持与 3D 点对齐)
|
||
PixelLR px;
|
||
if (pPoints2D != nullptr) {
|
||
px.leftX = pPoints2D[i].sLeft.x;
|
||
px.leftY = pPoints2D[i].sLeft.y;
|
||
px.rightX = pPoints2D[i].sRight.x;
|
||
px.rightY = pPoints2D[i].sRight.y;
|
||
}
|
||
scanLine2D.push_back(px);
|
||
}
|
||
|
||
// 逐线统计只留 TRACE(SPDLOG_ACTIVE_LEVEL=DEBUG 下编译期剔除,不再刷爆日志);
|
||
// 每次扫描的汇总信息由采集结束处的 "Organized grid built" 一条日志给出
|
||
LOG_TRACE("[VizumCamera] 收到扫描线: {} 个点, 有效点: {}", nPoints, validCount);
|
||
|
||
// 将扫描线添加到累积的扫描线数据中
|
||
if (!scanLine.empty()) {
|
||
m_accumulatedScanLines.push_back(std::move(scanLine));
|
||
m_accumulatedScanLines2D.push_back(std::move(scanLine2D));
|
||
ScanLineInfo meta;
|
||
meta.pointCount = (size_t)nPoints;
|
||
meta.timestamp = pLaserLinePoint->llTimeStamp;
|
||
meta.encodeNo = pLaserLinePoint->nEncodeNo;
|
||
m_accumulatedLineMeta.push_back(meta);
|
||
LOG_TRACE("[VizumCamera] 累积扫描线数量: {}", m_accumulatedScanLines.size());
|
||
}
|
||
#else
|
||
(void)laserLinePoint;
|
||
#endif
|
||
}
|
||
|
||
// 获取累积的扫描线数据
|
||
std::vector<std::vector<Point3D>> VizumCamera::GetAccumulatedScanLines()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanLineMutex);
|
||
return m_accumulatedScanLines;
|
||
}
|
||
|
||
// 清除累积的扫描线数据
|
||
void VizumCamera::ClearAccumulatedScanLines()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanLineMutex);
|
||
m_accumulatedScanLines.clear();
|
||
m_accumulatedScanLines2D.clear();
|
||
m_accumulatedLineMeta.clear();
|
||
m_accumulatedCloud.clear();
|
||
}
|
||
|
||
// ============================================================================
|
||
// 构造与析构
|
||
// ============================================================================
|
||
|
||
VizumCamera::VizumCamera()
|
||
: m_impl(std::make_unique<Impl>())
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
memset(&m_vzDeviceInfo, 0, sizeof(m_vzDeviceInfo));
|
||
#endif
|
||
}
|
||
|
||
VizumCamera::~VizumCamera()
|
||
{
|
||
Disconnect();
|
||
}
|
||
|
||
// ============================================================================
|
||
// SDK初始化/销毁
|
||
// ============================================================================
|
||
|
||
bool VizumCamera::InitSDK()
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
std::lock_guard<std::mutex> lock(s_sdkMutex);
|
||
|
||
if (s_sdkInitialized) {
|
||
return true;
|
||
}
|
||
|
||
// 与VznlSDKManager::InitializeSDK保持一致
|
||
SVzNLConfigParam configParam;
|
||
configParam.nDeviceTimeOut = 0;
|
||
|
||
int result = VzNL_Init(&configParam);
|
||
if (result != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(result, errorMsg);
|
||
LOG_ERROR("[VizumCamera] SDK initialization failed: {}", errorMsg);
|
||
return false;
|
||
}
|
||
|
||
// SDK 日志:只记警告及以上、只写文件(VizumLog/)
|
||
// Information 级会逐帧刷屏,现场排查需要更详细时临时调回
|
||
VzNL_SetLogLevel(keNLLogLevel_Warning, keNLLogType_File);
|
||
|
||
s_sdkInitialized = true;
|
||
LOG_INFO("[VizumCamera] SDK initialized successfully");
|
||
return true;
|
||
#else
|
||
LOG_ERROR("[VizumCamera] Vizum camera support not enabled");
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
void VizumCamera::DestroySDK()
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
std::lock_guard<std::mutex> lock(s_sdkMutex);
|
||
|
||
if (s_sdkInitialized) {
|
||
VzNL_Destroy();
|
||
s_sdkInitialized = false;
|
||
LOG_INFO("[VizumCamera] SDK destroyed");
|
||
}
|
||
#endif
|
||
}
|
||
|
||
// ============================================================================
|
||
// 基本操作实现
|
||
// ============================================================================
|
||
|
||
std::string VizumCamera::GetSDKVersion() const
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
if (m_deviceHandle) {
|
||
SVzNLVersionInfo versionInfo;
|
||
memset(&versionInfo, 0, sizeof(versionInfo));
|
||
if (VzNL_GetVersion(m_deviceHandle, &versionInfo) == 0) {
|
||
return std::string(versionInfo.szSDKVersion);
|
||
}
|
||
}
|
||
return "Unknown";
|
||
#else
|
||
return "Vizum SDK not enabled";
|
||
#endif
|
||
}
|
||
|
||
bool VizumCamera::SearchCameras(std::vector<SLDeviceInfo>& deviceList)
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
deviceList.clear();
|
||
|
||
// 确保SDK已初始化
|
||
if (!s_sdkInitialized) {
|
||
if (!InitSDK()) {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// 搜索设备 - 使用与成功实现相同的标志
|
||
int result = VzNL_ResearchDevice(keSearchDeviceFlag_EthLaserRobotEye);
|
||
if (result != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(result, errorMsg);
|
||
LOG_ERROR("[VizumCamera] Device search failed: {}", errorMsg);
|
||
return false;
|
||
}
|
||
|
||
// 获取设备数量
|
||
int deviceCount = 0;
|
||
result = VzNL_GetEyeCBDeviceInfo(nullptr, &deviceCount);
|
||
|
||
if (deviceCount <= 0) {
|
||
LOG_INFO("[VizumCamera] No devices found");
|
||
return false;
|
||
}
|
||
|
||
// 获取设备信息
|
||
std::vector<SVzNLEyeCBInfo> vzDevices(deviceCount);
|
||
result = VzNL_GetEyeCBDeviceInfo(vzDevices.data(), &deviceCount);
|
||
if (result != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(result, errorMsg);
|
||
LOG_ERROR("[VizumCamera] Failed to get device info: {}", errorMsg);
|
||
return false;
|
||
}
|
||
|
||
// 转换为通用设备信息，并绑定未绑定的设备
|
||
for (int i = 0; i < deviceCount; ++i) {
|
||
// 检查设备是否有效，如果无效则尝试绑定（与成功实现一致）
|
||
if (vzDevices[i].bValidDevice == VzFalse) {
|
||
LOG_INFO("[VizumCamera] Device {} is not bound, attempting to bind...",
|
||
std::string(vzDevices[i].byServerIP));
|
||
|
||
// 获取设备配置信息
|
||
SVzNLEthernetEyeConfigInfo ethConfigInfo;
|
||
int configResult = VzNL_GetEthernetEyeConfigInfo(&vzDevices[i], ðConfigInfo);
|
||
if (configResult != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(configResult, errorMsg);
|
||
LOG_WARN("[VizumCamera] Failed to get ethernet eye config: {}", errorMsg);
|
||
}
|
||
|
||
// 绑定设备
|
||
int bindResult = VzNL_BindEthernetEye(&vzDevices[i]);
|
||
if (bindResult != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(bindResult, errorMsg);
|
||
LOG_WARN("[VizumCamera] Failed to bind ethernet eye: {}", errorMsg);
|
||
continue; // 绑定失败，跳过此设备
|
||
}
|
||
LOG_INFO("[VizumCamera] Device bound successfully");
|
||
}
|
||
|
||
SLDeviceInfo info;
|
||
info.ipAddress = std::string(vzDevices[i].byServerIP);
|
||
info.serialNumber = std::string(vzDevices[i].szGUID);
|
||
info.model = std::string(vzDevices[i].szDeviceName);
|
||
info.alias = std::string(vzDevices[i].szDesc);
|
||
// 宽高暂时设置为默认值，连接后会更新
|
||
info.width = 1280;
|
||
info.height = 960;
|
||
deviceList.push_back(info);
|
||
}
|
||
|
||
LOG_INFO("[VizumCamera] Found {} device(s)", deviceCount);
|
||
return !deviceList.empty();
|
||
#else
|
||
LOG_ERROR("[VizumCamera] Vizum camera support not enabled");
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
bool VizumCamera::Connect(const std::string& ipAddress)
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
std::unique_lock<std::mutex> lock(m_mutex);
|
||
|
||
if (m_isConnected) {
|
||
if (m_ipAddress == ipAddress) {
|
||
return true; // 已连接到相同地址
|
||
}
|
||
lock.unlock();
|
||
Disconnect(); // 断开之前的连接
|
||
lock.lock();
|
||
}
|
||
|
||
// 确保SDK已初始化
|
||
if (!s_sdkInitialized) {
|
||
if (!InitSDK()) {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
m_ipAddress = ipAddress;
|
||
|
||
// 搜索设备 - 使用与成功实现相同的标志
|
||
int result = VzNL_ResearchDevice(keSearchDeviceFlag_EthLaserRobotEye);
|
||
if (result != 0) {
|
||
LOG_ERROR("[VizumCamera] Device search failed during connect");
|
||
m_ipAddress.clear();
|
||
return false;
|
||
}
|
||
|
||
// 获取设备列表
|
||
int deviceCount = 0;
|
||
VzNL_GetEyeCBDeviceInfo(nullptr, &deviceCount);
|
||
|
||
if (deviceCount <= 0) {
|
||
LOG_ERROR("[VizumCamera] No devices found");
|
||
m_ipAddress.clear();
|
||
return false;
|
||
}
|
||
|
||
std::vector<SVzNLEyeCBInfo> vzDevices(deviceCount);
|
||
VzNL_GetEyeCBDeviceInfo(vzDevices.data(), &deviceCount);
|
||
|
||
// 查找匹配的设备
|
||
bool found = false;
|
||
for (int i = 0; i < deviceCount; ++i) {
|
||
if (std::string(vzDevices[i].byServerIP) == ipAddress) {
|
||
m_vzDeviceInfo = vzDevices[i];
|
||
found = true;
|
||
|
||
// 检查设备是否有效，如果无效则尝试绑定（与成功实现一致）
|
||
if (m_vzDeviceInfo.bValidDevice == VzFalse) {
|
||
LOG_INFO("[VizumCamera] Device is not bound, attempting to bind...");
|
||
|
||
// 获取设备配置信息
|
||
SVzNLEthernetEyeConfigInfo ethConfigInfo;
|
||
int configResult = VzNL_GetEthernetEyeConfigInfo(&m_vzDeviceInfo, ðConfigInfo);
|
||
if (configResult != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(configResult, errorMsg);
|
||
LOG_WARN("[VizumCamera] Failed to get ethernet eye config: {}", errorMsg);
|
||
}
|
||
|
||
// 绑定设备
|
||
int bindResult = VzNL_BindEthernetEye(&m_vzDeviceInfo);
|
||
if (bindResult != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(bindResult, errorMsg);
|
||
LOG_ERROR("[VizumCamera] Failed to bind ethernet eye: {}", errorMsg);
|
||
m_ipAddress.clear();
|
||
return false;
|
||
}
|
||
LOG_INFO("[VizumCamera] Device bound successfully");
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (!found) {
|
||
LOG_ERROR("[VizumCamera] Device with IP {} not found", ipAddress);
|
||
m_ipAddress.clear();
|
||
return false;
|
||
}
|
||
|
||
// 打开设备（与VznlCamera::OpenCamera保持一致，不设置额外参数）
|
||
SVzNLOpenDeviceParam openParam{};
|
||
// 注意：正确示例中不设置任何参数，使用默认值
|
||
|
||
int errorCode = 0;
|
||
m_deviceHandle = VzNL_OpenDevice(&m_vzDeviceInfo, &openParam, &errorCode);
|
||
|
||
if (!m_deviceHandle || errorCode != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(errorCode, errorMsg);
|
||
LOG_ERROR("[VizumCamera] Failed to open device: {}", errorMsg);
|
||
m_ipAddress.clear();
|
||
return false;
|
||
}
|
||
|
||
// 设置设备状态回调。静态扫描以 Device_Auto_Stop 作为官方完成信号，注册失败不可静默继续。
|
||
const int statusNotifyResult = VzNL_SetDeviceStatusNotify(m_deviceHandle, &DeviceStatusCB, this);
|
||
if (statusNotifyResult != 0) {
|
||
char statusErrorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(statusNotifyResult, statusErrorMsg);
|
||
LOG_ERROR("[VizumCamera] Failed to register device status callback: {} (error code: {})",
|
||
statusErrorMsg, statusNotifyResult);
|
||
VzNL_CloseDevice(m_deviceHandle);
|
||
m_deviceHandle = nullptr;
|
||
m_ipAddress.clear();
|
||
return false;
|
||
}
|
||
LOG_INFO("[VizumCamera] Device status callback registered; scan completion events: "
|
||
"Device_Auto_Stop or Device_Swing_Finish");
|
||
|
||
// OpenDevice 之后立即设置图像输出格式（在此之前设置对本次连接无效）
|
||
VzNL_SetOutputImageFormat(keVzNLImageType_BGR888);
|
||
LOG_INFO("[VizumCamera] Output image format set to BGR888");
|
||
|
||
// 更新设备信息
|
||
if (!UpdateDeviceInfo()) {
|
||
VzNL_CloseDevice(m_deviceHandle);
|
||
m_deviceHandle = nullptr;
|
||
m_ipAddress.clear();
|
||
return false;
|
||
}
|
||
|
||
// 初始化激光检测（非致命，与 HandEyeKit 保持一致）
|
||
int laserResult = VzNL_BeginDetectLaser(m_deviceHandle);
|
||
if (laserResult != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(laserResult, errorMsg);
|
||
LOG_WARN("[VizumCamera] BeginDetectLaser failed (non-fatal): {}", errorMsg);
|
||
} else {
|
||
LOG_INFO("[VizumCamera] Laser detection initialized");
|
||
}
|
||
|
||
// 切到 Master 主动模式 — 避免设备停留在外触发模式导致无法取图/设参数（非致命）
|
||
{
|
||
const int trigResult = VzNL_SetTriggerMode(m_deviceHandle, keEyeTriggerMode_Master);
|
||
if (trigResult != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(trigResult, errorMsg);
|
||
LOG_WARN("[VizumCamera] SetTriggerMode(Master) failed (non-fatal): {}", errorMsg);
|
||
} else {
|
||
LOG_INFO("[VizumCamera] Trigger mode set to Master");
|
||
}
|
||
}
|
||
|
||
// 忽略外部触发使能信号（非致命）
|
||
{
|
||
const int ignResult = VzNL_IgnoreTriggerExtSignal(m_deviceHandle, VzTrue);
|
||
if (ignResult != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(ignResult, errorMsg);
|
||
LOG_WARN("[VizumCamera] IgnoreTriggerExtSignal failed (non-fatal): {}", errorMsg);
|
||
} else {
|
||
LOG_INFO("[VizumCamera] External trigger signal ignored");
|
||
}
|
||
}
|
||
|
||
m_isConnected = true;
|
||
LOG_INFO("[VizumCamera] Connected to camera at {}", ipAddress);
|
||
return true;
|
||
#else
|
||
LOG_ERROR("[VizumCamera] Vizum camera support not enabled");
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
void VizumCamera::Disconnect()
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
|
||
|
||
if (m_isAsyncCapturing) {
|
||
StopAsyncCapture();
|
||
}
|
||
|
||
if (m_deviceHandle) {
|
||
// 结束激光检测（与VZNLCameraAdapter保持一致）
|
||
VzNL_EndDetectLaser(m_deviceHandle);
|
||
VzNL_StopCapture(m_deviceHandle);
|
||
VzNL_CloseDevice(m_deviceHandle);
|
||
m_deviceHandle = nullptr;
|
||
}
|
||
if (s_sdkInitialized) {
|
||
VzNL_Destroy();
|
||
s_sdkInitialized = false;
|
||
}
|
||
|
||
m_isConnected = false;
|
||
m_ipAddress.clear();
|
||
m_deviceInfo = SLDeviceInfo();
|
||
memset(&m_vzDeviceInfo, 0, sizeof(m_vzDeviceInfo));
|
||
|
||
LOG_INFO("[VizumCamera] Disconnected");
|
||
#endif
|
||
}
|
||
|
||
bool VizumCamera::IsConnected() const
|
||
{
|
||
return m_isConnected;
|
||
}
|
||
|
||
bool VizumCamera::GetDeviceInfo(SLDeviceInfo& info)
|
||
{
|
||
if (!m_isConnected) {
|
||
return false;
|
||
}
|
||
|
||
info = m_deviceInfo;
|
||
return true;
|
||
}
|
||
|
||
bool VizumCamera::UpdateDeviceInfo()
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
if (!m_deviceHandle) {
|
||
return false;
|
||
}
|
||
|
||
// 获取分辨率
|
||
SVzVideoResolution resolution;
|
||
if (VzNL_GetResolution(m_deviceHandle, &resolution) == 0) {
|
||
m_deviceInfo.width = resolution.nFrameWidth;
|
||
m_deviceInfo.height = resolution.nFrameHeight;
|
||
} else {
|
||
m_deviceInfo.width = 1280;
|
||
m_deviceInfo.height = 960;
|
||
}
|
||
|
||
m_deviceInfo.ipAddress = m_ipAddress;
|
||
m_deviceInfo.serialNumber = std::string(m_vzDeviceInfo.szGUID);
|
||
m_deviceInfo.model = std::string(m_vzDeviceInfo.szDeviceName);
|
||
m_deviceInfo.alias = std::string(m_vzDeviceInfo.szDesc);
|
||
|
||
return true;
|
||
#else
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
// ============================================================================
|
||
// 同步采集实现
|
||
// ============================================================================
|
||
|
||
bool VizumCamera::TriggerCapture(std::string& frameId, CaptureType captureType)
|
||
{
|
||
LOG_INFO("[VizumCamera] TriggerCapture called, captureType={}", static_cast<int>(captureType));
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
if (!m_isConnected || !m_deviceHandle) {
|
||
LOG_ERROR("[VizumCamera] TriggerCapture failed: Camera not connected or handle invalid");
|
||
return false;
|
||
}
|
||
|
||
// 生成帧ID
|
||
m_frameCounter++;
|
||
frameId = "VZ_" + std::to_string(m_frameCounter);
|
||
|
||
// 执行采集并缓存数据
|
||
if (!CaptureAndCacheData(captureType)) {
|
||
LOG_ERROR("[VizumCamera] CaptureAndCacheData failed");
|
||
return false;
|
||
}
|
||
|
||
m_cachedFrame.frameId = frameId;
|
||
LOG_INFO("[VizumCamera] TriggerCapture success, frameId={}", frameId);
|
||
return true;
|
||
#else
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
bool VizumCamera::CaptureAndCacheData(CaptureType captureType)
|
||
{
|
||
LOG_INFO("[VizumCamera] CaptureAndCacheData called, captureType={}", static_cast<int>(captureType));
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
|
||
if (!m_deviceHandle) {
|
||
LOG_ERROR("[VizumCamera] CaptureAndCacheData failed: Device handle is null");
|
||
return false;
|
||
}
|
||
|
||
// 清空缓存
|
||
m_cachedFrame.leftImage.clear();
|
||
m_cachedFrame.rightImage.clear();
|
||
m_cachedFrame.pointCloud.clear();
|
||
m_cachedFrame.depthMap.clear();
|
||
|
||
// 获取当前时间戳
|
||
auto now = std::chrono::system_clock::now();
|
||
m_cachedFrame.timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
now.time_since_epoch()).count();
|
||
|
||
// 采集图像
|
||
if (captureType & CaptureType::Image) {
|
||
SVzNLImageData* pLeftImage = nullptr;
|
||
SVzNLImageData* pRightImage = nullptr;
|
||
|
||
int result = VzNL_GetEyeImage(m_deviceHandle, &pLeftImage, &pRightImage, 3000);
|
||
if (result == 0) {
|
||
if (pLeftImage) {
|
||
ConvertVzImageToSLImage(pLeftImage, m_cachedFrame.leftImage);
|
||
VzNL_ReleaseImage(&pLeftImage);
|
||
}
|
||
if (pRightImage) {
|
||
ConvertVzImageToSLImage(pRightImage, m_cachedFrame.rightImage);
|
||
VzNL_ReleaseImage(&pRightImage);
|
||
}
|
||
} else {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(result, errorMsg);
|
||
LOG_ERROR("[VizumCamera] Failed to get image: {}", errorMsg);
|
||
}
|
||
}
|
||
|
||
// 采集点云（通过激光扫描）
|
||
// 按照 VZNLCameraAdapter::scan 的逻辑，使用自动检测模式采集
|
||
if (captureType & CaptureType::PointCloud) {
|
||
LOG_INFO("[VizumCamera] CaptureAndCacheData: Requesting point cloud via CapturePointCloudWithAutoDetect...");
|
||
|
||
PointCloud pc;
|
||
// 使用默认超时或根据需要调整
|
||
if (CapturePointCloudWithAutoDetect(pc, 3000)) {
|
||
m_cachedFrame.pointCloud = std::move(pc);
|
||
LOG_INFO("[VizumCamera] CaptureAndCacheData: Successfully captured {} points", m_cachedFrame.pointCloud.points.size());
|
||
} else {
|
||
LOG_WARN("[VizumCamera] CaptureAndCacheData: Failed to capture point cloud");
|
||
}
|
||
}
|
||
|
||
return true;
|
||
#else
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
bool VizumCamera::ConvertVzImageToSLImage(void* vzImagePtr, SLImage& slImage)
|
||
{
|
||
SVzNLImageData* vzImage = static_cast<SVzNLImageData*>(vzImagePtr);
|
||
if (!vzImage || !vzImage->pBuffer) {
|
||
return false;
|
||
}
|
||
|
||
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;
|
||
|
||
size_t dataSize = vzImage->nWidth * vzImage->nHeight * vzImage->nChannels;
|
||
if (vzImage->byBitDepth == 16) {
|
||
dataSize *= 2;
|
||
}
|
||
|
||
slImage.data.resize(dataSize);
|
||
memcpy(slImage.data.data(), vzImage->pBuffer, dataSize);
|
||
|
||
return true;
|
||
}
|
||
#endif
|
||
|
||
bool VizumCamera::GetImage(const std::string& frameId, SLImage& image)
|
||
{
|
||
if (m_cachedFrame.frameId != frameId) {
|
||
return false;
|
||
}
|
||
|
||
// 返回左目图像作为主图像
|
||
image = m_cachedFrame.leftImage;
|
||
return !image.empty();
|
||
}
|
||
|
||
bool VizumCamera::GetLeftImage(SLImage& image)
|
||
{
|
||
image = m_cachedFrame.leftImage;
|
||
return !image.empty();
|
||
}
|
||
|
||
bool VizumCamera::GetRightImage(SLImage& image)
|
||
{
|
||
image = m_cachedFrame.rightImage;
|
||
return !image.empty();
|
||
}
|
||
|
||
bool VizumCamera::CaptureFullFrameLeftImage(SLImage& image, std::string& frameId)
|
||
{
|
||
image.clear();
|
||
frameId.clear();
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
const std::lock_guard<std::mutex> 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<int>(resolution.nFrameWidth),
|
||
0, static_cast<int>(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);
|
||
LOG_INFO("[VizumCamera] Input pointCloud state: scanLines.size()={}, points.size()={}",
|
||
pointCloud.scanLines.size(), pointCloud.points.size());
|
||
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
LOG_INFO("[VizumCamera] ENABLE_VIZUM_CAMERA is defined");
|
||
LOG_INFO("[VizumCamera] m_isConnected={}, m_deviceHandle={}", m_isConnected, (m_deviceHandle ? "valid" : "null"));
|
||
|
||
if (!m_isConnected || !m_deviceHandle) {
|
||
LOG_ERROR("[VizumCamera] Camera not connected, returning false");
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("[VizumCamera] ========================================");
|
||
LOG_INFO("[VizumCamera] Starting point cloud capture (AutoDetect mode)...");
|
||
LOG_INFO("[VizumCamera] ========================================");
|
||
|
||
int result = 0;
|
||
char errorMsg[256] = {0};
|
||
|
||
// 0. 检查并配置摆镜扫描（关键步骤！）
|
||
LOG_INFO("[VizumCamera] Step 0: Checking and configuring swing motor...");
|
||
|
||
// 检查是否支持摆镜
|
||
int errorCode = 0;
|
||
VzBool supportSwing = VzNL_IsSupportSwingMotor(m_deviceHandle, &errorCode);
|
||
if (supportSwing == VzTrue) {
|
||
LOG_INFO("[VizumCamera] Swing motor is supported");
|
||
|
||
// 检查摆镜是否已启用
|
||
VzBool swingEnabled = VzNL_IsEnableSwingMotor(m_deviceHandle, &errorCode);
|
||
if (swingEnabled != VzTrue) {
|
||
// 启用摆镜
|
||
LOG_INFO("[VizumCamera] Enabling swing motor...");
|
||
result = VzNL_EnableSwingMotor(m_deviceHandle, VzTrue);
|
||
if (result != 0) {
|
||
VzNL_GetErrorInfo(result, errorMsg);
|
||
LOG_WARN("[VizumCamera] Failed to enable swing motor: {} (error code: {})", errorMsg, result);
|
||
// 继续尝试，可能摆镜已经手动启用
|
||
} else {
|
||
LOG_INFO("[VizumCamera] Swing motor enabled successfully");
|
||
}
|
||
} else {
|
||
LOG_INFO("[VizumCamera] Swing motor is already enabled");
|
||
}
|
||
|
||
// 获取并记录摆镜配置信息
|
||
int startAngle = 0, endAngle = 0;
|
||
if (VzNL_GetSwingMotorAngle(m_deviceHandle, &startAngle, &endAngle) == 0) {
|
||
LOG_INFO("[VizumCamera] Swing motor angle range: {} to {}", startAngle, endAngle);
|
||
}
|
||
|
||
int angleSpeed = 0;
|
||
if (VzNL_GetSwingAngleSpeed(m_deviceHandle, &angleSpeed) == 0) {
|
||
LOG_INFO("[VizumCamera] Swing motor angle speed: {}", angleSpeed);
|
||
}
|
||
|
||
// 获取当前角度
|
||
float curAngle = VzNL_GetSwingCurAngle(m_deviceHandle, &errorCode);
|
||
if (errorCode == 0) {
|
||
LOG_INFO("[VizumCamera] Current swing angle: {:.2f}", curAngle);
|
||
}
|
||
} else {
|
||
LOG_WARN("[VizumCamera] Swing motor is NOT supported, point cloud may not be captured correctly");
|
||
}
|
||
|
||
// 设置点云处理模式为固定步长模式（摆镜扫描推荐）
|
||
// 注意：参考实现中没有设置此模式，如果遇到问题可以尝试注释掉
|
||
/*
|
||
LOG_INFO("[VizumCamera] Setting point cloud processing mode to FixedStep...");
|
||
result = VzNL_SetPointCloudProcMode(m_deviceHandle, kePointCloudProcMode_FixedStep);
|
||
if (result != 0) {
|
||
VzNL_GetErrorInfo(result, errorMsg);
|
||
LOG_WARN("[VizumCamera] Failed to set point cloud proc mode: {} (trying Speed mode...)", errorMsg);
|
||
// 尝试速度模式作为备选
|
||
result = VzNL_SetPointCloudProcMode(m_deviceHandle, kePointCloudProcMode_Speed);
|
||
if (result == 0) {
|
||
LOG_INFO("[VizumCamera] Set point cloud proc mode to Speed successfully");
|
||
}
|
||
} else {
|
||
LOG_INFO("[VizumCamera] Set point cloud proc mode to FixedStep successfully");
|
||
}
|
||
*/
|
||
|
||
// 获取当前点云处理模式
|
||
EVzPointCloudProcMode procMode = kePointCloudProcMode_Invalid;
|
||
if (VzNL_GetPointCloudProcMode(m_deviceHandle, &procMode) == 0) {
|
||
const char* modeStr = "Unknown";
|
||
switch(procMode) {
|
||
case kePointCloudProcMode_Speed: modeStr = "Speed"; break;
|
||
case kePointCloudProcMode_Encoder: modeStr = "Encoder"; break;
|
||
case kePointCloudProcMode_FixedStep: modeStr = "FixedStep"; break;
|
||
default: break;
|
||
}
|
||
LOG_INFO("[VizumCamera] Current point cloud proc mode: {}", modeStr);
|
||
}
|
||
|
||
// 1. 清空累积的点云和扫描线数据（与VZNLCameraAdapter::scan一致）
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanLineMutex);
|
||
m_accumulatedScanLines.clear();
|
||
m_accumulatedScanLines2D.clear();
|
||
m_accumulatedLineMeta.clear();
|
||
m_accumulatedCloud.clear();
|
||
}
|
||
LOG_INFO("[VizumCamera] Step 1: Cleared accumulated data");
|
||
|
||
// 2. 重新初始化激光检测（关键：参考示例代码，每次扫描前重新初始化）
|
||
// 示例代码中 StopAutoDetect 会调用 EndDetectLaser，所以这里需要 BeginDetectLaser
|
||
LOG_INFO("[VizumCamera] Step 1.5: Re-initializing laser detection...");
|
||
VzNL_EndDetectLaser(m_deviceHandle); // 确保之前的已结束
|
||
|
||
result = VzNL_BeginDetectLaser(m_deviceHandle);
|
||
if (result != 0) {
|
||
VzNL_GetErrorInfo(result, errorMsg);
|
||
LOG_WARN("[VizumCamera] Failed to begin detect laser: {}", errorMsg);
|
||
// 如果失败，可能是因为已经处于检测状态，尝试继续
|
||
} else {
|
||
LOG_INFO("[VizumCamera] Laser detection initialized successfully");
|
||
}
|
||
|
||
// 2.5 启用激光点填充:SDK 对每条激光线输出定长点序列,无测量处填充占位点,
|
||
// 使扫描线构成真正列对齐的 W×H 栅格(供组织化点云保存/栅格聚类使用)
|
||
result = VzNL_EnableFillLaserPoint(m_deviceHandle, VzTrue);
|
||
if (result != 0) {
|
||
VzNL_GetErrorInfo(result, errorMsg);
|
||
LOG_WARN("[VizumCamera] VzNL_EnableFillLaserPoint failed: {} (error code: {})", errorMsg, result);
|
||
} else {
|
||
LOG_INFO("[VizumCamera] Fill laser point mode enabled (rasterized scan lines)");
|
||
}
|
||
|
||
// 3. 启动自动检测模式（与VznlCamera::StartAutoDetect一致）
|
||
m_scanCompletionStatus = 0;
|
||
m_isPaused = false;
|
||
m_autoDetectRunning = true;
|
||
LOG_INFO("[VizumCamera] Step 2: Starting AutoDetectEx...");
|
||
result = VzNL_StartAutoDetectEx(m_deviceHandle, keResultDataType_PointXYZRGBA,
|
||
keFlipType_None,
|
||
&AutoOutputLaserLineExCB, this);
|
||
if (result != 0) {
|
||
m_autoDetectRunning = false;
|
||
VzNL_GetErrorInfo(result, errorMsg);
|
||
LOG_ERROR("[VizumCamera] Failed to start auto detect: {} (error code: {})", errorMsg, result);
|
||
// 尝试清理
|
||
VzNL_EndDetectLaser(m_deviceHandle);
|
||
return false;
|
||
}
|
||
LOG_INFO("[VizumCamera] AutoDetectEx started successfully");
|
||
|
||
// 稍微等待一下，确保状态更新
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||
|
||
// 检查自动检测状态
|
||
VzBool isDetecting = VzNL_IsAutoDetecting(m_deviceHandle, &errorCode);
|
||
if (errorCode == 0) {
|
||
LOG_INFO("[VizumCamera] IsAutoDetecting after start: {}",
|
||
isDetecting == VzTrue ? "Yes" : "No");
|
||
} else {
|
||
char detectingErrorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(errorCode, detectingErrorMsg);
|
||
LOG_WARN("[VizumCamera] Failed to query IsAutoDetecting after start: {} "
|
||
"(error code: {})", detectingErrorMsg, errorCode);
|
||
}
|
||
|
||
// 3. 等待扫描完成
|
||
// 策略：等待以下任一条件满足：
|
||
// 1. 收到 SDK 扫描完成信号 Device_Auto_Stop 或 Device_Swing_Finish
|
||
// 2. 等待超时
|
||
// 5秒后仍无数据时持续告警，但保留等待，避免慢启动时误判失败。
|
||
int maxWaitTime = (timeoutMs > 0) ? timeoutMs : 30000;
|
||
LOG_INFO("[VizumCamera] Step 3: Waiting up to {}ms for scan to finish...", maxWaitTime);
|
||
LOG_INFO("[VizumCamera] NOTE: The camera will collect point cloud data while the swing motor is running.");
|
||
LOG_INFO("[VizumCamera] If no data is received, please check if the swing motor is enabled in the camera software.");
|
||
|
||
auto startTime = std::chrono::steady_clock::now();
|
||
int lastLoggedPoints = 0;
|
||
int logInterval = 1000; // 每秒输出一次状态
|
||
int lastLogTime = 0;
|
||
int noDataTime = 0; // 无数据的持续时间
|
||
const int noDataWarningThreshold = 5000; // 5秒无数据发出警告
|
||
bool dataReceived = false;
|
||
|
||
while (true) {
|
||
// 检查是否暂停（扫描完成）
|
||
if (m_isPaused) {
|
||
// 获取当前点云状态
|
||
int currentPoints = 0;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanLineMutex);
|
||
currentPoints = static_cast<int>(m_accumulatedCloud.points.size());
|
||
}
|
||
|
||
if (currentPoints > 0) {
|
||
LOG_INFO("[VizumCamera] Scan finished with {} points.", currentPoints);
|
||
break;
|
||
} else {
|
||
auto now = std::chrono::steady_clock::now();
|
||
int elapsed = static_cast<int>(std::chrono::duration_cast<std::chrono::milliseconds>(now - startTime).count());
|
||
|
||
// 如果时间太短且没有点云，可能是虚假的结束信号（例如刚开始时的残留信号），忽略并继续
|
||
if (elapsed < 2000) { // 2秒内结束且无数据
|
||
const int completionStatus = m_scanCompletionStatus.load();
|
||
LOG_WARN("[VizumCamera] Received completion status {} ({}) too early "
|
||
"({}ms) without data. Ignoring...",
|
||
DeviceStatusName(completionStatus), completionStatus, elapsed);
|
||
m_scanCompletionStatus = 0;
|
||
m_isPaused = false;
|
||
} else {
|
||
LOG_WARN("[VizumCamera] Scan finished but no points received after {}ms.", elapsed);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||
|
||
auto now = std::chrono::steady_clock::now();
|
||
int elapsed = static_cast<int>(std::chrono::duration_cast<std::chrono::milliseconds>(now - startTime).count());
|
||
|
||
// 获取当前点云状态
|
||
int currentPoints = 0;
|
||
int scanLines = 0;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanLineMutex);
|
||
currentPoints = static_cast<int>(m_accumulatedCloud.points.size());
|
||
scanLines = static_cast<int>(m_accumulatedScanLines.size());
|
||
}
|
||
|
||
// 检测是否收到数据
|
||
if (currentPoints > 0 && !dataReceived) {
|
||
dataReceived = true;
|
||
LOG_INFO("[VizumCamera] First data received! Points: {}", currentPoints);
|
||
}
|
||
|
||
// 每秒输出一次当前状态
|
||
if (elapsed - lastLogTime >= logInterval) {
|
||
lastLogTime = elapsed;
|
||
|
||
if (currentPoints != lastLoggedPoints) {
|
||
LOG_INFO("[VizumCamera] Scanning... elapsed: {}ms, scan lines: {}, points: {} (new: {})",
|
||
elapsed, scanLines, currentPoints, currentPoints - lastLoggedPoints);
|
||
lastLoggedPoints = currentPoints;
|
||
noDataTime = 0; // 重置无数据计时
|
||
} else {
|
||
noDataTime += logInterval;
|
||
LOG_INFO("[VizumCamera] Waiting... elapsed: {}ms, no new points for {}ms (total: {})",
|
||
elapsed, noDataTime, currentPoints);
|
||
|
||
// 如果超过阈值没有数据，输出警告
|
||
if (noDataTime >= noDataWarningThreshold && !dataReceived) {
|
||
LOG_WARN("[VizumCamera] No data received for {}ms. Please check:", noDataTime);
|
||
LOG_WARN("[VizumCamera] 1. Is the swing motor enabled and running in the camera software?");
|
||
LOG_WARN("[VizumCamera] 2. Is the laser line visible on the calibration board?");
|
||
LOG_WARN("[VizumCamera] 3. Are the camera parameters (exposure, ROI) configured correctly?");
|
||
}
|
||
}
|
||
|
||
// 检查摆镜状态
|
||
float curAngle = VzNL_GetSwingCurAngle(m_deviceHandle, &errorCode);
|
||
if (errorCode == 0) {
|
||
LOG_INFO("[VizumCamera] Current swing angle: {:.2f}", curAngle);
|
||
}
|
||
}
|
||
|
||
if (elapsed > maxWaitTime) {
|
||
LOG_WARN("[VizumCamera] Scan timed out after {}ms", maxWaitTime);
|
||
break;
|
||
}
|
||
}
|
||
// 从此处起 StopAutoDetect 触发的 Auto_Stop 仅属于清理动作，不能改写本次完成原因。
|
||
m_autoDetectRunning = false;
|
||
|
||
int totalDuration = static_cast<int>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::steady_clock::now() - startTime).count());
|
||
|
||
const int completionStatus = m_scanCompletionStatus.load();
|
||
if (m_isPaused) {
|
||
LOG_INFO("[VizumCamera] Scan finished by completion status {} ({}). Duration: {}ms",
|
||
DeviceStatusName(completionStatus), completionStatus, totalDuration);
|
||
} else {
|
||
int detectingError = 0;
|
||
const VzBool stillDetecting = VzNL_IsAutoDetecting(m_deviceHandle, &detectingError);
|
||
char detectingErrorMsg[256] = {0};
|
||
if (detectingError != 0) {
|
||
VzNL_GetErrorInfo(detectingError, detectingErrorMsg);
|
||
}
|
||
size_t timeoutScanLines = 0;
|
||
size_t timeoutValidPoints = 0;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanLineMutex);
|
||
timeoutScanLines = m_accumulatedScanLines.size();
|
||
timeoutValidPoints = m_accumulatedCloud.points.size();
|
||
}
|
||
LOG_WARN("[VizumCamera] Timeout diagnostics: duration={}ms, autoDetecting={}, "
|
||
"detectError={} ({}), completion status={} ({}), scan lines={}, valid points={}",
|
||
totalDuration, stillDetecting == VzTrue ? "Yes" : "No", detectingError,
|
||
detectingError == 0 ? "none" : detectingErrorMsg,
|
||
DeviceStatusName(completionStatus), completionStatus,
|
||
timeoutScanLines, timeoutValidPoints);
|
||
}
|
||
|
||
// 输出扫描过程中的状态
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanLineMutex);
|
||
LOG_INFO("[VizumCamera] After waiting: 扫描线 {} 条, 点数: {}", m_accumulatedScanLines.size(), m_accumulatedCloud.points.size());
|
||
}
|
||
|
||
// 4. 停止自动检测（与VznlCamera::StopAutoDetect一致）
|
||
LOG_INFO("[VizumCamera] Step 4: Stopping AutoDetect...");
|
||
result = VzNL_StopAutoDetect(m_deviceHandle);
|
||
if (result != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(result, errorMsg);
|
||
LOG_WARN("[VizumCamera] Failed to stop auto detect: {}", errorMsg);
|
||
}
|
||
|
||
// 4.1 结束激光检测（关键：与示例代码一致，每次扫描后结束检测）
|
||
LOG_INFO("[VizumCamera] Step 4.1: Ending laser detection...");
|
||
VzNL_EndDetectLaser(m_deviceHandle);
|
||
|
||
// 5. 获取累积的点云数据
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanLineMutex);
|
||
|
||
// 如果有扫描线数据，优先构建带结构的扫描线点云（用于VZNL格式保存）
|
||
if (!m_accumulatedScanLines.empty()) {
|
||
// 每条扫描线=一行(回调已保全宽、无效点为 (0,0,0) 占位)，补齐到最大列宽 → 规整组织化栅格。
|
||
// 这样保存/识别可走"栅格化"链路(去壁面+网格聚类分离粘着圆柱)。
|
||
const size_t H = m_accumulatedScanLines.size();
|
||
size_t W = 0;
|
||
for (const auto& line : m_accumulatedScanLines) {
|
||
if (line.size() > W) W = line.size();
|
||
}
|
||
LOG_INFO("[VizumCamera] Step 5: Building organized grid {}x{} from {} scan lines", W, H, H);
|
||
|
||
pointCloud.clear();
|
||
pointCloud.width = static_cast<uint32_t>(W);
|
||
pointCloud.height = static_cast<uint32_t>(H);
|
||
pointCloud.isOrganized = true;
|
||
pointCloud.points.assign(W * H, Point3D{}); // (0,0,0) = 无效占位
|
||
pointCloud.pixels.assign(W * H, PixelLR{}); // 双目2D坐标,零占位
|
||
pointCloud.scanLines.reserve(H);
|
||
for (size_t r = 0; r < H; ++r) {
|
||
const auto& line = m_accumulatedScanLines[r];
|
||
ScanLineInfo info;
|
||
info.startIndex = r * W;
|
||
info.pointCount = line.size();
|
||
if (r < m_accumulatedLineMeta.size()) {
|
||
info.timestamp = m_accumulatedLineMeta[r].timestamp;
|
||
info.encodeNo = m_accumulatedLineMeta[r].encodeNo;
|
||
}
|
||
pointCloud.scanLines.push_back(info);
|
||
for (size_t c = 0; c < line.size() && c < W; ++c) {
|
||
pointCloud.points[r * W + c] = line[c];
|
||
}
|
||
if (r < m_accumulatedScanLines2D.size()) {
|
||
const auto& line2D = m_accumulatedScanLines2D[r];
|
||
for (size_t c = 0; c < line2D.size() && c < W; ++c) {
|
||
pointCloud.pixels[r * W + c] = line2D[c];
|
||
}
|
||
}
|
||
}
|
||
|
||
// 记录摆镜角速度(供保存元信息用,查询失败保持 0)
|
||
int angleSpeed = 0;
|
||
if (VzNL_GetSwingAngleSpeed(m_deviceHandle, &angleSpeed) == 0) {
|
||
pointCloud.scanSpeed = static_cast<float>(angleSpeed);
|
||
}
|
||
|
||
LOG_INFO("[VizumCamera] Organized grid built: {}x{} ({} cells), {} scan lines, total {} points",
|
||
W, H, W * H, pointCloud.scanLines.size(), pointCloud.points.size());
|
||
}
|
||
else {
|
||
// 如果没有扫描线数据（异常情况），回退到仅使用有效点
|
||
LOG_INFO("[VizumCamera] Step 5: Building point cloud from {} valid points (No scanlines)", m_accumulatedCloud.points.size());
|
||
|
||
pointCloud = std::move(m_accumulatedCloud);
|
||
pointCloud.width = static_cast<uint32_t>(pointCloud.points.size());
|
||
pointCloud.height = 1;
|
||
pointCloud.isOrganized = false;
|
||
}
|
||
|
||
// 清空累积数据
|
||
m_accumulatedScanLines.clear();
|
||
m_accumulatedScanLines2D.clear();
|
||
m_accumulatedLineMeta.clear();
|
||
m_accumulatedCloud.clear();
|
||
}
|
||
|
||
// 在返回前再次验证点云状态
|
||
LOG_INFO("[VizumCamera] Before return: pointCloud.scanLines.size()={}, points.size()={}",
|
||
pointCloud.scanLines.size(), pointCloud.points.size());
|
||
|
||
LOG_INFO("[VizumCamera] ========================================");
|
||
LOG_INFO("[VizumCamera] Capture completed: {} points", pointCloud.points.size());
|
||
LOG_INFO("[VizumCamera] ========================================");
|
||
|
||
return !pointCloud.empty();
|
||
#else
|
||
LOG_WARN("[VizumCamera] ENABLE_VIZUM_CAMERA is NOT defined, returning false");
|
||
(void)pointCloud;
|
||
(void)timeoutMs;
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
bool VizumCamera::GetPointCloud(const std::string& frameId, PointCloud& cloud)
|
||
{
|
||
if (m_cachedFrame.frameId != frameId) {
|
||
return false;
|
||
}
|
||
|
||
cloud = m_cachedFrame.pointCloud;
|
||
return !cloud.empty();
|
||
}
|
||
|
||
bool VizumCamera::GetPointCloudRaw(const std::string& frameId,
|
||
float* buffer, size_t bufferSize)
|
||
{
|
||
if (m_cachedFrame.frameId != frameId || !buffer) {
|
||
return false;
|
||
}
|
||
|
||
const auto& cloud = m_cachedFrame.pointCloud;
|
||
size_t requiredSize = cloud.points.size() * 3 * sizeof(float);
|
||
if (bufferSize < requiredSize) {
|
||
return false;
|
||
}
|
||
|
||
for (size_t i = 0; i < cloud.points.size(); ++i) {
|
||
buffer[i * 3 + 0] = cloud.points[i].x;
|
||
buffer[i * 3 + 1] = cloud.points[i].y;
|
||
buffer[i * 3 + 2] = cloud.points[i].z;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool VizumCamera::GetPointCloudVector(const std::string& frameId,
|
||
std::vector<float>& data)
|
||
{
|
||
if (m_cachedFrame.frameId != frameId) {
|
||
return false;
|
||
}
|
||
|
||
const auto& cloud = m_cachedFrame.pointCloud;
|
||
data.resize(cloud.points.size() * 3);
|
||
|
||
for (size_t i = 0; i < cloud.points.size(); ++i) {
|
||
data[i * 3 + 0] = cloud.points[i].x;
|
||
data[i * 3 + 1] = cloud.points[i].y;
|
||
data[i * 3 + 2] = cloud.points[i].z;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool VizumCamera::GetDepthMap(const std::string& frameId, DepthMap& depthMap)
|
||
{
|
||
if (m_cachedFrame.frameId != frameId) {
|
||
return false;
|
||
}
|
||
|
||
depthMap = m_cachedFrame.depthMap;
|
||
return !depthMap.empty();
|
||
}
|
||
|
||
bool VizumCamera::GetDepthMapRaw(const std::string& frameId,
|
||
float* buffer, size_t bufferSize)
|
||
{
|
||
if (m_cachedFrame.frameId != frameId || !buffer) {
|
||
return false;
|
||
}
|
||
|
||
const auto& depth = m_cachedFrame.depthMap;
|
||
size_t requiredSize = depth.data.size() * sizeof(float);
|
||
if (bufferSize < requiredSize) {
|
||
return false;
|
||
}
|
||
|
||
memcpy(buffer, depth.data.data(), requiredSize);
|
||
return true;
|
||
}
|
||
|
||
bool VizumCamera::CaptureFrame(FrameData& frameData, CaptureType captureType)
|
||
{
|
||
std::string frameId;
|
||
if (!TriggerCapture(frameId, captureType)) {
|
||
return false;
|
||
}
|
||
|
||
frameData.frameId = frameId;
|
||
frameData.timestamp = m_cachedFrame.timestamp;
|
||
|
||
if (captureType & CaptureType::Image) {
|
||
frameData.image = m_cachedFrame.leftImage;
|
||
}
|
||
|
||
if (captureType & CaptureType::PointCloud) {
|
||
frameData.pointCloud = m_cachedFrame.pointCloud;
|
||
}
|
||
|
||
if (captureType & CaptureType::DepthMap) {
|
||
frameData.depthMap = m_cachedFrame.depthMap;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
// ============================================================================
|
||
// 异步采集实现（预留）
|
||
// ============================================================================
|
||
|
||
bool VizumCamera::StartAsyncCapture(FrameCallback callback, CaptureType captureType)
|
||
{
|
||
(void)callback;
|
||
(void)captureType;
|
||
return false; // 暂不实现
|
||
}
|
||
|
||
void VizumCamera::StopAsyncCapture()
|
||
{
|
||
// 暂不实现
|
||
}
|
||
|
||
bool VizumCamera::IsAsyncCapturing() const
|
||
{
|
||
return false;
|
||
}
|
||
|
||
// ============================================================================
|
||
// 相机参数实现
|
||
// ============================================================================
|
||
|
||
bool VizumCamera::GetCameraMatrix(CameraMatrix& matrix)
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
if (!m_isConnected || !m_deviceHandle) {
|
||
return false;
|
||
}
|
||
|
||
// 获取左目内参 (3x3 + 5x1 畸变参数)
|
||
double calibData[14] = {0};
|
||
int result = VzNL_QueryLeftEyeCalibData(m_deviceHandle, calibData);
|
||
if (result != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(result, errorMsg);
|
||
LOG_ERROR("[VizumCamera] Failed to get camera matrix: {}", errorMsg);
|
||
return false;
|
||
}
|
||
|
||
// calibData格式: [fx, fy, cx, cy, k1, k2, p1, p2, k3, ...]
|
||
// 内参矩阵: [fx, 0, cx; 0, fy, cy; 0, 0, 1]
|
||
matrix.fx = static_cast<float>(calibData[0]);
|
||
matrix.fy = static_cast<float>(calibData[1]);
|
||
matrix.cx = static_cast<float>(calibData[2]);
|
||
matrix.cy = static_cast<float>(calibData[3]);
|
||
|
||
return true;
|
||
#else
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
bool VizumCamera::GetCameraMatrixVector(std::vector<float>& matrixVec)
|
||
{
|
||
CameraMatrix matrix;
|
||
if (!GetCameraMatrix(matrix)) {
|
||
return false;
|
||
}
|
||
|
||
matrixVec = matrix.toVector();
|
||
return true;
|
||
}
|
||
|
||
bool VizumCamera::GetDistortionCoeffs(DistortionCoeffs& coeffs)
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
if (!m_isConnected || !m_deviceHandle) {
|
||
return false;
|
||
}
|
||
|
||
// 获取左目标定数据
|
||
double calibData[14] = {0};
|
||
int result = VzNL_QueryLeftEyeCalibData(m_deviceHandle, calibData);
|
||
if (result != 0) {
|
||
return false;
|
||
}
|
||
|
||
// calibData格式: [fx, fy, cx, cy, k1, k2, p1, p2, k3, ...]
|
||
coeffs.k1 = static_cast<float>(calibData[4]);
|
||
coeffs.k2 = static_cast<float>(calibData[5]);
|
||
coeffs.p1 = static_cast<float>(calibData[6]);
|
||
coeffs.p2 = static_cast<float>(calibData[7]);
|
||
coeffs.k3 = static_cast<float>(calibData[8]);
|
||
|
||
return true;
|
||
#else
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
bool VizumCamera::GetDistortionCoeffsVector(std::vector<float>& coeffsVec)
|
||
{
|
||
DistortionCoeffs coeffs;
|
||
if (!GetDistortionCoeffs(coeffs)) {
|
||
return false;
|
||
}
|
||
|
||
coeffsVec = coeffs.toVector();
|
||
return true;
|
||
}
|
||
|
||
// ============================================================================
|
||
// 配置接口实现
|
||
// ============================================================================
|
||
|
||
bool VizumCamera::GetIntParam(const std::string& name, int& value)
|
||
{
|
||
(void)name;
|
||
(void)value;
|
||
// TODO: 根据需要实现
|
||
return false;
|
||
}
|
||
|
||
bool VizumCamera::SetIntParam(const std::string& name, int value)
|
||
{
|
||
(void)name;
|
||
(void)value;
|
||
// TODO: 根据需要实现
|
||
return false;
|
||
}
|
||
|
||
bool VizumCamera::GetFloatParam(const std::string& name, float& value)
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
|
||
if (!m_isConnected || !m_deviceHandle) {
|
||
return false;
|
||
}
|
||
|
||
if (name == "ExposureTime" || name == "Exposure" || name == "exposure") {
|
||
EVzNLExposeMode exposeMode = keVzNLExposeMode_Fix;
|
||
uint32_t exposeUs = 0;
|
||
const int result = VzNL_GetConfigEyeExpose(m_deviceHandle, &exposeMode, &exposeUs);
|
||
if (result != 0) {
|
||
return false;
|
||
}
|
||
value = static_cast<float>(exposeUs);
|
||
return true;
|
||
}
|
||
|
||
if (name == "Gain" || name == "gain") {
|
||
unsigned short rawGain = 0;
|
||
int result = VzNL_GetCameraGain(m_deviceHandle, keEyeSensorType_Left, &rawGain);
|
||
if (result != 0) {
|
||
result = VzNL_GetCameraGain(m_deviceHandle, keEyeSensorType_Right, &rawGain);
|
||
}
|
||
if (result != 0) {
|
||
return false;
|
||
}
|
||
value = static_cast<float>(rawGain);
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
#else
|
||
(void)name;
|
||
(void)value;
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
bool VizumCamera::SetFloatParam(const std::string& name, float value)
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
|
||
if (!m_isConnected || !m_deviceHandle) {
|
||
return false;
|
||
}
|
||
|
||
if (name == "ExposureTime" || name == "Exposure" || name == "exposure") {
|
||
unsigned int minVal = 1;
|
||
unsigned int maxVal = 65535;
|
||
VzNL_QueryParamRange(m_deviceHandle, keDeviceParamType_Expose, &minVal, &maxVal);
|
||
LOG_INFO("[VizumCamera] SetExposure: requested={:.1f}us, range=[{}, {}]", value, minVal, maxVal);
|
||
const float bounded = std::clamp(value, static_cast<float>(minVal), static_cast<float>(maxVal));
|
||
const uint32_t exposeUs = static_cast<uint32_t>(bounded + 0.5f);
|
||
const int result = VzNL_ConfigEyeExpose(m_deviceHandle, keVzNLExposeMode_Fix, exposeUs);
|
||
if (result != 0) {
|
||
char errMsg[256] = {0};
|
||
VzNL_GetErrorInfo(result, errMsg);
|
||
LOG_ERROR("[VizumCamera] VzNL_ConfigEyeExpose failed: code={} msg={}", result, errMsg);
|
||
} else {
|
||
LOG_INFO("[VizumCamera] VzNL_ConfigEyeExpose OK: {}us", exposeUs);
|
||
}
|
||
return result == 0;
|
||
}
|
||
|
||
if (name == "Gain" || name == "gain") {
|
||
unsigned int minVal = 0;
|
||
unsigned int maxVal = 255;
|
||
VzNL_QueryParamRange(m_deviceHandle, keDeviceParamType_Gain, &minVal, &maxVal);
|
||
LOG_INFO("[VizumCamera] SetGain: requested={:.2f}, range=[{}, {}]", value, minVal, maxVal);
|
||
const float bounded = std::clamp(value, static_cast<float>(minVal), static_cast<float>(maxVal));
|
||
const unsigned short rawGain = static_cast<unsigned short>(bounded + 0.5f);
|
||
const int leftResult = VzNL_SetCameraGain(m_deviceHandle, keEyeSensorType_Left, rawGain);
|
||
const int rightResult = VzNL_SetCameraGain(m_deviceHandle, keEyeSensorType_Right, rawGain);
|
||
if (leftResult != 0 && rightResult != 0) {
|
||
char errMsgL[256] = {0}, errMsgR[256] = {0};
|
||
VzNL_GetErrorInfo(leftResult, errMsgL);
|
||
VzNL_GetErrorInfo(rightResult, errMsgR);
|
||
LOG_ERROR("[VizumCamera] VzNL_SetCameraGain both failed: L={}({}) R={}({})",
|
||
leftResult, errMsgL, rightResult, errMsgR);
|
||
} else {
|
||
LOG_INFO("[VizumCamera] VzNL_SetCameraGain OK: gain={} L={} R={}", rawGain, leftResult, rightResult);
|
||
}
|
||
return leftResult == 0 || rightResult == 0;
|
||
}
|
||
|
||
return false;
|
||
#else
|
||
(void)name;
|
||
(void)value;
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
bool VizumCamera::GetStringParam(const std::string& name, std::string& value)
|
||
{
|
||
(void)name;
|
||
(void)value;
|
||
// TODO: 根据需要实现
|
||
return false;
|
||
}
|
||
|
||
bool VizumCamera::SetStringParam(const std::string& name, const std::string& value)
|
||
{
|
||
(void)name;
|
||
(void)value;
|
||
// TODO: 根据需要实现
|
||
return false;
|
||
}
|
||
|
||
bool VizumCamera::GetConfig(std::string& configJson)
|
||
{
|
||
(void)configJson;
|
||
// TODO: 根据需要实现
|
||
return false;
|
||
}
|
||
|
||
bool VizumCamera::SetConfig(const std::string& configJson)
|
||
{
|
||
(void)configJson;
|
||
// TODO: 根据需要实现
|
||
return false;
|
||
}
|
||
|
||
bool VizumCamera::GetDetectROI(ROIRect& leftROI, ROIRect& rightROI)
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
|
||
if (!m_isConnected || !m_deviceHandle) {
|
||
LOG_WARN("[VizumCamera] GetDetectROI failed: device not connected");
|
||
return false;
|
||
}
|
||
|
||
SVzNLROIRect sdkLeft{};
|
||
SVzNLROIRect sdkRight{};
|
||
const int ret = VzNL_GetConfigDetectROI(m_deviceHandle, &sdkLeft, &sdkRight);
|
||
if (ret != 0) {
|
||
char errorMsg[256] = {0};
|
||
VzNL_GetErrorInfo(ret, errorMsg);
|
||
LOG_ERROR("[VizumCamera] VzNL_GetConfigDetectROI failed: {} (error code: {})",
|
||
errorMsg, ret);
|
||
return false;
|
||
}
|
||
|
||
leftROI = {sdkLeft.left, sdkLeft.right, sdkLeft.top, sdkLeft.bottom};
|
||
rightROI = {sdkRight.left, sdkRight.right, sdkRight.top, sdkRight.bottom};
|
||
LOG_INFO("[VizumCamera] Detect ROI read only: left=[{}, {}, {}, {}], "
|
||
"right=[{}, {}, {}, {}]",
|
||
leftROI.left, leftROI.right, leftROI.top, leftROI.bottom,
|
||
rightROI.left, rightROI.right, rightROI.top, rightROI.bottom);
|
||
return true;
|
||
#else
|
||
(void)leftROI;
|
||
(void)rightROI;
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
bool VizumCamera::SetCalibROIEnabled(bool enable)
|
||
{
|
||
#ifdef ENABLE_VIZUM_CAMERA
|
||
const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
|
||
if (!m_isConnected || !m_deviceHandle) {
|
||
LOG_WARN("[VizumCamera] SetCalibROIEnabled({}) failed: device not connected", enable);
|
||
return false;
|
||
}
|
||
const int ret = VzNL_EnableCalibROI(m_deviceHandle, enable ? VzTrue : VzFalse);
|
||
if (ret != 0) {
|
||
LOG_ERROR("[VizumCamera] VzNL_EnableCalibROI({}) failed, error={}", enable, ret);
|
||
return false;
|
||
}
|
||
LOG_INFO("[VizumCamera] CalibROI (rectified image output) {}", enable ? "enabled" : "disabled");
|
||
return true;
|
||
#else
|
||
(void)enable;
|
||
return false;
|
||
#endif
|
||
}
|
||
|
||
SLCameraPtr CreateVizumCamera()
|
||
{
|
||
return std::make_unique<VizumCamera>();
|
||
}
|
||
|
||
#undef m_deviceHandle
|
||
#undef m_vzDeviceInfo
|
||
#undef m_ipAddress
|
||
#undef m_deviceInfo
|
||
#undef m_isConnected
|
||
#undef m_isAsyncCapturing
|
||
#undef m_frameCallback
|
||
#undef m_captureType
|
||
#undef m_mutex
|
||
#undef m_deviceAccessMutex
|
||
#undef m_frameCounter
|
||
#undef m_cachedFrame
|
||
#undef m_accumulatedScanLines
|
||
#undef m_scanLineMutex
|
||
#undef m_autoDetectRunning
|
||
#undef m_isPaused
|
||
#undef m_scanCompletionStatus
|
||
#undef m_accumulatedCloud
|
||
|
||
} // namespace StructuredLight
|