点云补齐双目2D像素坐标(SVzNL2DLRPoint)/每线encodeNo+timestamp/摆镜速度,供官方 LaserData 格式导出

src/VizumCamera.cpp

@@ -73,6 +73,8 @@ struct VizumCamera::Impl {
     } 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};
@@ -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
@@ -177,6 +181,9 @@ void VizumCamera::ConvertLaserLineDataToPointCloud(void* laserLinePoint)
     // 将 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);
 
@@ -186,6 +193,8 @@ void VizumCamera::ConvertLaserLineDataToPointCloud(void* laserLinePoint)
     // 同时保存扫描线数据
     std::vector<Point3D> scanLine;
     scanLine.reserve(nPoints);
+    std::vector<PixelLR> scanLine2D;
+    scanLine2D.reserve(nPoints);
 
     int validCount = 0;
     for (int i = 0; i < nPoints; ++i) {
@@ -207,6 +216,16 @@ 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);
     }
 
     // 统计信息
@@ -215,6 +234,12 @@ void VizumCamera::ConvertLaserLineDataToPointCloud(void* laserLinePoint)
     // 将扫描线添加到累积的扫描线数据中
     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<std::mutex> 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<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");
@@ -1089,16 +1118,33 @@ bool VizumCamera::CapturePointCloudWithAutoDetect(PointCloud& pointCloud, int ti
             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",
@@ -1116,6 +1162,8 @@ bool VizumCamera::CapturePointCloudWithAutoDetect(PointCloud& pointCloud, int ti
         
         // 清空累积数据
         m_accumulatedScanLines.clear();
+        m_accumulatedScanLines2D.clear();
+        m_accumulatedLineMeta.clear();
         m_accumulatedCloud.clear();
     }
     

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<Point3D> points;
+    std::vector<PixelLR> pixels;        // 与 points 一一对应;空 = 无 2D 数据
     std::vector<ScanLineInfo> 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); }