feat(点云处理): 添加基于Z阈值的点云旋转和保存功能

CloudUtils/Inc/LaserDataLoader.h

@@ -6,6 +6,17 @@
 #include "VZNL_Types.h"
 #include "VrError.h"
 
+// 保存进度回调接口
+class ISaveProgressCallback
+{
+public:
+    virtual ~ISaveProgressCallback() = default;
+    // 进度回调，progress 范围 0.0~1.0
+    virtual void onSaveProgress(float progress) = 0;
+    // 返回 true 取消保存
+    virtual bool isSaveCancelled() const { return false; }
+};
+
 // 激光数据加载器类
 class LaserDataLoader
 {
@@ -30,6 +41,8 @@ public:
                          int clockPerSecond);
 
     int DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition>> xyzData);
+    int DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition>> xyzData,
+                       ISaveProgressCallback* callback);
 
     // 释放统一格式数据内存
     void FreeLaserScanData(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines);

CloudUtils/Src/LaserDataLoader.cpp

@@ -246,6 +246,12 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName,
 }
 
 int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition> > xyzData)
+{
+    return DebugSaveLaser(std::move(fileName), std::move(xyzData), nullptr);
+}
+
+int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition>> xyzData,
+                                     ISaveProgressCallback* callback)
 {
     LOG_INFO("Saving unified laser scan data to file: %s\n", fileName.c_str());
 
@@ -270,10 +276,18 @@ int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vecto
         sw << "PointAdjust: 1" << std::endl;
         sw << "MaxTimeStamp:" << 0 << "_" << 0 << std::endl;
 
+        const size_t totalLines = xyzData.size();
         int index = 0;
         // 写入每条扫描线数据
         for (const auto& linePair : xyzData) {
-            sw << "Line_" << index++ << "_" << 0 << "_" << linePair.size()   << std::endl;
+            if (callback && callback->isSaveCancelled()) {
+                sw.close();
+                m_lastError = "Save cancelled by user";
+                LOG_INFO("Save cancelled by user at line %d/%zu\n", index, totalLines);
+                return ERR_CODE(DEV_ARG_INVAILD);
+            }
+
+            sw << "Line_" << index << "_" << 0 << "_" << linePair.size() << std::endl;
             // 根据数据类型写入点云数据
             for(const auto& point : linePair){
 
@@ -281,12 +295,17 @@ int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vecto
                 float x = static_cast<float>(point.pt3D.x);
                 float y = static_cast<float>(point.pt3D.y);
                 float z = static_cast<float>(point.pt3D.z);
-                sw << "{ " 
+                sw << "{ "
-                    << std::fixed << std::setprecision(6) << x << ", " 
+                    << std::fixed << std::setprecision(6) << x << ", "
                     << std::fixed << std::setprecision(6) << y << ", "
                     << std::fixed << std::setprecision(6) << z << " } - ";
                 sw << "{ 0, 0 } - { 0, 0 }" << std::endl;
             }
+
+            ++index;
+            if (callback) {
+                callback->onSaveProgress(static_cast<float>(index) / static_cast<float>(totalLines));
+            }
         }
 
         sw.close();

CloudView/Inc/CloudViewMainWindow.h

@@ -162,11 +162,16 @@ private slots:
      * @brief 应用矩阵变换到所有点云
      */
     void onApplyMatrix();
+    void onApplyEyeInHandTransform();
 
     /**
      * @brief 重置矩阵为单位矩阵
      */
     void onResetMatrix();
+    void onGeneratePointCloud();
+    void onRotateCloudByZ();
+    void onSavePointCloud();
+    void onConvertEulerMatrix();
 
 private:
     /**
@@ -256,6 +261,8 @@ private:
      * @return 是否成功加载到包围盒数据
      */
     bool loadBoundingBoxFile(const QString& fileName);
+    void applyTransformToAllClouds(const QMatrix4x4& matrix);
+    void addGeneratedCloud(const PointCloudXYZ& cloud, const QString& name);
 
     // 点云显示控件
     PointCloudGLWidget* m_glWidget;
@@ -351,6 +358,13 @@ private:
     QTextEdit* m_matrixEdit;
     QPushButton* m_btnLoadMatrix;
     QPushButton* m_btnApplyMatrix;
+    QLineEdit* m_editRobotX;
+    QLineEdit* m_editRobotY;
+    QLineEdit* m_editRobotZ;
+    QLineEdit* m_editRobotRx;
+    QLineEdit* m_editRobotRy;
+    QLineEdit* m_editRobotRz;
+    QPushButton* m_btnApplyEyeInHand;
     QPushButton* m_btnResetMatrix;
 
     // 悬浮缩放按钮

CloudView/Inc/PointCloudGLWidget.h

@@ -12,6 +12,7 @@
 #include <vector>
 
 #include "PointCloudConverter.h"
+#include "VZNL_Types.h"
 
 /**
  * @brief 点云显示颜色枚举
@@ -243,6 +244,21 @@ public:
      */
     void transformAllClouds(const QMatrix4x4& matrix);
 
+    /**
+     * @brief 对所有点云中 Z 值小于阈值的点应用旋转矩阵
+     * @param rotMatrix 旋转矩阵
+     * @param zThreshold Z 轴阈值
+     * @return 被旋转的点数
+     */
+    size_t rotateCloudsByZThreshold(const QMatrix4x4& rotMatrix, float zThreshold);
+
+    /**
+     * @brief 获取所有点云数据（按线分组），用于保存
+     * @param scanLines 输出：每条扫描线的点列表
+     * @return 总点数
+     */
+    size_t getAllCloudsByLines(std::vector<std::vector<SVzNL3DPosition>>& scanLines) const;
+
     /**
      * @brief 添加线段
      */

CloudView/Src/PointCloudGLWidget.cpp

@@ -750,6 +750,98 @@ void PointCloudGLWidget::transformAllClouds(const QMatrix4x4& matrix)
     update();
 }
 
+size_t PointCloudGLWidget::rotateCloudsByZThreshold(const QMatrix4x4& rotMatrix, float zThreshold)
+{
+    size_t rotatedCount = 0;
+
+    for (auto& cloudData : m_pointClouds) {
+        for (size_t i = 0; i + 2 < cloudData.vertices.size(); i += 3) {
+            if (cloudData.vertices[i + 2] < zThreshold) {
+                QVector3D pt(cloudData.vertices[i], cloudData.vertices[i + 1], cloudData.vertices[i + 2]);
+                QVector3D transformed = rotMatrix.map(pt);
+                cloudData.vertices[i] = transformed.x();
+                cloudData.vertices[i + 1] = transformed.y();
+                cloudData.vertices[i + 2] = transformed.z();
+                ++rotatedCount;
+            }
+        }
+    }
+
+    if (rotatedCount > 0) {
+        makeCurrent();
+        for (auto& cloudData : m_pointClouds) {
+            uploadToVBO(cloudData);
+        }
+        doneCurrent();
+        computeBoundingBox();
+        resetView();
+        update();
+    }
+
+    return rotatedCount;
+}
+
+size_t PointCloudGLWidget::getAllCloudsByLines(std::vector<std::vector<SVzNL3DPosition>>& scanLines) const
+{
+    scanLines.clear();
+    size_t totalPoints = 0;
+
+    for (const auto& cloudData : m_pointClouds) {
+        const size_t displayCount = cloudData.vertices.size() / 3;
+        if (displayCount == 0) continue;
+
+        if (cloudData.hasLineInfo && cloudData.totalLines > 0 && cloudData.pointsPerLine > 0) {
+            // 有线信息：用 pointsPerLine 还原每条线的完整点数，过滤掉的 (0,0,0) 点补零
+            const int lines = cloudData.totalLines;
+            const int ptsPerLine = cloudData.pointsPerLine;
+
+            // 初始化所有线，每条线 pointsPerLine 个零点
+            const size_t baseIdx = scanLines.size();
+            for (int i = 0; i < lines; ++i) {
+                std::vector<SVzNL3DPosition> line(ptsPerLine);
+                memset(line.data(), 0, sizeof(SVzNL3DPosition) * ptsPerLine);
+                for (int j = 0; j < ptsPerLine; ++j) {
+                    line[j].nPointIdx = j;
+                }
+                scanLines.push_back(std::move(line));
+            }
+
+            // 用实际显示的点填充对应位置
+            for (size_t i = 0; i < displayCount; ++i) {
+                const int lineIdx = (i < cloudData.lineIndices.size()) ? cloudData.lineIndices[i] : 0;
+                const int ptInLine = (i < cloudData.pointInLineIndices.size()) ? cloudData.pointInLineIndices[i] : static_cast<int>(i);
+
+                if (lineIdx >= 0 && lineIdx < lines && ptInLine >= 0 && ptInLine < ptsPerLine) {
+                    auto& pos = scanLines[baseIdx + lineIdx][ptInLine];
+                    pos.pt3D.x = cloudData.vertices[i * 3];
+                    pos.pt3D.y = cloudData.vertices[i * 3 + 1];
+                    pos.pt3D.z = cloudData.vertices[i * 3 + 2];
+                    pos.nPointIdx = ptInLine;
+                }
+            }
+
+            totalPoints += static_cast<size_t>(lines) * ptsPerLine;
+        } else {
+            // 无线信息，每个点云作为一条线
+            std::vector<SVzNL3DPosition> line;
+            line.reserve(displayCount);
+            for (size_t i = 0; i < displayCount; ++i) {
+                SVzNL3DPosition pos;
+                memset(&pos, 0, sizeof(pos));
+                pos.nPointIdx = static_cast<int>(i);
+                pos.pt3D.x = cloudData.vertices[i * 3];
+                pos.pt3D.y = cloudData.vertices[i * 3 + 1];
+                pos.pt3D.z = cloudData.vertices[i * 3 + 2];
+                line.push_back(pos);
+            }
+            totalPoints += displayCount;
+            scanLines.push_back(std::move(line));
+        }
+    }
+
+    return totalPoints;
+}
+
 void PointCloudGLWidget::computeBoundingBox()
 {
     if (m_pointClouds.empty()) {