From ff7552a147769390374e4d56f34211f96d387a21 Mon Sep 17 00:00:00 2001 From: Ben <987807597@qq.com> Date: Mon, 6 Jul 2026 00:12:44 -0700 Subject: [PATCH] =?UTF-8?q?=E7=82=B9=E4=BA=91=E8=A1=A5=E9=BD=90=E5=8F=8C?= =?UTF-8?q?=E7=9B=AE2D=E5=83=8F=E7=B4=A0=E5=9D=90=E6=A0=87(SVzNL2DLRPoint)?= =?UTF-8?q?/=E6=AF=8F=E7=BA=BFencodeNo+timestamp/=E6=91=86=E9=95=9C?= =?UTF-8?q?=E9=80=9F=E5=BA=A6,=E4=BE=9B=E5=AE=98=E6=96=B9=20LaserData=20?= =?UTF-8?q?=E6=A0=BC=E5=BC=8F=E5=AF=BC=E5=87=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/VizumCamera.cpp | 60 +++++++++++++++++++++++++++++---- src/core/StructuredLightTypes.h | 15 +++++++++ 2 files changed, 69 insertions(+), 6 deletions(-) diff --git a/src/VizumCamera.cpp b/src/VizumCamera.cpp index 07b8ff3..8fd5749 100644 --- a/src/VizumCamera.cpp +++ b/src/VizumCamera.cpp @@ -73,6 +73,8 @@ struct VizumCamera::Impl { } cachedFrame; std::vector> accumulatedScanLines; + std::vector> accumulatedScanLines2D; // 与扫描线一一对应的双目2D坐标 + std::vector accumulatedLineMeta; // 每线 encodeNo/timestamp std::mutex scanLineMutex; std::atomic autoDetectRunning{false}; std::atomic isPaused{true}; @@ -92,6 +94,8 @@ struct VizumCamera::Impl { #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 @@ -176,17 +180,22 @@ void VizumCamera::ConvertLaserLineDataToPointCloud(void* laserLinePoint) // 将 p3DPoint 转换为 SVzNLPointXYZRGBA* 类型 SVzNLPointXYZRGBA* pPoints = static_cast(pLaserLinePoint->p3DPoint); - + + // keResultDataType_PointXYZRGBA 对应的 2D 数据类型为 SVzNL2DLRPoint(左右目像素坐标) + SVzNL2DLRPoint* pPoints2D = static_cast(pLaserLinePoint->p2DPoint); + // 使用锁保护对累积点云的访问 std::lock_guard lock(m_scanLineMutex); - + // 预先分配内存 m_accumulatedCloud.points.reserve(m_accumulatedCloud.points.size() + nPoints); - + // 同时保存扫描线数据 std::vector scanLine; scanLine.reserve(nPoints); - + std::vector scanLine2D; + scanLine2D.reserve(nPoints); + int validCount = 0; for (int i = 0; i < nPoints; ++i) { SVzNLPointXYZRGBA& point = pPoints[i]; @@ -207,14 +216,30 @@ void VizumCamera::ConvertLaserLineDataToPointCloud(void* laserLinePoint) 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); } - + // 统计信息 LOG_INFO("[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_INFO("[VizumCamera] 累积扫描线数量: {}", m_accumulatedScanLines.size()); } #else @@ -234,6 +259,8 @@ void VizumCamera::ClearAccumulatedScanLines() { std::lock_guard lock(m_scanLineMutex); m_accumulatedScanLines.clear(); + m_accumulatedScanLines2D.clear(); + m_accumulatedLineMeta.clear(); m_accumulatedCloud.clear(); } @@ -889,6 +916,8 @@ bool VizumCamera::CapturePointCloudWithAutoDetect(PointCloud& pointCloud, int ti { std::lock_guard lock(m_scanLineMutex); m_accumulatedScanLines.clear(); + m_accumulatedScanLines2D.clear(); + m_accumulatedLineMeta.clear(); m_accumulatedCloud.clear(); } LOG_INFO("[VizumCamera] Step 1: Cleared accumulated data"); @@ -1089,16 +1118,33 @@ bool VizumCamera::CapturePointCloudWithAutoDetect(PointCloud& pointCloud, int ti pointCloud.height = static_cast(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(angleSpeed); } LOG_INFO("[VizumCamera] Organized grid built: {}x{} ({} cells), {} scan lines, total {} points", @@ -1116,6 +1162,8 @@ bool VizumCamera::CapturePointCloudWithAutoDetect(PointCloud& pointCloud, int ti // 清空累积数据 m_accumulatedScanLines.clear(); + m_accumulatedScanLines2D.clear(); + m_accumulatedLineMeta.clear(); m_accumulatedCloud.clear(); } diff --git a/src/core/StructuredLightTypes.h b/src/core/StructuredLightTypes.h index 7b31de5..17aeae9 100644 --- a/src/core/StructuredLightTypes.h +++ b/src/core/StructuredLightTypes.h @@ -44,14 +44,26 @@ struct ColorPoint3D { struct ScanLineInfo { size_t startIndex = 0; size_t pointCount = 0; + unsigned long long timestamp = 0; // 该激光线时间戳(SDK llTimeStamp) + unsigned int encodeNo = 0; // 该激光线编码器值(SDK nEncodeNo) +}; + +// 双目 2D 像素坐标(极线校正后,左右目 y 相同,x 差为视差) +struct PixelLR { + int leftX = 0; + int leftY = 0; + int rightX = 0; + int rightY = 0; }; struct PointCloud { std::vector points; + std::vector pixels; // 与 points 一一对应;空 = 无 2D 数据 std::vector scanLines; uint32_t width = 0; uint32_t height = 0; bool isOrganized = true; + float scanSpeed = 0.0f; // 采集时的摆镜角速度(查询失败为 0) size_t size() const { return points.size(); } bool empty() const { return points.empty(); } @@ -61,10 +73,13 @@ struct PointCloud { { points.clear(); points.shrink_to_fit(); + pixels.clear(); + pixels.shrink_to_fit(); scanLines.clear(); scanLines.shrink_to_fit(); width = 0; height = 0; + scanSpeed = 0.0f; } Point3D& at(size_t index) { return points.at(index); }