From c088ad39c4b0ee5d41d9a6ec2ca5f1efdaea9bf6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=B0=E4=BB=94?= Date: Sat, 25 Apr 2026 19:00:49 +0800 Subject: [PATCH] =?UTF-8?q?feat(=E7=82=B9=E4=BA=91=E5=A4=84=E7=90=86):=20?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=9F=BA=E4=BA=8EZ=E9=98=88=E5=80=BC?= =?UTF-8?q?=E7=9A=84=E7=82=B9=E4=BA=91=E6=97=8B=E8=BD=AC=E5=92=8C=E4=BF=9D?= =?UTF-8?q?=E5=AD=98=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CloudUtils/Inc/LaserDataLoader.h | 13 + CloudUtils/Src/LaserDataLoader.cpp | 25 +- CloudView/Inc/CloudViewMainWindow.h | 14 + CloudView/Inc/PointCloudGLWidget.h | 16 + CloudView/Src/CloudViewMainWindow.cpp | 5513 +++++++++++++++---------- CloudView/Src/PointCloudGLWidget.cpp | 92 + 6 files changed, 3557 insertions(+), 2116 deletions(-) diff --git a/CloudUtils/Inc/LaserDataLoader.h b/CloudUtils/Inc/LaserDataLoader.h index 2ee550c..bd342a1 100644 --- a/CloudUtils/Inc/LaserDataLoader.h +++ b/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> xyzData); + int DebugSaveLaser(std::string fileName, std::vector> xyzData, + ISaveProgressCallback* callback); // 释放统一格式数据内存 void FreeLaserScanData(std::vector>& laserLines); diff --git a/CloudUtils/Src/LaserDataLoader.cpp b/CloudUtils/Src/LaserDataLoader.cpp index f324c56..2f3447a 100644 --- a/CloudUtils/Src/LaserDataLoader.cpp +++ b/CloudUtils/Src/LaserDataLoader.cpp @@ -246,6 +246,12 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName, } int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector > xyzData) +{ + return DebugSaveLaser(std::move(fileName), std::move(xyzData), nullptr); +} + +int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector> 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::vectorisSaveCancelled()) { + 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(point.pt3D.x); float y = static_cast(point.pt3D.y); float z = static_cast(point.pt3D.z); - sw << "{ " - << std::fixed << std::setprecision(6) << x << ", " + sw << "{ " + << 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(index) / static_cast(totalLines)); + } } sw.close(); diff --git a/CloudView/Inc/CloudViewMainWindow.h b/CloudView/Inc/CloudViewMainWindow.h index e7e34f8..80cb71d 100644 --- a/CloudView/Inc/CloudViewMainWindow.h +++ b/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; // 悬浮缩放按钮 diff --git a/CloudView/Inc/PointCloudGLWidget.h b/CloudView/Inc/PointCloudGLWidget.h index fb0b55e..86af55f 100644 --- a/CloudView/Inc/PointCloudGLWidget.h +++ b/CloudView/Inc/PointCloudGLWidget.h @@ -12,6 +12,7 @@ #include #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>& scanLines) const; + /** * @brief 添加线段 */ diff --git a/CloudView/Src/CloudViewMainWindow.cpp b/CloudView/Src/CloudViewMainWindow.cpp index 73fe5ed..613b2a7 100644 --- a/CloudView/Src/CloudViewMainWindow.cpp +++ b/CloudView/Src/CloudViewMainWindow.cpp @@ -1,2113 +1,3400 @@ -#include "CloudViewMainWindow.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "VrLog.h" -#include "LaserDataLoader.h" - -CloudViewMainWindow::CloudViewMainWindow(QWidget* parent) - : QMainWindow(parent) - , m_glWidget(nullptr) - , m_converter(std::make_unique()) - , m_cloudCount(0) - , m_currentLineNum(0) - , m_currentLinePtNum(0) - , m_linePointsDialog(nullptr) - , m_linePointsTable(nullptr) -{ - setupUI(); - LOG_INFO("CloudViewMainWindow initialized\n"); -} - -CloudViewMainWindow::~CloudViewMainWindow() -{ -} - -bool CloudViewMainWindow::eventFilter(QObject* obj, QEvent* event) -{ - if (obj == m_glWidget && event->type() == QEvent::Resize) { - auto* resizeEvt = static_cast(event); - int w = resizeEvt->size().width(); - int h = resizeEvt->size().height(); - int btnW = m_btnZoomIn->width(); - int btnH = m_btnZoomIn->height(); - int margin = 8; - int gap = 4; - int x = w - btnW - margin; - int y = (h - btnH * 2 - gap) / 2; - m_btnZoomIn->move(x, y); - m_btnZoomOut->move(x, y + btnH + gap); - } - return QMainWindow::eventFilter(obj, event); -} - -void CloudViewMainWindow::setupUI() -{ - // 创建中央控件 - QWidget* centralWidget = new QWidget(this); - setCentralWidget(centralWidget); - - // 创建主布局 - QHBoxLayout* mainLayout = new QHBoxLayout(centralWidget); - mainLayout->setContentsMargins(5, 5, 5, 5); - mainLayout->setSpacing(5); - - // 创建分割器 - QSplitter* splitter = new QSplitter(Qt::Horizontal, centralWidget); - - // 左侧：视图工具栏 + 点云显示区域 - QWidget* viewerContainer = new QWidget(splitter); - QHBoxLayout* viewerLayout = new QHBoxLayout(viewerContainer); - viewerLayout->setContentsMargins(0, 0, 0, 0); - viewerLayout->setSpacing(2); - - // 视图方向工具栏（纵向排列，在3D视图左侧） - QWidget* viewToolbar = createViewToolbar(); - viewerLayout->addWidget(viewToolbar); - - // 点云显示区域 - QWidget* viewerArea = createViewerArea(); - viewerLayout->addWidget(viewerArea); - - splitter->addWidget(viewerContainer); - - // 右侧：控制面板 - QWidget* controlPanel = createControlPanel(); - splitter->addWidget(controlPanel); - - // 设置分割器初始大小（左侧 70%，右侧 30%） - splitter->setSizes({700, 300}); - - mainLayout->addWidget(splitter); - - // 状态栏 - statusBar()->showMessage("就绪"); -} - -QWidget* CloudViewMainWindow::createViewerArea() -{ - QWidget* widget = new QWidget(this); - QVBoxLayout* layout = new QVBoxLayout(widget); - layout->setContentsMargins(0, 0, 0, 0); - - m_glWidget = new PointCloudGLWidget(widget); - m_glWidget->setMinimumSize(400, 300); - m_glWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - layout->addWidget(m_glWidget); - - // 悬浮缩放按钮（父控件为 m_glWidget，悬浮在右侧中间） - int zoomBtnSize = 28; - QString zoomBtnStyle = - "QToolButton {" - " background-color: rgba(50, 50, 50, 180);" - " color: white;" - " border: 1px solid rgba(120, 120, 120, 150);" - " border-radius: 4px;" - " font-size: 32px;" - " font-weight: bold;" - "}" - "QToolButton:hover {" - " background-color: rgba(80, 80, 80, 220);" - "}" - "QToolButton:pressed {" - " background-color: rgba(30, 30, 30, 240);" - "}"; - - m_btnZoomIn = new QToolButton(m_glWidget); - m_btnZoomIn->setText("+"); - m_btnZoomIn->setFixedSize(zoomBtnSize, zoomBtnSize); - m_btnZoomIn->setStyleSheet(zoomBtnStyle); - m_btnZoomIn->setAutoRepeat(true); - m_btnZoomIn->setAutoRepeatDelay(300); - m_btnZoomIn->setAutoRepeatInterval(50); - connect(m_btnZoomIn, &QToolButton::clicked, m_glWidget, &PointCloudGLWidget::zoomIn); - - m_btnZoomOut = new QToolButton(m_glWidget); - m_btnZoomOut->setText("-"); - m_btnZoomOut->setFixedSize(zoomBtnSize, zoomBtnSize); - m_btnZoomOut->setStyleSheet(zoomBtnStyle); - m_btnZoomOut->setAutoRepeat(true); - m_btnZoomOut->setAutoRepeatDelay(300); - m_btnZoomOut->setAutoRepeatInterval(50); - connect(m_btnZoomOut, &QToolButton::clicked, m_glWidget, &PointCloudGLWidget::zoomOut); - - // 安装事件过滤器，在 GL 控件 resize 时更新按钮位置 - m_glWidget->installEventFilter(this); - - // 连接信号 - connect(m_glWidget, &PointCloudGLWidget::pointSelected, - this, &CloudViewMainWindow::onPointSelected); - connect(m_glWidget, &PointCloudGLWidget::twoPointsSelected, - this, &CloudViewMainWindow::onTwoPointsSelected); - connect(m_glWidget, &PointCloudGLWidget::lineSelected, - this, &CloudViewMainWindow::onLineSelected); - connect(m_glWidget, &PointCloudGLWidget::viewAnglesChanged, - this, &CloudViewMainWindow::onViewAnglesChanged); - - return widget; -} - -QWidget* CloudViewMainWindow::createControlPanel() -{ - QWidget* widget = new QWidget(this); - widget->setMaximumWidth(350); - QVBoxLayout* layout = new QVBoxLayout(widget); - layout->setContentsMargins(5, 5, 5, 5); - layout->setSpacing(5); - - // 文件操作组 - layout->addWidget(createFileGroup()); - - // 创建 Tab 控件 - QTabWidget* tabWidget = new QTabWidget(widget); - tabWidget->addTab(createMeasurePage(), "选点测距"); - tabWidget->addTab(createLinePage(), "选线"); - tabWidget->addTab(createTransformPage(), "矩阵变换"); - layout->addWidget(tabWidget); - - // 点云列表组 - layout->addWidget(createCloudListGroup()); - - // 添加弹性空间 - layout->addStretch(); - - return widget; -} - -QGroupBox* CloudViewMainWindow::createFileGroup() -{ - QGroupBox* group = new QGroupBox("文件操作", this); - group->setMaximumWidth(350); - QVBoxLayout* layout = new QVBoxLayout(group); - layout->setSpacing(3); - layout->setContentsMargins(5, 5, 5, 5); - - m_btnOpenFile = new QPushButton("打开文件", group); - m_btnOpenFile->setMinimumHeight(24); - m_btnOpenFile->setMaximumHeight(24); - connect(m_btnOpenFile, &QPushButton::clicked, this, &CloudViewMainWindow::onOpenFile); - layout->addWidget(m_btnOpenFile); - - m_btnOpenPose = new QPushButton("打开姿态点 {x,y,z}-{r,p,y}", group); - m_btnOpenPose->setMinimumHeight(24); - m_btnOpenPose->setMaximumHeight(24); - connect(m_btnOpenPose, &QPushButton::clicked, this, &CloudViewMainWindow::onOpenPoseFile); - layout->addWidget(m_btnOpenPose); - - m_btnOpenBBox = new QPushButton("加载包围盒 (grasp.json)", group); - m_btnOpenBBox->setMinimumHeight(24); - m_btnOpenBBox->setMaximumHeight(24); - connect(m_btnOpenBBox, &QPushButton::clicked, this, &CloudViewMainWindow::onOpenBBoxFile); - layout->addWidget(m_btnOpenBBox); - - m_btnClearAll = new QPushButton("清除所有", group); - m_btnClearAll->setMinimumHeight(24); - m_btnClearAll->setMaximumHeight(24); - connect(m_btnClearAll, &QPushButton::clicked, this, &CloudViewMainWindow::onClearAll); - layout->addWidget(m_btnClearAll); - - return group; -} - -QWidget* CloudViewMainWindow::createViewToolbar() -{ - QWidget* widget = new QWidget(this); - QVBoxLayout* layout = new QVBoxLayout(widget); - layout->setContentsMargins(2, 2, 2, 2); - layout->setSpacing(3); - - // 视图预设：图标、提示、旋转角度 - struct ViewPreset { - const char* iconPath; - const char* tooltip; - float rotX; - float rotY; - float rotZ; - }; - - // 坐标系定义：X向右，Y向下，Z朝后 - ViewPreset presets[] = { - {":/common/resource/view_front.png", "正视 (XY面)", 180.0f, 0.0f, 0.0f}, - {":/common/resource/view_back.png", "后视", 180.0f, 180.0f, 0.0f}, - {":/common/resource/view_left.png", "左视 (YZ面)", 180.0f, 90.0f, 0.0f}, - {":/common/resource/view_right.png", "右视", 180.0f, -90.0f, 0.0f}, - {":/common/resource/view_top.png", "俯视 (XZ面)", 90.0f, 0.0f, 0.0f}, - {":/common/resource/view_bottom.png", "仰视", -90.0f, 0.0f, 0.0f}, - {":/common/resource/view_robot.png", "机械臂", -90.0f, 90.0f, 0.0f}, - }; - - int btnSize = 32; - int iconSize = 24; - - for (const auto& preset : presets) { - QToolButton* btn = new QToolButton(widget); - btn->setIcon(QIcon(preset.iconPath)); - btn->setIconSize(QSize(iconSize, iconSize)); - btn->setFixedSize(btnSize, btnSize); - btn->setToolTip(preset.tooltip); - btn->setAutoRaise(true); - - float rx = preset.rotX; - float ry = preset.rotY; - float rz = preset.rotZ; - connect(btn, &QToolButton::clicked, this, [this, rx, ry, rz]() { - m_glWidget->setViewAngles(rx, ry, rz); - m_editRotX->setText(QString::number(rx, 'f', 1)); - m_editRotY->setText(QString::number(ry, 'f', 1)); - m_editRotZ->setText(QString::number(rz, 'f', 1)); - }); - layout->addWidget(btn); - } - - layout->addStretch(); - - // 旋转角度输入（纵向紧凑布局） - m_editRotX = new QLineEdit("180.0", widget); - m_editRotY = new QLineEdit("0.0", widget); - m_editRotZ = new QLineEdit("0.0", widget); - - auto makeAngleRow = [&](const QString& label, QLineEdit* edit) { - QWidget* row = new QWidget(widget); - QVBoxLayout* rowLayout = new QVBoxLayout(row); - rowLayout->setContentsMargins(0, 0, 0, 0); - rowLayout->setSpacing(0); - QLabel* lbl = new QLabel(label, row); - lbl->setStyleSheet("font-size: 10px; color: gray;"); - rowLayout->addWidget(lbl); - edit->setFixedWidth(btnSize); - edit->setMaximumHeight(20); - edit->setStyleSheet("font-size: 10px;"); - rowLayout->addWidget(edit); - return row; - }; - - layout->addWidget(makeAngleRow("RX", m_editRotX)); - layout->addWidget(makeAngleRow("RY", m_editRotY)); - layout->addWidget(makeAngleRow("RZ", m_editRotZ)); - - // 应用按钮 - QPushButton* btnApply = new QPushButton("GO", widget); - btnApply->setFixedSize(btnSize, 20); - btnApply->setStyleSheet("font-size: 10px;"); - auto applyAngles = [this]() { - bool okX, okY, okZ; - float rotX = m_editRotX->text().toFloat(&okX); - float rotY = m_editRotY->text().toFloat(&okY); - float rotZ = m_editRotZ->text().toFloat(&okZ); - if (okX && okY && okZ) { - m_glWidget->setViewAngles(rotX, rotY, rotZ); - } - }; - connect(btnApply, &QPushButton::clicked, this, applyAngles); - connect(m_editRotX, &QLineEdit::returnPressed, this, applyAngles); - connect(m_editRotY, &QLineEdit::returnPressed, this, applyAngles); - connect(m_editRotZ, &QLineEdit::returnPressed, this, applyAngles); - layout->addWidget(btnApply); - - widget->setFixedWidth(btnSize + 8); - return widget; -} - -QWidget* CloudViewMainWindow::createMeasurePage() -{ - QWidget* page = new QWidget(this); - QVBoxLayout* layout = new QVBoxLayout(page); - layout->setContentsMargins(0, 5, 0, 0); - layout->addWidget(createMeasureGroup()); - layout->addStretch(); - return page; -} - -QWidget* CloudViewMainWindow::createLinePage() -{ - QWidget* page = new QWidget(this); - QVBoxLayout* layout = new QVBoxLayout(page); - layout->setContentsMargins(0, 5, 0, 0); - - // 选线拟合组 - layout->addWidget(createLineGroup()); - - // 输入线段组 - QGroupBox* inputLineGroup = new QGroupBox("输入线段", page); - QVBoxLayout* inputLayout = new QVBoxLayout(inputLineGroup); - - // 提示 - QLabel* lblTip = new QLabel("输入两点坐标显示线段", inputLineGroup); - lblTip->setStyleSheet("color: gray; font-size: 10px;"); - inputLayout->addWidget(lblTip); - - // 点1坐标 - QLabel* lblPoint1 = new QLabel("点1:", inputLineGroup); - lblPoint1->setStyleSheet("font-weight: bold;"); - inputLayout->addWidget(lblPoint1); - - QHBoxLayout* p1Layout = new QHBoxLayout(); - p1Layout->addWidget(new QLabel("X:", inputLineGroup)); - m_editLineX1 = new QLineEdit("0.0", inputLineGroup); - m_editLineX1->setMaximumWidth(70); - p1Layout->addWidget(m_editLineX1); - - p1Layout->addWidget(new QLabel("Y:", inputLineGroup)); - m_editLineY1 = new QLineEdit("0.0", inputLineGroup); - m_editLineY1->setMaximumWidth(70); - p1Layout->addWidget(m_editLineY1); - - p1Layout->addWidget(new QLabel("Z:", inputLineGroup)); - m_editLineZ1 = new QLineEdit("0.0", inputLineGroup); - m_editLineZ1->setMaximumWidth(70); - p1Layout->addWidget(m_editLineZ1); - inputLayout->addLayout(p1Layout); - - // 点2坐标 - QLabel* lblPoint2 = new QLabel("点2:", inputLineGroup); - lblPoint2->setStyleSheet("font-weight: bold;"); - inputLayout->addWidget(lblPoint2); - - QHBoxLayout* p2Layout = new QHBoxLayout(); - p2Layout->addWidget(new QLabel("X:", inputLineGroup)); - m_editLineX2 = new QLineEdit("100.0", inputLineGroup); - m_editLineX2->setMaximumWidth(70); - p2Layout->addWidget(m_editLineX2); - - p2Layout->addWidget(new QLabel("Y:", inputLineGroup)); - m_editLineY2 = new QLineEdit("100.0", inputLineGroup); - m_editLineY2->setMaximumWidth(70); - p2Layout->addWidget(m_editLineY2); - - p2Layout->addWidget(new QLabel("Z:", inputLineGroup)); - m_editLineZ2 = new QLineEdit("100.0", inputLineGroup); - m_editLineZ2->setMaximumWidth(70); - p2Layout->addWidget(m_editLineZ2); - inputLayout->addLayout(p2Layout); - - // 按钮 - QHBoxLayout* btnLayout = new QHBoxLayout(); - m_btnShowLine = new QPushButton("显示线段", inputLineGroup); - connect(m_btnShowLine, &QPushButton::clicked, this, &CloudViewMainWindow::onShowInputLine); - btnLayout->addWidget(m_btnShowLine); - - m_btnClearLine2 = new QPushButton("清除线段", inputLineGroup); - connect(m_btnClearLine2, &QPushButton::clicked, this, &CloudViewMainWindow::onClearInputLine); - btnLayout->addWidget(m_btnClearLine2); - inputLayout->addLayout(btnLayout); - - // 连接输入线段坐标框的回车信号 - connect(m_editLineX1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); - connect(m_editLineY1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); - connect(m_editLineZ1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); - connect(m_editLineX2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); - connect(m_editLineY2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); - connect(m_editLineZ2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); - - layout->addWidget(inputLineGroup); - layout->addStretch(); - return page; -} - -QGroupBox* CloudViewMainWindow::createMeasureGroup() -{ - QGroupBox* group = new QGroupBox("选点测距", this); - group->setMaximumWidth(350); - QVBoxLayout* layout = new QVBoxLayout(group); - layout->setSpacing(4); - layout->setContentsMargins(5, 5, 5, 5); - - // 操作说明 - QLabel* lblTip = new QLabel("Ctrl+左键点击点云选择点", group); - lblTip->setWordWrap(true); - lblTip->setStyleSheet("color: gray; font-size: 12px;"); - layout->addWidget(lblTip); - // 测距复选框 - m_cbMeasureDistance = new QCheckBox("启用测距", group); - m_cbMeasureDistance->setChecked(false); - connect(m_cbMeasureDistance, &QCheckBox::toggled, this, [this](bool checked) { - m_glWidget->setMeasureDistanceEnabled(checked); - m_glWidget->clearSelectedPoints(); - m_lblPoint1->setText("点1: --"); - m_lblPoint2->setText("点2: --"); - m_lblDistance->setText("--"); - m_editPoint1X->setText("--"); - m_editPoint1Y->setText("--"); - m_editPoint1Z->setText("--"); - m_editPoint2X->setText("--"); - m_editPoint2Y->setText("--"); - m_editPoint2Z->setText("--"); - }); - layout->addWidget(m_cbMeasureDistance); - - // 清除选点按钮 - m_btnClearPoints = new QPushButton("清除选点", group); - m_btnClearPoints->setMinimumHeight(24); - connect(m_btnClearPoints, &QPushButton::clicked, this, &CloudViewMainWindow::onClearSelectedPoints); - layout->addWidget(m_btnClearPoints); - - // 分隔线 - QFrame* line1 = new QFrame(group); - line1->setFrameShape(QFrame::HLine); - line1->setFrameShadow(QFrame::Sunken); - layout->addWidget(line1); - - // 点1信息（坐标直接显示在标题后） - m_lblPoint1 = new QLabel("点1: --", group); - m_lblPoint1->setWordWrap(true); - m_lblPoint1->setStyleSheet("font-weight: bold; font-size: 11px;"); - layout->addWidget(m_lblPoint1); - - // 点1坐标编辑（可修改） - QHBoxLayout* coord1Layout = new QHBoxLayout(); - coord1Layout->setSpacing(5); - coord1Layout->addWidget(new QLabel("X:", group)); - m_editPoint1X = new QLineEdit("--", group); - m_editPoint1X->setMaximumWidth(70); - m_editPoint1X->setMaximumHeight(24); - coord1Layout->addWidget(m_editPoint1X); - - coord1Layout->addWidget(new QLabel("Y:", group)); - m_editPoint1Y = new QLineEdit("--", group); - m_editPoint1Y->setMaximumWidth(70); - m_editPoint1Y->setMaximumHeight(24); - coord1Layout->addWidget(m_editPoint1Y); - - coord1Layout->addWidget(new QLabel("Z:", group)); - m_editPoint1Z = new QLineEdit("--", group); - m_editPoint1Z->setMaximumWidth(70); - m_editPoint1Z->setMaximumHeight(24); - coord1Layout->addWidget(m_editPoint1Z); - coord1Layout->addStretch(); - layout->addLayout(coord1Layout); - - // 连接回车信号 - connect(m_editPoint1X, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint1CoordChanged); - connect(m_editPoint1Y, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint1CoordChanged); - connect(m_editPoint1Z, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint1CoordChanged); - - // 点1姿态输入（紧凑布局） - QHBoxLayout* pose1Layout = new QHBoxLayout(); - pose1Layout->setSpacing(5); - pose1Layout->addWidget(new QLabel("RX:", group)); - m_editRx1 = new QLineEdit("0.0", group); - m_editRx1->setMaximumWidth(50); - pose1Layout->addWidget(m_editRx1); - - pose1Layout->addWidget(new QLabel("RY:", group)); - m_editRy1 = new QLineEdit("0.0", group); - m_editRy1->setMaximumWidth(50); - pose1Layout->addWidget(m_editRy1); - - pose1Layout->addWidget(new QLabel("RZ:", group)); - m_editRz1 = new QLineEdit("0.0", group); - m_editRz1->setMaximumWidth(50); - pose1Layout->addWidget(m_editRz1); - pose1Layout->addStretch(); - layout->addLayout(pose1Layout); - - m_btnShowPose1 = new QPushButton("显示点1姿态", group); - m_btnShowPose1->setMinimumHeight(24); - connect(m_btnShowPose1, &QPushButton::clicked, this, &CloudViewMainWindow::onShowPose1); - layout->addWidget(m_btnShowPose1); - - // 连接姿态输入框的回车信号 - connect(m_editRx1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose1); - connect(m_editRy1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose1); - connect(m_editRz1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose1); - - // 分隔线 - QFrame* line2 = new QFrame(group); - line2->setFrameShape(QFrame::HLine); - line2->setFrameShadow(QFrame::Sunken); - layout->addWidget(line2); - - // 点2信息（坐标直接显示在标题后） - m_lblPoint2 = new QLabel("点2: --", group); - m_lblPoint2->setWordWrap(true); - m_lblPoint2->setStyleSheet("font-weight: bold; font-size: 11px;"); - layout->addWidget(m_lblPoint2); - - // 点2坐标编辑（可修改） - QHBoxLayout* coord2Layout = new QHBoxLayout(); - coord2Layout->setSpacing(5); - coord2Layout->addWidget(new QLabel("X:", group)); - m_editPoint2X = new QLineEdit("--", group); - m_editPoint2X->setMaximumWidth(70); - m_editPoint2X->setMaximumHeight(24); - coord2Layout->addWidget(m_editPoint2X); - - coord2Layout->addWidget(new QLabel("Y:", group)); - m_editPoint2Y = new QLineEdit("--", group); - m_editPoint2Y->setMaximumWidth(70); - m_editPoint2Y->setMaximumHeight(24); - coord2Layout->addWidget(m_editPoint2Y); - - coord2Layout->addWidget(new QLabel("Z:", group)); - m_editPoint2Z = new QLineEdit("--", group); - m_editPoint2Z->setMaximumWidth(70); - m_editPoint2Z->setMaximumHeight(24); - coord2Layout->addWidget(m_editPoint2Z); - coord2Layout->addStretch(); - layout->addLayout(coord2Layout); - - // 连接回车信号 - connect(m_editPoint2X, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint2CoordChanged); - connect(m_editPoint2Y, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint2CoordChanged); - connect(m_editPoint2Z, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint2CoordChanged); - - // 点2姿态输入（紧凑布局） - QHBoxLayout* pose2Layout = new QHBoxLayout(); - pose2Layout->setSpacing(5); - pose2Layout->addWidget(new QLabel("RX:", group)); - m_editRx2 = new QLineEdit("0.0", group); - m_editRx2->setMaximumWidth(50); - pose2Layout->addWidget(m_editRx2); - - pose2Layout->addWidget(new QLabel("RY:", group)); - m_editRy2 = new QLineEdit("0.0", group); - m_editRy2->setMaximumWidth(50); - pose2Layout->addWidget(m_editRy2); - - pose2Layout->addWidget(new QLabel("RZ:", group)); - m_editRz2 = new QLineEdit("0.0", group); - m_editRz2->setMaximumWidth(50); - pose2Layout->addWidget(m_editRz2); - pose2Layout->addStretch(); - layout->addLayout(pose2Layout); - - m_btnShowPose2 = new QPushButton("显示点2姿态", group); - m_btnShowPose2->setMinimumHeight(24); - connect(m_btnShowPose2, &QPushButton::clicked, this, &CloudViewMainWindow::onShowPose2); - layout->addWidget(m_btnShowPose2); - - // 连接姿态输入框的回车信号 - connect(m_editRx2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose2); - connect(m_editRy2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose2); - connect(m_editRz2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose2); - - // 姿态坐标轴缩放输入 - QHBoxLayout* scaleLayout = new QHBoxLayout(); - scaleLayout->setSpacing(5); - scaleLayout->addWidget(new QLabel("坐标轴长度:", group)); - m_editPoseScale = new QLineEdit("10.0", group); - m_editPoseScale->setMaximumWidth(60); - scaleLayout->addWidget(m_editPoseScale); - scaleLayout->addStretch(); - layout->addLayout(scaleLayout); - - // 分隔线 - QFrame* line3 = new QFrame(group); - line3->setFrameShape(QFrame::HLine); - line3->setFrameShadow(QFrame::Sunken); - layout->addWidget(line3); - - // 欧拉角旋转顺序选择 - QLabel* lblEulerOrder = new QLabel("欧拉角旋转顺序:", group); - lblEulerOrder->setStyleSheet("font-weight: bold; font-size: 10px;"); - layout->addWidget(lblEulerOrder); - - m_comboEulerOrder = new QComboBox(group); - m_comboEulerOrder->addItem("ZYX (Yaw-Pitch-Roll)", static_cast(EulerRotationOrder::ZYX)); - m_comboEulerOrder->addItem("XYZ (Roll-Pitch-Yaw)", static_cast(EulerRotationOrder::XYZ)); - m_comboEulerOrder->addItem("ZXY (Yaw-Roll-Pitch)", static_cast(EulerRotationOrder::ZXY)); - m_comboEulerOrder->addItem("YXZ (Pitch-Roll-Yaw)", static_cast(EulerRotationOrder::YXZ)); - m_comboEulerOrder->addItem("XZY (Roll-Yaw-Pitch)", static_cast(EulerRotationOrder::XZY)); - m_comboEulerOrder->addItem("YZX (Pitch-Yaw-Roll)", static_cast(EulerRotationOrder::YZX)); - m_comboEulerOrder->setCurrentIndex(0); - m_comboEulerOrder->setMaximumHeight(24); - connect(m_comboEulerOrder, QOverload::of(&QComboBox::currentIndexChanged), - this, &CloudViewMainWindow::onEulerOrderChanged); - layout->addWidget(m_comboEulerOrder); - - // 分隔线 - QFrame* line4 = new QFrame(group); - line4->setFrameShape(QFrame::HLine); - line4->setFrameShadow(QFrame::Sunken); - layout->addWidget(line4); - - // 距离信息 - QHBoxLayout* distLayout = new QHBoxLayout(); - QLabel* lblDistTitle = new QLabel("距离:", group); - lblDistTitle->setStyleSheet("font-size: 10px;"); - m_lblDistance = new QLabel("--", group); - m_lblDistance->setStyleSheet("font-weight: bold; color: green; font-size: 10px;"); - distLayout->addWidget(lblDistTitle); - distLayout->addWidget(m_lblDistance, 1); - layout->addLayout(distLayout); - - return group; -} - -QGroupBox* CloudViewMainWindow::createLineGroup() -{ - QGroupBox* group = new QGroupBox("选线", this); - group->setMaximumWidth(400); - QVBoxLayout* layout = new QVBoxLayout(group); - layout->setSpacing(3); - layout->setContentsMargins(5, 5, 5, 5); - - // 操作说明 - QLabel* lblTip = new QLabel("Shift+左键点击点云选择线", group); - lblTip->setWordWrap(true); - lblTip->setStyleSheet("color: gray; font-size: 9px;"); - layout->addWidget(lblTip); - - // 选线模式选择 - QHBoxLayout* modeLayout = new QHBoxLayout(); - modeLayout->setSpacing(5); - m_rbVertical = new QRadioButton("纵向", group); - m_rbHorizontal = new QRadioButton("横向", group); - m_rbVertical->setChecked(true); - connect(m_rbVertical, &QRadioButton::toggled, this, &CloudViewMainWindow::onLineSelectModeChanged); - modeLayout->addWidget(m_rbVertical); - modeLayout->addWidget(m_rbHorizontal); - modeLayout->addStretch(); - layout->addLayout(modeLayout); - - // 输入索引选择 - QHBoxLayout* inputLayout = new QHBoxLayout(); - inputLayout->setSpacing(5); - m_lineNumberInput = new QLineEdit(group); - m_lineNumberInput->setPlaceholderText("输入索引"); - m_lineNumberInput->setMaximumHeight(24); - m_btnSelectByNumber = new QPushButton("选择", group); - m_btnSelectByNumber->setMaximumHeight(24); - m_btnSelectByNumber->setMaximumWidth(50); - connect(m_btnSelectByNumber, &QPushButton::clicked, this, &CloudViewMainWindow::onSelectLineByNumber); - connect(m_lineNumberInput, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onSelectLineByNumber); - inputLayout->addWidget(m_lineNumberInput, 1); - inputLayout->addWidget(m_btnSelectByNumber); - layout->addLayout(inputLayout); - - // 清除选线按钮 - m_btnClearLine = new QPushButton("清除选线", group); - m_btnClearLine->setMinimumHeight(24); - connect(m_btnClearLine, &QPushButton::clicked, this, &CloudViewMainWindow::onClearLinePoints); - layout->addWidget(m_btnClearLine); - - // 显示线上点按钮 - m_btnShowLinePoints = new QPushButton("显示线上点", group); - m_btnShowLinePoints->setMinimumHeight(24); - connect(m_btnShowLinePoints, &QPushButton::clicked, this, &CloudViewMainWindow::onShowLinePoints); - layout->addWidget(m_btnShowLinePoints); - - // 线索引信息 - QHBoxLayout* indexLayout = new QHBoxLayout(); - QLabel* lblIndexTitle = new QLabel("索引:", group); - lblIndexTitle->setStyleSheet("font-size: 10px;"); - m_lblLineIndex = new QLabel("--", group); - m_lblLineIndex->setStyleSheet("font-size: 10px;"); - indexLayout->addWidget(lblIndexTitle); - indexLayout->addWidget(m_lblLineIndex, 1); - layout->addLayout(indexLayout); - - // 点数信息 - QHBoxLayout* countLayout = new QHBoxLayout(); - QLabel* lblCountTitle = new QLabel("点数:", group); - lblCountTitle->setStyleSheet("font-size: 10px;"); - m_lblLinePointCount = new QLabel("--", group); - m_lblLinePointCount->setStyleSheet("font-size: 10px;"); - countLayout->addWidget(lblCountTitle); - countLayout->addWidget(m_lblLinePointCount, 1); - layout->addLayout(countLayout); - - return group; -} - -QGroupBox* CloudViewMainWindow::createCloudListGroup() -{ - QGroupBox* group = new QGroupBox("点云列表", this); - group->setMaximumWidth(350); - QVBoxLayout* layout = new QVBoxLayout(group); - layout->setSpacing(3); - layout->setContentsMargins(5, 5, 5, 5); - - m_cloudList = new QListWidget(group); - m_cloudList->setMinimumHeight(70); - layout->addWidget(m_cloudList); - - return group; -} - -void CloudViewMainWindow::onOpenFile() -{ - // 如果已有打开的文件，提示是否清除 - if (m_cloudList->count() > 0) { - auto ret = QMessageBox::question(this, "提示", - "当前已有打开的文件，是否清除？", - QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); - if (ret == QMessageBox::Cancel) { - return; - } - if (ret == QMessageBox::Yes) { - onClearAll(); - } - } - - QString fileName = QFileDialog::getOpenFileName( - this, - "打开文件", - QString(), - "所有支持格式 (*.pcd *.ply *.txt);;PCD 文件 (*.pcd);;PLY 文件 (*.ply);;TXT 文件 (*.txt);;所有文件 (*.*)" - ); - - if (fileName.isEmpty()) { - return; - } - - // 同一文件中可能同时包含点云和线段，两者都尝试加载 - bool cloudOk = loadPointCloudFile(fileName); - bool segmentOk = loadSegmentFile(fileName); - - if (!cloudOk && !segmentOk) { - QMessageBox::critical(this, "错误", "无法从文件中加载点云或线段数据"); - statusBar()->showMessage("加载失败"); - } -} - -bool CloudViewMainWindow::loadPointCloudFile(const QString& fileName) -{ - statusBar()->showMessage("正在加载点云..."); - - QFileInfo fileInfo(fileName); - QString ext = fileInfo.suffix().toLower(); - QString cloudName = QString("Cloud_%1 (%2)").arg(++m_cloudCount).arg(fileInfo.fileName()); - - // 统一使用 PointCloudXYZRGB 加载，支持带颜色和不带颜色的文件 - PointCloudXYZRGB rgbCloud; - int result = m_converter->loadFromFile(fileName.toStdString(), rgbCloud); - - if (result != 0) { - LOG_INFO("[CloudView] Load point cloud failed: %s\n", m_converter->getLastError().c_str()); - return false; - } - - // 保存原始完整点云 XYZ（用于旋转/线上点等功能） - m_originalCloud.clear(); - m_originalCloud.reserve(rgbCloud.size()); - for (size_t i = 0; i < rgbCloud.points.size(); ++i) { - const auto& pt = rgbCloud.points[i]; - Point3D xyzPt(pt.x, pt.y, pt.z); - int lineIdx = (i < rgbCloud.lineIndices.size()) ? rgbCloud.lineIndices[i] : 0; - m_originalCloud.push_back(xyzPt, lineIdx); - } - - // 根据是否有颜色选择显示方式 - bool hadColor = m_converter->lastLoadHadColor(); - - if (ext == "pcd" || ext == "ply") { - // PCD/PLY 文件始终走 XYZRGB 路径，避免被颜色轮换表染成蓝色等 - if (!hadColor) { - // 无 rgb 字段：默认灰色显示 - for (size_t i = 0; i < rgbCloud.points.size(); ++i) { - rgbCloud.points[i].r = 192; - rgbCloud.points[i].g = 192; - rgbCloud.points[i].b = 192; - } - } - m_glWidget->addPointCloud(rgbCloud, cloudName); - LOG_INFO("[CloudView] PCD/PLY loaded with XYZRGB path (hasRgbField=%d), points: %zu\n", - m_converter->lastLoadHadColor(), rgbCloud.size()); - } else if (hadColor) { - m_glWidget->addPointCloud(rgbCloud, cloudName); - LOG_INFO("[CloudView] Loaded with original color, points: %zu\n", rgbCloud.size()); - } else { - m_glWidget->addPointCloud(m_originalCloud, cloudName); - LOG_INFO("[CloudView] Loaded without color (color table), points: %zu\n", m_originalCloud.size()); - } - - // 保存线信息（用于旋转功能） - int lineCount = m_converter->getLoadedLineCount(); - if (lineCount > 0) { - m_currentLineNum = lineCount; - m_currentLinePtNum = static_cast(m_converter->getLoadedPointCount()) / lineCount; - } else { - m_currentLineNum = 0; - m_currentLinePtNum = 0; - } - - // 添加到列表 - QString itemText; - if (lineCount > 0) { - itemText = QString("%1 - %2 点, %3 线").arg(cloudName).arg(m_converter->getLoadedPointCount()).arg(lineCount); - } else { - itemText = QString("%1 - %2 点").arg(cloudName).arg(m_converter->getLoadedPointCount()); - } - if (hadColor) { - itemText += " [彩色]"; - } - m_cloudList->addItem(itemText); - - statusBar()->showMessage(QString("已加载 %1 个点%2").arg(m_converter->getLoadedPointCount()).arg(hadColor ? " (彩色)" : "")); - return true; -} - -bool CloudViewMainWindow::loadSegmentFile(const QString& fileName) -{ - statusBar()->showMessage("正在加载线段..."); - - LaserDataLoader loader; - std::vector> polyLines; - int result = loader.LoadPolySegments(fileName.toStdString(), polyLines); - - if (result != 0) { - LOG_INFO("[CloudView] Load segments failed: %s\n", loader.GetLastError().c_str()); - return false; - } - - if (polyLines.empty()) { - return false; - } - - // 将折线点转换为线段（使用点自带的颜色） - QVector segments; - for (const auto& polyLine : polyLines) { - for (size_t i = 0; i + 1 < polyLine.size(); ++i) { - const auto& p1 = polyLine[i]; - const auto& p2 = polyLine[i + 1]; - // 取起点颜色作为线段颜色，nRGB 为 (A<<24)|BGR 打包格式 - float r = ((p1.nRGB >> 0) & 0xFF) / 255.0f; - float g = ((p1.nRGB >> 8) & 0xFF) / 255.0f; - float b = ((p1.nRGB >> 16) & 0xFF) / 255.0f; - uint8_t a = static_cast((p1.nRGB >> 24) & 0xFF); - // A > 1 时作为线宽使用 - float lineWidth = (a > 1) ? static_cast(a) : 0.0f; - segments.append(LineSegment( - p1.x, p1.y, p1.z, - p2.x, p2.y, p2.z, - r, g, b, lineWidth)); - } - } - - if (segments.isEmpty()) { - return false; - } - - m_glWidget->addLineSegments(segments); - - // 添加到列表 - QFileInfo fileInfo(fileName); - int totalPolyCount = static_cast(polyLines.size()); - QString itemName = QString("Segments (%1)").arg(fileInfo.fileName()); - m_cloudList->addItem(QString("%1 - %2 条折线, %3 条线段") - .arg(itemName).arg(totalPolyCount).arg(segments.size())); - - statusBar()->showMessage(QString("已加载 %1 条折线, %2 条线段").arg(totalPolyCount).arg(segments.size())); - LOG_INFO("[CloudView] Loaded %d polylines, %d segments from %s\n", - totalPolyCount, segments.size(), fileName.toStdString().c_str()); - return true; -} - -void CloudViewMainWindow::onOpenPoseFile() -{ - QString fileName = QFileDialog::getOpenFileName( - this, - "打开姿态点文件", - QString(), - "文本文件 (*.txt);;所有文件 (*.*)" - ); - - if (fileName.isEmpty()) { - return; - } - - statusBar()->showMessage("正在加载姿态点..."); - - QFile file(fileName); - if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { - QMessageBox::critical(this, "错误", QString("无法打开文件: %1").arg(fileName)); - statusBar()->showMessage("加载失败"); - return; - } - - QVector poses; - QTextStream in(&file); - int lineNum = 0; - int validCount = 0; - - while (!in.atEnd()) { - QString line = in.readLine().trimmed(); - lineNum++; - - // 跳过空行和注释 - if (line.isEmpty() || line.startsWith('#')) { - continue; - } - - // 解析格式：{x,y,z}-{r,p,y} - QRegExp regex("\\{([^}]+)\\}-\\{([^}]+)\\}"); - if (regex.indexIn(line) == -1) { - LOG_WARN("[CloudView] Line %d: Invalid format, expected {x,y,z}-{r,p,y}\n", lineNum); - continue; - } - - QString posStr = regex.cap(1); - QString rotStr = regex.cap(2); - - QStringList pos = posStr.split(','); - QStringList rot = rotStr.split(','); - - if (pos.size() != 3 || rot.size() != 3) { - LOG_WARN("[CloudView] Line %d: Invalid point format\n", lineNum); - continue; - } - - bool ok = true; - float x = pos[0].toFloat(&ok); if (!ok) continue; - float y = pos[1].toFloat(&ok); if (!ok) continue; - float z = pos[2].toFloat(&ok); if (!ok) continue; - float roll = rot[0].toFloat(&ok); if (!ok) continue; - float pitch = rot[1].toFloat(&ok); if (!ok) continue; - float yaw = rot[2].toFloat(&ok); if (!ok) continue; - - // 固定大小为10 - float scale = 10.0f; - - poses.append(PosePoint(x, y, z, roll, pitch, yaw, scale)); - validCount++; - } - - file.close(); - - if (poses.isEmpty()) { - QMessageBox::warning(this, "警告", "文件中没有有效的姿态点数据"); - statusBar()->showMessage("加载失败"); - return; - } - - m_glWidget->addPosePoints(poses); - statusBar()->showMessage(QString("已加载 %1 个姿态点").arg(validCount)); - LOG_INFO("[CloudView] Loaded %d pose points from %s\n", validCount, fileName.toStdString().c_str()); -} - -void CloudViewMainWindow::onOpenBBoxFile() -{ - QString fileName = QFileDialog::getOpenFileName( - this, - "打开包围盒文件", - QString(), - "JSON 文件 (*.json);;所有文件 (*.*)" - ); - - if (fileName.isEmpty()) { - return; - } - - if (!loadBoundingBoxFile(fileName)) { - QMessageBox::critical(this, "错误", "无法加载包围盒数据"); - statusBar()->showMessage("加载失败"); - } -} - -bool CloudViewMainWindow::loadBoundingBoxFile(const QString& fileName) -{ - QFile file(fileName); - if (!file.open(QIODevice::ReadOnly)) { - LOG_INFO("[CloudView] Cannot open bbox file: %s\n", fileName.toStdString().c_str()); - return false; - } - - QByteArray data = file.readAll(); - file.close(); - - QJsonParseError parseError; - QJsonDocument doc = QJsonDocument::fromJson(data, &parseError); - if (doc.isNull()) { - LOG_INFO("[CloudView] JSON parse error: %s\n", parseError.errorString().toStdString().c_str()); - return false; - } - - QJsonObject root = doc.object(); - QJsonArray records = root["records"].toArray(); - if (records.isEmpty()) { - LOG_INFO("[CloudView] No records in bbox file\n"); - return false; - } - - // 包围盒8个顶点的12条边索引 - // 底面: 0-1, 1-2, 2-3, 3-0 - // 顶面: 4-5, 5-6, 6-7, 7-4 - // 立柱: 0-4, 1-5, 2-6, 3-7 - static const int edgeIndices[][2] = { - {0, 1}, {1, 2}, {2, 3}, {3, 0}, // 底面 - {4, 5}, {5, 6}, {6, 7}, {7, 4}, // 顶面 - {0, 4}, {1, 5}, {2, 6}, {3, 7} // 立柱 - }; - - QVector segments; - QVector graspPoses; - int bboxCount = 0; - - // 为每个 record 使用不同的颜色 - const float colors[][3] = { - {0.0f, 1.0f, 0.0f}, // 绿色 - {1.0f, 1.0f, 0.0f}, // 黄色 - {0.0f, 1.0f, 1.0f}, // 青色 - {1.0f, 0.5f, 0.0f}, // 橙色 - {1.0f, 0.0f, 1.0f}, // 紫色 - {0.5f, 1.0f, 0.5f}, // 浅绿 - }; - const int colorCount = sizeof(colors) / sizeof(colors[0]); - - for (int i = 0; i < records.size(); ++i) { - QJsonObject record = records[i].toObject(); - QJsonObject bbox = record["bounding_box"].toObject(); - QJsonArray vertices = bbox["vertices"].toArray(); - - if (vertices.size() != 8) { - LOG_WARN("[CloudView] Record %d: vertices count != 8, skip\n", i); - continue; - } - - // 解析8个顶点 - float vx[8], vy[8], vz[8]; - for (int j = 0; j < 8; ++j) { - QJsonObject v = vertices[j].toObject(); - vx[j] = static_cast(v["x"].toDouble()); - vy[j] = static_cast(v["y"].toDouble()); - vz[j] = static_cast(v["z"].toDouble()); - } - - // 选择颜色 - float r = colors[i % colorCount][0]; - float g = colors[i % colorCount][1]; - float b = colors[i % colorCount][2]; - - // 生成12条边的线段 - for (const auto& edge : edgeIndices) { - int a = edge[0]; - int b_idx = edge[1]; - segments.append(LineSegment( - vx[a], vy[a], vz[a], - vx[b_idx], vy[b_idx], vz[b_idx], - r, g, b, 2.0f)); - } - - bboxCount++; - - // 解析抓取点（右抓取点） - if (record.contains("has_right_point") && record["has_right_point"].toBool()) { - QJsonObject graspPt = record["right_grasp_point"].toObject(); - QJsonObject pos = graspPt["position"].toObject(); - QJsonObject ori = graspPt["orientation"].toObject(); - - float px = static_cast(pos["x"].toDouble()); - float py = static_cast(pos["y"].toDouble()); - float pz = static_cast(pos["z"].toDouble()); - float roll = static_cast(ori["roll"].toDouble()); - float pitch = static_cast(ori["pitch"].toDouble()); - float yaw = static_cast(ori["yaw"].toDouble()); - - graspPoses.append(PosePoint(px, py, pz, roll, pitch, yaw, 15.0f)); - } - - // 解析左抓取点 - if (record.contains("has_left_point") && record["has_left_point"].toBool()) { - QJsonObject graspPt = record["left_grasp_point"].toObject(); - QJsonObject pos = graspPt["position"].toObject(); - QJsonObject ori = graspPt["orientation"].toObject(); - - float px = static_cast(pos["x"].toDouble()); - float py = static_cast(pos["y"].toDouble()); - float pz = static_cast(pos["z"].toDouble()); - float roll = static_cast(ori["roll"].toDouble()); - float pitch = static_cast(ori["pitch"].toDouble()); - float yaw = static_cast(ori["yaw"].toDouble()); - - graspPoses.append(PosePoint(px, py, pz, roll, pitch, yaw, 15.0f)); - } - } - - if (segments.isEmpty()) { - return false; - } - - m_glWidget->addLineSegments(segments); - - if (!graspPoses.isEmpty()) { - m_glWidget->addPosePoints(graspPoses); - } - - // 添加到列表 - QFileInfo fileInfo(fileName); - QString itemName = QString("BBox (%1)").arg(fileInfo.fileName()); - QString itemText = QString("%1 - %2 个包围盒, %3 条边") - .arg(itemName).arg(bboxCount).arg(segments.size()); - if (!graspPoses.isEmpty()) { - itemText += QString(", %1 个抓取点").arg(graspPoses.size()); - } - m_cloudList->addItem(itemText); - - statusBar()->showMessage(QString("已加载 %1 个包围盒").arg(bboxCount)); - LOG_INFO("[CloudView] Loaded %d bounding boxes, %d grasp points from %s\n", - bboxCount, graspPoses.size(), fileName.toStdString().c_str()); - return true; -} - -void CloudViewMainWindow::onClearAll() -{ - m_glWidget->clearPointClouds(); - m_glWidget->clearLineSegments(); - m_glWidget->clearPosePoints(); - m_cloudList->clear(); - m_cloudCount = 0; - m_currentLineNum = 0; - m_currentLinePtNum = 0; - m_originalCloud.clear(); - - // 清除选点信息 - m_lblPoint1->setText("点1: --"); - m_lblPoint2->setText("点2: --"); - m_lblDistance->setText("--"); - m_editPoint1X->setText("--"); - m_editPoint1Y->setText("--"); - m_editPoint1Z->setText("--"); - m_editPoint2X->setText("--"); - m_editPoint2Y->setText("--"); - m_editPoint2Z->setText("--"); - - // 清除选线信息 - m_lblLineIndex->setText("--"); - m_lblLinePointCount->setText("--"); - - statusBar()->showMessage("已清除所有数据"); -} - -void CloudViewMainWindow::onResetView() -{ - m_glWidget->resetView(); - statusBar()->showMessage("视图已重置"); -} - -void CloudViewMainWindow::onClearSelectedPoints() -{ - m_glWidget->clearSelectedPoints(); - m_glWidget->clearPosePoints(); // 清除选点时也清除姿态 - m_lblPoint1->setText("点1: --"); - m_lblPoint2->setText("点2: --"); - m_lblDistance->setText("--"); - m_editPoint1X->setText("--"); - m_editPoint1Y->setText("--"); - m_editPoint1Z->setText("--"); - m_editPoint2X->setText("--"); - m_editPoint2Y->setText("--"); - m_editPoint2Z->setText("--"); - statusBar()->showMessage("已清除选中的点"); -} - -void CloudViewMainWindow::onPointSelected(const SelectedPointInfo& point) -{ - if (!point.valid) { - return; - } - - // 选择新点时清除之前的姿态显示 - m_glWidget->clearPosePoints(); - - updateSelectedPointsDisplay(); - - // 状态栏显示：坐标、线号、索引号 - QString statusMsg = QString("选中点: (%1, %2, %3)") - .arg(point.x, 0, 'f', 3) - .arg(point.y, 0, 'f', 3) - .arg(point.z, 0, 'f', 3); - if (point.lineIndex >= 0) { - statusMsg += QString(" | 线号: %1").arg(point.lineIndex); - if (point.pointIndexInLine >= 0) { - statusMsg += QString(" | 索引号: %1").arg(point.pointIndexInLine); - } - } - statusBar()->showMessage(statusMsg); -} - -void CloudViewMainWindow::onTwoPointsSelected(const SelectedPointInfo& p1, const SelectedPointInfo& p2, float distance) -{ - updateSelectedPointsDisplay(); - m_lblDistance->setText(QString("%1 mm").arg(distance, 0, 'f', 3)); - statusBar()->showMessage(QString("测量距离: %1 mm").arg(distance, 0, 'f', 3)); -} - -void CloudViewMainWindow::updateSelectedPointsDisplay() -{ - auto selectedPoints = m_glWidget->getSelectedPoints(); - - if (selectedPoints.size() >= 1 && selectedPoints[0].valid) { - QString text; - if (selectedPoints[0].lineIndex >= 0) { - text = QString("点1: 线号:%1 点序:%2") - .arg(selectedPoints[0].lineIndex) - .arg(selectedPoints[0].pointIndexInLine); - } else { - text = QString("点1:"); - } - m_lblPoint1->setText(text); - - // 更新坐标编辑框 - m_editPoint1X->setText(QString::number(selectedPoints[0].x, 'f', 3)); - m_editPoint1Y->setText(QString::number(selectedPoints[0].y, 'f', 3)); - m_editPoint1Z->setText(QString::number(selectedPoints[0].z, 'f', 3)); - } else { - m_lblPoint1->setText("点1: --"); - m_editPoint1X->setText("--"); - m_editPoint1Y->setText("--"); - m_editPoint1Z->setText("--"); - } - - if (selectedPoints.size() >= 2 && selectedPoints[1].valid) { - QString text; - if (selectedPoints[1].lineIndex >= 0) { - text = QString("点2: 线号:%1 点序:%2") - .arg(selectedPoints[1].lineIndex) - .arg(selectedPoints[1].pointIndexInLine); - } else { - text = QString("点2:"); - } - m_lblPoint2->setText(text); - - // 更新坐标编辑框 - m_editPoint2X->setText(QString::number(selectedPoints[1].x, 'f', 3)); - m_editPoint2Y->setText(QString::number(selectedPoints[1].y, 'f', 3)); - m_editPoint2Z->setText(QString::number(selectedPoints[1].z, 'f', 3)); - } else { - m_lblPoint2->setText("点2: --"); - m_editPoint2X->setText("--"); - m_editPoint2Y->setText("--"); - m_editPoint2Z->setText("--"); - } -} - -void CloudViewMainWindow::onLineSelectModeChanged(bool checked) -{ - if (checked) { - // 纵向模式 - m_glWidget->setLineSelectMode(LineSelectMode::Vertical); - } else { - // 横向模式 - m_glWidget->setLineSelectMode(LineSelectMode::Horizontal); - } - m_lineNumberInput->setPlaceholderText("输入索引"); -} - -void CloudViewMainWindow::onClearLinePoints() -{ - m_glWidget->clearSelectedLine(); - m_glWidget->clearListHighlightPoint(); // 清除列表选中的高亮点 - m_lblLineIndex->setText("--"); - m_lblLinePointCount->setText("--"); - statusBar()->showMessage("已清除选线"); -} - -void CloudViewMainWindow::onLineSelected(const SelectedLineInfo& line) -{ - // 重新选线时清除列表高亮点 - m_glWidget->clearListHighlightPoint(); - - if (!line.valid) { - m_lblLineIndex->setText("--"); - m_lblLinePointCount->setText("--"); - return; - } - - // 状态栏显示：线号/索引号、线点数 - if (line.mode == LineSelectMode::Vertical) { - m_lblLineIndex->setText(QString::number(line.lineIndex)); - statusBar()->showMessage(QString("选中线 | 线号: %1 | 线点数: %2") - .arg(line.lineIndex) - .arg(line.pointCount)); - } else { - // 横向选线：显示索引号 - m_lblLineIndex->setText(QString::number(line.pointIndex)); - statusBar()->showMessage(QString("选中横向线 | 索引号: %1 | 线点数: %2") - .arg(line.pointIndex) - .arg(line.pointCount)); - } - m_lblLinePointCount->setText(QString::number(line.pointCount)); - - // 如果线上点对话框已打开，刷新内容 - updateLinePointsDialog(); -} - -void CloudViewMainWindow::onSelectLineByNumber() -{ - if (m_glWidget->getCloudCount() == 0) { - QMessageBox::warning(this, "提示", "请先加载点云"); - return; - } - - QString text = m_lineNumberInput->text().trimmed(); - if (text.isEmpty()) { - QMessageBox::warning(this, "提示", "请输入索引"); - return; - } - - bool ok; - int index = text.toInt(&ok); - if (!ok || index < 0) { - QMessageBox::warning(this, "提示", "请输入有效的索引（非负整数）"); - return; - } - - bool success = false; - if (m_rbVertical->isChecked()) { - // 纵向选线：直接使用索引 - success = m_glWidget->selectLineByIndex(index); - if (!success) { - QMessageBox::warning(this, "提示", QString("索引 %1 不存在").arg(index)); - } - } else { - // 横向选线：直接使用索引 - success = m_glWidget->selectHorizontalLineByIndex(index); - if (!success) { - QMessageBox::warning(this, "提示", QString("索引 %1 不存在").arg(index)); - } - } -} - -QVector CloudViewMainWindow::getOriginalLinePoints(const SelectedLineInfo& lineInfo) -{ - QVector points; - - if (!lineInfo.valid || m_originalCloud.empty()) { - return points; - } - - if (lineInfo.mode == LineSelectMode::Vertical) { - // 纵向选线：获取同一条扫描线上的所有点 - for (size_t i = 0; i < m_originalCloud.points.size(); ++i) { - if (i < m_originalCloud.lineIndices.size() && - m_originalCloud.lineIndices[i] == lineInfo.lineIndex) { - const auto& pt = m_originalCloud.points[i]; - points.append(QVector3D(pt.x, pt.y, pt.z)); - } - } - } else { - // 横向选线：获取所有线的相同索引点 - if (m_currentLinePtNum > 0 && lineInfo.pointIndex >= 0) { - for (size_t i = 0; i < m_originalCloud.points.size(); ++i) { - int originalIdx = static_cast(i); - if (originalIdx % m_currentLinePtNum == lineInfo.pointIndex) { - const auto& pt = m_originalCloud.points[i]; - points.append(QVector3D(pt.x, pt.y, pt.z)); - } - } - } - } - - return points; -} - -void CloudViewMainWindow::updateLinePointsDialog() -{ - if (!m_linePointsDialog || !m_linePointsTable) { - return; - } - - SelectedLineInfo lineInfo = m_glWidget->getSelectedLine(); - if (!lineInfo.valid) { - m_linePointsTable->setRowCount(0); - m_linePointsDialog->setWindowTitle("线上点坐标"); - m_currentLinePoints.clear(); - return; - } - - // 从原始数据获取线上点（包含0,0,0） - m_currentLinePoints = getOriginalLinePoints(lineInfo); - - // 更新标题 - QString title; - if (lineInfo.mode == LineSelectMode::Vertical) { - title = QString("线上点坐标 - 线号: %1 (共 %2 个点)") - .arg(lineInfo.lineIndex) - .arg(m_currentLinePoints.size()); - } else { - title = QString("线上点坐标 - 索引号: %1 (共 %2 个点)") - .arg(lineInfo.pointIndex) - .arg(m_currentLinePoints.size()); - } - m_linePointsDialog->setWindowTitle(title); - - // 更新表格 - m_linePointsTable->setRowCount(m_currentLinePoints.size()); - - // 斑马线颜色 - QColor evenColor(245, 245, 245); // 浅灰色 - QColor oddColor(255, 255, 255); // 白色 - - for (int i = 0; i < m_currentLinePoints.size(); ++i) { - const QVector3D& pt = m_currentLinePoints[i]; - QColor rowColor = (i % 2 == 0) ? evenColor : oddColor; - - // 序号 - QTableWidgetItem* indexItem = new QTableWidgetItem(QString::number(i)); - indexItem->setTextAlignment(Qt::AlignCenter); - indexItem->setBackground(rowColor); - indexItem->setFlags(indexItem->flags() & ~Qt::ItemIsEditable); - m_linePointsTable->setItem(i, 0, indexItem); - - // X - QTableWidgetItem* xItem = new QTableWidgetItem(QString::number(pt.x(), 'f', 3)); - xItem->setTextAlignment(Qt::AlignCenter); - xItem->setBackground(rowColor); - xItem->setFlags(xItem->flags() & ~Qt::ItemIsEditable); - m_linePointsTable->setItem(i, 1, xItem); - - // Y - QTableWidgetItem* yItem = new QTableWidgetItem(QString::number(pt.y(), 'f', 3)); - yItem->setTextAlignment(Qt::AlignCenter); - yItem->setBackground(rowColor); - yItem->setFlags(yItem->flags() & ~Qt::ItemIsEditable); - m_linePointsTable->setItem(i, 2, yItem); - - // Z - QTableWidgetItem* zItem = new QTableWidgetItem(QString::number(pt.z(), 'f', 3)); - zItem->setTextAlignment(Qt::AlignCenter); - zItem->setBackground(rowColor); - zItem->setFlags(zItem->flags() & ~Qt::ItemIsEditable); - m_linePointsTable->setItem(i, 3, zItem); - } -} - -void CloudViewMainWindow::onShowLinePoints() -{ - SelectedLineInfo lineInfo = m_glWidget->getSelectedLine(); - if (!lineInfo.valid) { - QMessageBox::warning(this, "提示", "请先选择一条线"); - return; - } - - // 如果对话框已存在，刷新内容并显示 - if (m_linePointsDialog) { - updateLinePointsDialog(); - m_linePointsDialog->raise(); - m_linePointsDialog->activateWindow(); - return; - } - - // 创建对话框 - m_linePointsDialog = new QDialog(this); - m_linePointsDialog->resize(450, 500); - m_linePointsDialog->setAttribute(Qt::WA_DeleteOnClose); - - // 对话框关闭时清理指针 - connect(m_linePointsDialog, &QDialog::destroyed, this, [this]() { - m_linePointsDialog = nullptr; - m_linePointsTable = nullptr; - m_currentLinePoints.clear(); - m_glWidget->clearListHighlightPoint(); - }); - - QVBoxLayout* layout = new QVBoxLayout(m_linePointsDialog); - - // 提示标签 - QLabel* lblTip = new QLabel("点击行在3D视图中高亮显示", m_linePointsDialog); - lblTip->setStyleSheet("color: gray; font-size: 10px;"); - layout->addWidget(lblTip); - - // 创建表格控件 - m_linePointsTable = new QTableWidget(m_linePointsDialog); - m_linePointsTable->setColumnCount(4); - m_linePointsTable->setHorizontalHeaderLabels({"序号", "X", "Y", "Z"}); - m_linePointsTable->setFont(QFont("Consolas", 9)); - m_linePointsTable->setSelectionBehavior(QAbstractItemView::SelectRows); - m_linePointsTable->setSelectionMode(QAbstractItemView::SingleSelection); - m_linePointsTable->verticalHeader()->setVisible(false); - - // 设置列宽 - m_linePointsTable->setColumnWidth(0, 60); // 序号 - m_linePointsTable->setColumnWidth(1, 110); // X - m_linePointsTable->setColumnWidth(2, 110); // Y - m_linePointsTable->setColumnWidth(3, 110); // Z - - // 表头样式 - m_linePointsTable->horizontalHeader()->setStretchLastSection(true); - m_linePointsTable->horizontalHeader()->setDefaultAlignment(Qt::AlignCenter); - - connect(m_linePointsTable, &QTableWidget::cellClicked, - this, &CloudViewMainWindow::onLinePointTableClicked); - layout->addWidget(m_linePointsTable); - - // 关闭按钮 - QPushButton* btnClose = new QPushButton("关闭", m_linePointsDialog); - connect(btnClose, &QPushButton::clicked, m_linePointsDialog, &QDialog::close); - layout->addWidget(btnClose); - - // 填充数据 - updateLinePointsDialog(); - - m_linePointsDialog->show(); -} - -void CloudViewMainWindow::onLinePointTableClicked(int row, int column) -{ - Q_UNUSED(column); - - if (row >= 0 && row < m_currentLinePoints.size()) { - const QVector3D& pt = m_currentLinePoints[row]; - m_glWidget->setListHighlightPoint(pt); - - // 在状态栏显示选中点信息 - statusBar()->showMessage(QString("列表选中点 %1: (%2, %3, %4)") - .arg(row) - .arg(pt.x(), 0, 'f', 3) - .arg(pt.y(), 0, 'f', 3) - .arg(pt.z(), 0, 'f', 3)); - } -} - -void CloudViewMainWindow::onShowPose1() -{ - // 从输入框读取点1坐标 - bool ok = true; - float x1 = m_editPoint1X->text().toFloat(&ok); - if (!ok) { QMessageBox::warning(this, "错误", "点1 X 值无效"); return; } - float y1 = m_editPoint1Y->text().toFloat(&ok); - if (!ok) { QMessageBox::warning(this, "错误", "点1 Y 值无效"); return; } - float z1 = m_editPoint1Z->text().toFloat(&ok); - if (!ok) { QMessageBox::warning(this, "错误", "点1 Z 值无效"); return; } - - // 设置选中点（不存在则创建） - m_glWidget->setSelectedPointCoord(0, x1, y1, z1); - - // 读取姿态参数 - float rx = m_editRx1->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点1 RX 值无效"); - return; - } - - float ry = m_editRy1->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点1 RY 值无效"); - return; - } - - float rz = m_editRz1->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点1 RZ 值无效"); - return; - } - - // 读取坐标轴缩放 - float scale = m_editPoseScale->text().toFloat(&ok); - if (!ok || scale <= 0) scale = 25.0f; - - // 清除之前的姿态点 - m_glWidget->clearPosePoints(); - - // 创建点1的姿态点 - PosePoint pose1(x1, y1, z1, rx, ry, rz, scale); - QVector poses; - poses.append(pose1); - - // 如果点2输入框有值，也一起显示 - bool ok2 = true; - float x2 = m_editPoint2X->text().toFloat(&ok2); - float y2 = ok2 ? m_editPoint2Y->text().toFloat(&ok2) : 0; - float z2 = ok2 ? m_editPoint2Z->text().toFloat(&ok2) : 0; - if (ok2 && !(x2 == 0 && y2 == 0 && z2 == 0 && m_editPoint2X->text().isEmpty())) { - m_glWidget->setSelectedPointCoord(1, x2, y2, z2); - - bool okR = true; - float rx2 = m_editRx2->text().toFloat(&okR); - float ry2 = okR ? m_editRy2->text().toFloat(&okR) : 0; - float rz2 = okR ? m_editRz2->text().toFloat(&okR) : 0; - if (okR) { - PosePoint pose2(x2, y2, z2, rx2, ry2, rz2, scale); - poses.append(pose2); - } - } - - // 添加到显示 - m_glWidget->addPosePoints(poses); - - statusBar()->showMessage(QString("已显示点1姿态 (%1, %2, %3) 旋转(%4°, %5°, %6°)") - .arg(x1, 0, 'f', 3).arg(y1, 0, 'f', 3).arg(z1, 0, 'f', 3) - .arg(rx, 0, 'f', 1).arg(ry, 0, 'f', 1).arg(rz, 0, 'f', 1)); - - LOG_INFO("[CloudView] Show pose1 at (%.3f, %.3f, %.3f) with rotation (%.1f, %.1f, %.1f)\n", - x1, y1, z1, rx, ry, rz); -} - -void CloudViewMainWindow::onShowPose2() -{ - // 从输入框读取点2坐标 - bool ok = true; - float x2 = m_editPoint2X->text().toFloat(&ok); - if (!ok) { QMessageBox::warning(this, "错误", "点2 X 值无效"); return; } - float y2 = m_editPoint2Y->text().toFloat(&ok); - if (!ok) { QMessageBox::warning(this, "错误", "点2 Y 值无效"); return; } - float z2 = m_editPoint2Z->text().toFloat(&ok); - if (!ok) { QMessageBox::warning(this, "错误", "点2 Z 值无效"); return; } - - // 设置选中点（不存在则创建） - m_glWidget->setSelectedPointCoord(1, x2, y2, z2); - - // 读取姿态参数 - float rx = m_editRx2->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点2 RX 值无效"); - return; - } - - float ry = m_editRy2->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点2 RY 值无效"); - return; - } - - float rz = m_editRz2->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点2 RZ 值无效"); - return; - } - - // 读取坐标轴缩放 - float scale = m_editPoseScale->text().toFloat(&ok); - if (!ok || scale <= 0) scale = 25.0f; - - // 清除之前的姿态点 - m_glWidget->clearPosePoints(); - - QVector poses; - - // 如果点1输入框有值，也一起显示 - bool ok1 = true; - float x1 = m_editPoint1X->text().toFloat(&ok1); - float y1 = ok1 ? m_editPoint1Y->text().toFloat(&ok1) : 0; - float z1 = ok1 ? m_editPoint1Z->text().toFloat(&ok1) : 0; - if (ok1 && !(x1 == 0 && y1 == 0 && z1 == 0 && m_editPoint1X->text().isEmpty())) { - m_glWidget->setSelectedPointCoord(0, x1, y1, z1); - - bool okR = true; - float rx1 = m_editRx1->text().toFloat(&okR); - float ry1 = okR ? m_editRy1->text().toFloat(&okR) : 0; - float rz1 = okR ? m_editRz1->text().toFloat(&okR) : 0; - if (okR) { - PosePoint pose1(x1, y1, z1, rx1, ry1, rz1, scale); - poses.append(pose1); - } - } - - // 创建点2的姿态点 - PosePoint pose2(x2, y2, z2, rx, ry, rz, scale); - poses.append(pose2); - - // 添加到显示 - m_glWidget->addPosePoints(poses); - - statusBar()->showMessage(QString("已显示点2姿态 (%1, %2, %3) 旋转(%4°, %5°, %6°)") - .arg(x2, 0, 'f', 3).arg(y2, 0, 'f', 3).arg(z2, 0, 'f', 3) - .arg(rx, 0, 'f', 1).arg(ry, 0, 'f', 1).arg(rz, 0, 'f', 1)); - - LOG_INFO("[CloudView] Show pose2 at (%.3f, %.3f, %.3f) with rotation (%.1f, %.1f, %.1f)\n", - x2, y2, z2, rx, ry, rz); -} - -void CloudViewMainWindow::onShowInputLine() -{ - // 读取点1坐标 - bool ok = true; - float x1 = m_editLineX1->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点1 X 值无效"); - return; - } - - float y1 = m_editLineY1->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点1 Y 值无效"); - return; - } - - float z1 = m_editLineZ1->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点1 Z 值无效"); - return; - } - - // 读取点2坐标 - float x2 = m_editLineX2->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点2 X 值无效"); - return; - } - - float y2 = m_editLineY2->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点2 Y 值无效"); - return; - } - - float z2 = m_editLineZ2->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点2 Z 值无效"); - return; - } - - // 清除之前的线段 - m_glWidget->clearLineSegments(); - - // 创建线段（红色） - LineSegment segment(x1, y1, z1, x2, y2, z2, 1.0f, 0.0f, 0.0f); - QVector segments; - segments.append(segment); - - // 添加到显示 - m_glWidget->addLineSegments(segments); - - // 计算距离 - float dx = x2 - x1; - float dy = y2 - y1; - float dz = z2 - z1; - float distance = std::sqrt(dx * dx + dy * dy + dz * dz); - - statusBar()->showMessage(QString("已显示线段 (%1,%2,%3) → (%4,%5,%6) 长度: %7") - .arg(x1, 0, 'f', 1).arg(y1, 0, 'f', 1).arg(z1, 0, 'f', 1) - .arg(x2, 0, 'f', 1).arg(y2, 0, 'f', 1).arg(z2, 0, 'f', 1) - .arg(distance, 0, 'f', 3)); - - LOG_INFO("[CloudView] Show input line from (%.3f, %.3f, %.3f) to (%.3f, %.3f, %.3f) length=%.3f\n", - x1, y1, z1, x2, y2, z2, distance); -} - -void CloudViewMainWindow::onClearInputLine() -{ - m_glWidget->clearLineSegments(); - statusBar()->showMessage("已清除输入的线段"); -} - -QWidget* CloudViewMainWindow::createTransformPage() -{ - QWidget* page = new QWidget(this); - QVBoxLayout* layout = new QVBoxLayout(page); - layout->setContentsMargins(0, 5, 0, 0); - - QGroupBox* group = new QGroupBox("矩阵变换", page); - group->setMaximumWidth(400); - QVBoxLayout* groupLayout = new QVBoxLayout(group); - - // 操作说明 - QLabel* lblTip = new QLabel("输入或从文件加载 4x4 变换矩阵，应用到所有点云", group); - lblTip->setWordWrap(true); - lblTip->setStyleSheet("color: gray; font-size: 10px;"); - groupLayout->addWidget(lblTip); - - // 矩阵编辑区域 - QLabel* lblMatrix = new QLabel("变换矩阵 (4x4):", group); - lblMatrix->setStyleSheet("font-weight: bold;"); - groupLayout->addWidget(lblMatrix); - - m_matrixEdit = new QTextEdit(group); - m_matrixEdit->setFont(QFont("Consolas", 10)); - m_matrixEdit->setMinimumHeight(100); - m_matrixEdit->setMaximumHeight(120); - // 初始化为单位矩阵 - m_matrixEdit->setPlainText( - "1.0 0.0 0.0 0.0\n" - "0.0 1.0 0.0 0.0\n" - "0.0 0.0 1.0 0.0\n" - "0.0 0.0 0.0 1.0" - ); - groupLayout->addWidget(m_matrixEdit); - - // 从文件加载按钮 - m_btnLoadMatrix = new QPushButton("从文件加载矩阵", group); - m_btnLoadMatrix->setMinimumHeight(30); - connect(m_btnLoadMatrix, &QPushButton::clicked, this, &CloudViewMainWindow::onLoadMatrix); - groupLayout->addWidget(m_btnLoadMatrix); - - // 按钮行 - QHBoxLayout* btnLayout = new QHBoxLayout(); - m_btnApplyMatrix = new QPushButton("应用变换", group); - m_btnApplyMatrix->setMinimumHeight(30); - m_btnApplyMatrix->setStyleSheet("QPushButton { background-color: #4CAF50; color: white; font-weight: bold; }" - "QPushButton:hover { background-color: #45a049; }"); - connect(m_btnApplyMatrix, &QPushButton::clicked, this, &CloudViewMainWindow::onApplyMatrix); - btnLayout->addWidget(m_btnApplyMatrix); - - m_btnResetMatrix = new QPushButton("重置矩阵", group); - connect(m_btnResetMatrix, &QPushButton::clicked, this, &CloudViewMainWindow::onResetMatrix); - btnLayout->addWidget(m_btnResetMatrix); - groupLayout->addLayout(btnLayout); - - // 文件格式说明 - QLabel* lblFormat = new QLabel( - "矩阵文件格式：4行，每行4个数值\n" - "分隔符：空格/Tab/逗号\n" - "#开头的行为注释", group); - lblFormat->setWordWrap(true); - lblFormat->setStyleSheet("color: gray; font-size: 9px;"); - groupLayout->addWidget(lblFormat); - - layout->addWidget(group); - layout->addStretch(); - return page; -} - -void CloudViewMainWindow::onLoadMatrix() -{ - QString fileName = QFileDialog::getOpenFileName( - this, - "打开矩阵文件", - QString(), - "文本文件 (*.txt);;所有文件 (*.*)" - ); - - if (fileName.isEmpty()) { - return; - } - - QFile file(fileName); - if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { - QMessageBox::critical(this, "错误", QString("无法打开文件: %1").arg(fileName)); - return; - } - - QTextStream in(&file); - QVector> rows; - - while (!in.atEnd()) { - QString line = in.readLine().trimmed(); - // 跳过空行和注释 - if (line.isEmpty() || line.startsWith('#')) { - continue; - } - - // 将逗号替换为空格，统一分隔符 - line.replace(',', ' '); - line.replace('\t', ' '); - QStringList parts = line.split(' ', QString::SkipEmptyParts); - - QVector row; - bool ok = true; - for (const QString& part : parts) { - float val = part.toFloat(&ok); - if (!ok) break; - row.append(val); - } - - if (!ok || row.size() != 4) { - QMessageBox::warning(this, "格式错误", - QString("第 %1 行格式无效，需要4个数值").arg(rows.size() + 1)); - file.close(); - return; - } - rows.append(row); - - if (rows.size() == 4) { - break; - } - } - file.close(); - - if (rows.size() != 4) { - QMessageBox::warning(this, "格式错误", - QString("矩阵需要4行数据，当前只有 %1 行").arg(rows.size())); - return; - } - - // 将矩阵显示到编辑区域 - QString matrixText; - for (int r = 0; r < 4; ++r) { - QStringList vals; - for (int c = 0; c < 4; ++c) { - vals.append(QString::number(static_cast(rows[r][c]), 'f', 6)); - } - matrixText += vals.join(" "); - if (r < 3) matrixText += "\n"; - } - m_matrixEdit->setPlainText(matrixText); - - statusBar()->showMessage(QString("已从 %1 加载矩阵").arg(QFileInfo(fileName).fileName())); - LOG_INFO("[CloudView] Loaded matrix from %s\n", fileName.toStdString().c_str()); -} - -void CloudViewMainWindow::onApplyMatrix() -{ - if (m_glWidget->getCloudCount() == 0) { - QMessageBox::warning(this, "提示", "请先加载点云"); - return; - } - - // 解析编辑区域中的矩阵 - QString text = m_matrixEdit->toPlainText().trimmed(); - QStringList lines = text.split('\n', QString::SkipEmptyParts); - - QVector> rows; - for (const QString& line : lines) { - QString cleaned = line.trimmed(); - if (cleaned.isEmpty() || cleaned.startsWith('#')) { - continue; - } - cleaned.replace(',', ' '); - cleaned.replace('\t', ' '); - QStringList parts = cleaned.split(' ', QString::SkipEmptyParts); - - QVector row; - bool ok = true; - for (const QString& part : parts) { - float val = part.toFloat(&ok); - if (!ok) break; - row.append(val); - } - - if (!ok || row.size() != 4) { - QMessageBox::warning(this, "格式错误", "矩阵格式无效，需要4行4列数值"); - return; - } - rows.append(row); - } - - if (rows.size() != 4) { - QMessageBox::warning(this, "格式错误", - QString("矩阵需要4行数据，当前 %1 行").arg(rows.size())); - return; - } - - // 构造 QMatrix4x4（按行优先存储） - float values[16]; - for (int r = 0; r < 4; ++r) { - for (int c = 0; c < 4; ++c) { - values[r * 4 + c] = rows[r][c]; - } - } - QMatrix4x4 matrix(values); - - // 检查是否为单位矩阵 - if (matrix.isIdentity()) { - QMessageBox::information(this, "提示", "当前矩阵为单位矩阵，无需变换"); - return; - } - - // 应用变换 - m_glWidget->transformAllClouds(matrix); - - statusBar()->showMessage("已应用矩阵变换到所有点云"); - LOG_INFO("[CloudView] Applied matrix transform to all point clouds\n"); -} - -void CloudViewMainWindow::onResetMatrix() -{ - m_matrixEdit->setPlainText( - "1.0 0.0 0.0 0.0\n" - "0.0 1.0 0.0 0.0\n" - "0.0 0.0 1.0 0.0\n" - "0.0 0.0 0.0 1.0" - ); - statusBar()->showMessage("矩阵已重置为单位矩阵"); -} - -void CloudViewMainWindow::onEulerOrderChanged(int index) -{ - if (!m_glWidget) { - return; - } - - EulerRotationOrder order = static_cast(m_comboEulerOrder->itemData(index).toInt()); - m_glWidget->setEulerRotationOrder(order); - - // 如果有姿态点，刷新显示 - m_glWidget->update(); - - QString orderName = m_comboEulerOrder->currentText(); - statusBar()->showMessage(QString("欧拉角旋转顺序已切换为: %1").arg(orderName)); - LOG_INFO("[CloudView] Euler rotation order changed to: %s\n", orderName.toStdString().c_str()); -} - -void CloudViewMainWindow::onViewAnglesChanged(float rotX, float rotY, float rotZ) -{ - // 更新显示的角度值 - m_editRotX->setText(QString::number(rotX, 'f', 1)); - m_editRotY->setText(QString::number(rotY, 'f', 1)); - m_editRotZ->setText(QString::number(rotZ, 'f', 1)); -} - -void CloudViewMainWindow::onPoint1CoordChanged() -{ - // 读取编辑框中的坐标 - bool ok = true; - float x = m_editPoint1X->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点1 X 值无效"); - return; - } - - float y = m_editPoint1Y->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点1 Y 值无效"); - return; - } - - float z = m_editPoint1Z->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点1 Z 值无效"); - return; - } - - // 直接设置选中点坐标（无论是否已有鼠标选点） - m_glWidget->setSelectedPointCoord(0, x, y, z); - updateSelectedPointsDisplay(); - - // 如果启用了测距且有两个点，重新计算距离 - auto updatedPoints = m_glWidget->getSelectedPoints(); - if (m_glWidget->isMeasureDistanceEnabled() && updatedPoints.size() >= 2 && updatedPoints[1].valid) { - float distance = m_glWidget->calculateDistance(updatedPoints[0], updatedPoints[1]); - m_lblDistance->setText(QString("%1 mm").arg(distance, 0, 'f', 3)); - statusBar()->showMessage(QString("点1坐标已更新，距离: %1 mm").arg(distance, 0, 'f', 3)); - } else { - statusBar()->showMessage(QString("点1坐标已更新为 (%1, %2, %3)").arg(x, 0, 'f', 3).arg(y, 0, 'f', 3).arg(z, 0, 'f', 3)); - } - - LOG_INFO("[CloudView] Point1 coord updated to (%.3f, %.3f, %.3f)\n", x, y, z); -} - -void CloudViewMainWindow::onPoint2CoordChanged() -{ - // 读取编辑框中的坐标 - bool ok = true; - float x = m_editPoint2X->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点2 X 值无效"); - return; - } - - float y = m_editPoint2Y->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点2 Y 值无效"); - return; - } - - float z = m_editPoint2Z->text().toFloat(&ok); - if (!ok) { - QMessageBox::warning(this, "错误", "点2 Z 值无效"); - return; - } - - // 直接设置选中点坐标（无论是否已有鼠标选点） - m_glWidget->setSelectedPointCoord(1, x, y, z); - updateSelectedPointsDisplay(); - - // 如果启用了测距，重新计算距离 - auto updatedPoints = m_glWidget->getSelectedPoints(); - if (m_glWidget->isMeasureDistanceEnabled() && updatedPoints.size() >= 2 && updatedPoints[0].valid) { - float distance = m_glWidget->calculateDistance(updatedPoints[0], updatedPoints[1]); - m_lblDistance->setText(QString("%1 mm").arg(distance, 0, 'f', 3)); - statusBar()->showMessage(QString("点2坐标已更新，距离: %1 mm").arg(distance, 0, 'f', 3)); - } else { - statusBar()->showMessage(QString("点2坐标已更新为 (%1, %2, %3)").arg(x, 0, 'f', 3).arg(y, 0, 'f', 3).arg(z, 0, 'f', 3)); - } - - LOG_INFO("[CloudView] Point2 coord updated to (%.3f, %.3f, %.3f)\n", x, y, z); -} - +#include "CloudViewMainWindow.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "VrLog.h" +#include "LaserDataLoader.h" + +namespace +{ +constexpr double kPi = 3.14159265358979323846; +constexpr double kDegToRad = kPi / 180.0; + +bool parseMatrixText(const QString& text, QMatrix4x4& matrix, QString* errorMessage = nullptr) +{ + QStringList lines = text.split('\n', QString::SkipEmptyParts); + QVector> rows; + + for (const QString& line : lines) { + QString cleaned = line.trimmed(); + if (cleaned.isEmpty() || cleaned.startsWith('#')) { + continue; + } + + cleaned.replace(',', ' '); + cleaned.replace('\t', ' '); + const QStringList parts = cleaned.split(' ', QString::SkipEmptyParts); + + QVector row; + bool ok = true; + for (const QString& part : parts) { + const float value = part.toFloat(&ok); + if (!ok) { + break; + } + row.append(value); + } + + if (!ok || row.size() != 4) { + if (errorMessage) { + *errorMessage = QString("Matrix row %1 must contain 4 numeric values.").arg(rows.size() + 1); + } + return false; +#if 0 + *errorMessage = QString("矩阵格式无效，第 %1 行需要 4 个数值").arg(rows.size() + 1); + } + */ return false; +#endif + } + + rows.append(row); + } + + if (rows.size() != 4) { + if (errorMessage) { + *errorMessage = QString("Matrix must contain 4 rows. Current row count: %1.").arg(rows.size()); + } + return false; +#if 0 + *errorMessage = QString("矩阵需要 4 行数值，当前为 %1 行").arg(rows.size()); + } + */ return false; +#endif + } + + matrix.setToIdentity(); + for (int row = 0; row < 4; ++row) { + for (int col = 0; col < 4; ++col) { + matrix(row, col) = rows[row][col]; + } + } + return true; +} + +QString formatMatrixText(const QMatrix4x4& matrix) +{ + QStringList lines; + for (int row = 0; row < 4; ++row) { + QStringList values; + for (int col = 0; col < 4; ++col) { + values.append(QString::number(matrix(row, col), 'f', 6)); + } + lines.append(values.join(" ")); + } + return lines.join("\n"); +} + +// 从 EyeHandCalibMatrixInfo.ini 格式加载标定矩阵 +// 返回 true 表示成功，matrixList 填充所有找到的矩阵及其描述信息 +bool parseCalibIniFile(const QString& fileName, QVector>& matrixList, QString* errorMessage = nullptr) +{ + QSettings settings(fileName, QSettings::IniFormat); + + const int nExist = settings.value("CommInfo/nExistMatrixNum", 0).toInt(); + if (nExist <= 0) { + if (errorMessage) *errorMessage = "INI 文件中未找到标定矩阵 (nExistMatrixNum=0)"; + return false; + } + + for (int i = 0; i < nExist; ++i) { + const QString section = QString("CalibMatrixInfo_%1").arg(i); + settings.beginGroup(section); + + QMatrix4x4 matrix; + matrix.setToIdentity(); + bool ok = true; + for (int idx = 0; idx < 16; ++idx) { + const QString key = QString("dCalibMatrix_%1").arg(idx); + const double val = settings.value(key, 0.0).toDouble(&ok); + if (!ok) break; + const int row = idx / 4; + const int col = idx % 4; + matrix(row, col) = static_cast(val); + } + + if (!ok) { + settings.endGroup(); + if (errorMessage) *errorMessage = QString("矩阵 %1 的数据格式无效").arg(i); + return false; + } + + const int posIdx = settings.value("nCalibPosIdx", i).toInt(); + const QString posName = settings.value("sCalibPosName", "").toString(); + const QString calibTime = settings.value("sCalibTime", "").toString(); + QString desc = QString("矩阵 %1 (位置%2").arg(i).arg(posIdx); + if (!posName.isEmpty()) desc += QString(", %1").arg(posName); + if (!calibTime.isEmpty()) desc += QString(", %1").arg(calibTime); + desc += ")"; + + settings.endGroup(); + matrixList.append(qMakePair(desc, matrix)); + } + return true; +} + +QMatrix4x4 makeAxisRotationMatrix(char axis, double angleRad) +{ + QMatrix4x4 matrix; + matrix.setToIdentity(); + + const float c = static_cast(std::cos(angleRad)); + const float s = static_cast(std::sin(angleRad)); + + switch (axis) { + case 'x': + matrix(1, 1) = c; + matrix(1, 2) = -s; + matrix(2, 1) = s; + matrix(2, 2) = c; + break; + case 'y': + matrix(0, 0) = c; + matrix(0, 2) = s; + matrix(2, 0) = -s; + matrix(2, 2) = c; + break; + case 'z': + matrix(0, 0) = c; + matrix(0, 1) = -s; + matrix(1, 0) = s; + matrix(1, 1) = c; + break; + default: + break; + } + + return matrix; +} + +QMatrix4x4 buildRobotPoseMatrix(float x, float y, float z, float rxDeg, float ryDeg, float rzDeg) +{ + const QMatrix4x4 rotation = + makeAxisRotationMatrix('z', rzDeg * kDegToRad) * + makeAxisRotationMatrix('y', ryDeg * kDegToRad) * + makeAxisRotationMatrix('x', rxDeg * kDegToRad); + + QMatrix4x4 matrix = rotation; + matrix(0, 3) = x; + matrix(1, 3) = y; + matrix(2, 3) = z; + matrix(3, 0) = 0.0f; + matrix(3, 1) = 0.0f; + matrix(3, 2) = 0.0f; + matrix(3, 3) = 1.0f; + return matrix; +} + +void transformPointCloud(PointCloudXYZ& cloud, const QMatrix4x4& matrix) +{ + for (auto& point : cloud.points) { + const QVector3D transformed = matrix.map(QVector3D(point.x, point.y, point.z)); + point.x = transformed.x(); + point.y = transformed.y(); + point.z = transformed.z(); + } +} + +double clampValue(double value, double minValue, double maxValue) +{ + return std::max(minValue, std::min(maxValue, value)); +} + +QMatrix4x4 buildEulerRotationMatrix(EulerRotationOrder order, double rxDeg, double ryDeg, double rzDeg) +{ + const QMatrix4x4 Rx = makeAxisRotationMatrix('x', rxDeg * kDegToRad); + const QMatrix4x4 Ry = makeAxisRotationMatrix('y', ryDeg * kDegToRad); + const QMatrix4x4 Rz = makeAxisRotationMatrix('z', rzDeg * kDegToRad); + + switch (order) { + case EulerRotationOrder::ZYX: return Rz * Ry * Rx; + case EulerRotationOrder::XYZ: return Rx * Ry * Rz; + case EulerRotationOrder::ZXY: return Rz * Rx * Ry; + case EulerRotationOrder::YXZ: return Ry * Rx * Rz; + case EulerRotationOrder::XZY: return Rx * Rz * Ry; + case EulerRotationOrder::YZX: return Ry * Rz * Rx; + } + + QMatrix4x4 identity; + identity.setToIdentity(); + return identity; +} + +QVector3D rotationMatrixToEuler(EulerRotationOrder order, const QMatrix4x4& m) +{ + constexpr double kEps = 1e-8; + double rx = 0.0; + double ry = 0.0; + double rz = 0.0; + + switch (order) { + case EulerRotationOrder::ZYX: { + const double cy = std::sqrt(m(0, 0) * m(0, 0) + m(1, 0) * m(1, 0)); + if (cy > kEps) { + rx = std::atan2(m(2, 1), m(2, 2)); + ry = std::atan2(-m(2, 0), cy); + rz = std::atan2(m(1, 0), m(0, 0)); + } else { + rx = std::atan2(-m(1, 2), m(1, 1)); + ry = std::atan2(-m(2, 0), cy); + rz = 0.0; + } + break; + } + case EulerRotationOrder::XYZ: { + const double cy = std::sqrt(m(0, 0) * m(0, 0) + m(0, 1) * m(0, 1)); + if (cy > kEps) { + rx = std::atan2(-m(1, 2), m(2, 2)); + ry = std::atan2(m(0, 2), cy); + rz = std::atan2(-m(0, 1), m(0, 0)); + } else { + rx = std::atan2(m(2, 1), m(1, 1)); + ry = std::atan2(m(0, 2), cy); + rz = 0.0; + } + break; + } + case EulerRotationOrder::ZXY: { + const double cx = std::sqrt(m(2, 0) * m(2, 0) + m(2, 2) * m(2, 2)); + if (cx > kEps) { + rx = std::atan2(m(2, 1), cx); + ry = std::atan2(-m(2, 0), m(2, 2)); + rz = std::atan2(-m(0, 1), m(1, 1)); + } else { + rx = std::atan2(m(2, 1), cx); + ry = 0.0; + rz = std::atan2(m(1, 0), m(0, 0)); + } + break; + } + case EulerRotationOrder::YXZ: { + const double cx = std::sqrt(m(1, 0) * m(1, 0) + m(1, 1) * m(1, 1)); + if (cx > kEps) { + rx = std::atan2(-m(1, 2), cx); + ry = std::atan2(m(0, 2), m(2, 2)); + rz = std::atan2(m(1, 0), m(1, 1)); + } else { + rx = std::atan2(-m(1, 2), cx); + ry = std::atan2(-m(2, 0), m(0, 0)); + rz = 0.0; + } + break; + } + case EulerRotationOrder::XZY: { + const double cz = std::sqrt(m(0, 0) * m(0, 0) + m(0, 2) * m(0, 2)); + if (cz > kEps) { + rx = std::atan2(m(2, 1), m(1, 1)); + ry = std::atan2(m(0, 2), m(0, 0)); + rz = std::atan2(-m(0, 1), cz); + } else { + rx = std::atan2(-m(1, 2), m(2, 2)); + ry = 0.0; + rz = std::atan2(-m(0, 1), cz); + } + break; + } + case EulerRotationOrder::YZX: { + const double cz = std::sqrt(m(1, 1) * m(1, 1) + m(1, 2) * m(1, 2)); + if (cz > kEps) { + rx = std::atan2(-m(1, 2), m(1, 1)); + ry = std::atan2(-m(2, 0), m(0, 0)); + rz = std::atan2(m(1, 0), cz); + } else { + rx = 0.0; + ry = std::atan2(m(0, 2), m(2, 2)); + rz = std::atan2(m(1, 0), cz); + } + break; + } + } + + return QVector3D(static_cast(rx / kDegToRad), + static_cast(ry / kDegToRad), + static_cast(rz / kDegToRad)); +} + +PointCloudXYZ generateBoxPointCloud(const QVector3D& center, const QVector3D& size, float spacing) +{ + PointCloudXYZ cloud; + + const int xSteps = std::max(1, static_cast(std::ceil(size.x() / spacing))); + const int ySteps = std::max(1, static_cast(std::ceil(size.y() / spacing))); + const int zSteps = std::max(1, static_cast(std::ceil(size.z() / spacing))); + + const float stepX = size.x() / xSteps; + const float stepY = size.y() / ySteps; + const float stepZ = size.z() / zSteps; + + cloud.reserve(static_cast((xSteps + 1) * (ySteps + 1) * (zSteps + 1))); + + const float minX = center.x() - size.x() * 0.5f; + const float minY = center.y() - size.y() * 0.5f; + const float minZ = center.z() - size.z() * 0.5f; + + for (int ix = 0; ix <= xSteps; ++ix) { + const bool onXBoundary = (ix == 0 || ix == xSteps); + const float x = minX + stepX * ix; + for (int iy = 0; iy <= ySteps; ++iy) { + const bool onYBoundary = (iy == 0 || iy == ySteps); + const float y = minY + stepY * iy; + for (int iz = 0; iz <= zSteps; ++iz) { + const bool onZBoundary = (iz == 0 || iz == zSteps); + if (!(onXBoundary || onYBoundary || onZBoundary)) { + continue; + } + + const float z = minZ + stepZ * iz; + cloud.push_back(Point3D(x, y, z)); + } + } + } + + return cloud; +} +} // namespace + +CloudViewMainWindow::CloudViewMainWindow(QWidget* parent) + : QMainWindow(parent) + , m_glWidget(nullptr) + , m_converter(std::make_unique()) + , m_cloudCount(0) + , m_currentLineNum(0) + , m_currentLinePtNum(0) + , m_linePointsDialog(nullptr) + , m_linePointsTable(nullptr) +{ + setupUI(); + LOG_INFO("CloudViewMainWindow initialized\n"); +} + +CloudViewMainWindow::~CloudViewMainWindow() +{ +} + +bool CloudViewMainWindow::eventFilter(QObject* obj, QEvent* event) +{ + if (obj == m_glWidget && event->type() == QEvent::Resize) { + auto* resizeEvt = static_cast(event); + int w = resizeEvt->size().width(); + int h = resizeEvt->size().height(); + int btnW = m_btnZoomIn->width(); + int btnH = m_btnZoomIn->height(); + int margin = 8; + int gap = 4; + int x = w - btnW - margin; + int y = (h - btnH * 2 - gap) / 2; + m_btnZoomIn->move(x, y); + m_btnZoomOut->move(x, y + btnH + gap); + } + return QMainWindow::eventFilter(obj, event); +} + +void CloudViewMainWindow::setupUI() +{ + QMenu* cloudMenu = menuBar()->addMenu("点云"); + QAction* actGenerateCloud = cloudMenu->addAction("生成长方体点云..."); + connect(actGenerateCloud, &QAction::triggered, this, &CloudViewMainWindow::onGeneratePointCloud); + QAction* actRotateByZ = cloudMenu->addAction("点云旋转..."); + connect(actRotateByZ, &QAction::triggered, this, &CloudViewMainWindow::onRotateCloudByZ); + QAction* actSaveCloud = cloudMenu->addAction("保存点云..."); + connect(actSaveCloud, &QAction::triggered, this, &CloudViewMainWindow::onSavePointCloud); + + QMenu* toolMenu = menuBar()->addMenu("工具"); + QAction* actEulerPose = toolMenu->addAction("欧拉角与方向向量矩阵互转..."); + connect(actEulerPose, &QAction::triggered, this, &CloudViewMainWindow::onConvertEulerMatrix); + // 创建中央控件 + QWidget* centralWidget = new QWidget(this); + setCentralWidget(centralWidget); + + // 创建主布局 + QHBoxLayout* mainLayout = new QHBoxLayout(centralWidget); + mainLayout->setContentsMargins(5, 5, 5, 5); + mainLayout->setSpacing(5); + + // 创建分割器 + QSplitter* splitter = new QSplitter(Qt::Horizontal, centralWidget); + + // 左侧：视图工具栏 + 点云显示区域 + QWidget* viewerContainer = new QWidget(splitter); + QHBoxLayout* viewerLayout = new QHBoxLayout(viewerContainer); + viewerLayout->setContentsMargins(0, 0, 0, 0); + viewerLayout->setSpacing(2); + + // 视图方向工具栏（纵向排列，在3D视图左侧） + QWidget* viewToolbar = createViewToolbar(); + viewerLayout->addWidget(viewToolbar); + + // 点云显示区域 + QWidget* viewerArea = createViewerArea(); + viewerLayout->addWidget(viewerArea); + + splitter->addWidget(viewerContainer); + + // 右侧：控制面板 + QWidget* controlPanel = createControlPanel(); + splitter->addWidget(controlPanel); + + // 设置分割器初始大小（左侧 70%，右侧 30%） + splitter->setSizes({700, 300}); + + mainLayout->addWidget(splitter); + + // 状态栏 + statusBar()->showMessage("就绪"); +} + +QWidget* CloudViewMainWindow::createViewerArea() +{ + QWidget* widget = new QWidget(this); + QVBoxLayout* layout = new QVBoxLayout(widget); + layout->setContentsMargins(0, 0, 0, 0); + + m_glWidget = new PointCloudGLWidget(widget); + m_glWidget->setMinimumSize(400, 300); + m_glWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + layout->addWidget(m_glWidget); + + // 悬浮缩放按钮（父控件为 m_glWidget，悬浮在右侧中间） + int zoomBtnSize = 28; + QString zoomBtnStyle = + "QToolButton {" + " background-color: rgba(50, 50, 50, 180);" + " color: white;" + " border: 1px solid rgba(120, 120, 120, 150);" + " border-radius: 4px;" + " font-size: 32px;" + " font-weight: bold;" + "}" + "QToolButton:hover {" + " background-color: rgba(80, 80, 80, 220);" + "}" + "QToolButton:pressed {" + " background-color: rgba(30, 30, 30, 240);" + "}"; + + m_btnZoomIn = new QToolButton(m_glWidget); + m_btnZoomIn->setText("+"); + m_btnZoomIn->setFixedSize(zoomBtnSize, zoomBtnSize); + m_btnZoomIn->setStyleSheet(zoomBtnStyle); + m_btnZoomIn->setAutoRepeat(true); + m_btnZoomIn->setAutoRepeatDelay(300); + m_btnZoomIn->setAutoRepeatInterval(50); + connect(m_btnZoomIn, &QToolButton::clicked, m_glWidget, &PointCloudGLWidget::zoomIn); + + m_btnZoomOut = new QToolButton(m_glWidget); + m_btnZoomOut->setText("-"); + m_btnZoomOut->setFixedSize(zoomBtnSize, zoomBtnSize); + m_btnZoomOut->setStyleSheet(zoomBtnStyle); + m_btnZoomOut->setAutoRepeat(true); + m_btnZoomOut->setAutoRepeatDelay(300); + m_btnZoomOut->setAutoRepeatInterval(50); + connect(m_btnZoomOut, &QToolButton::clicked, m_glWidget, &PointCloudGLWidget::zoomOut); + + // 安装事件过滤器，在 GL 控件 resize 时更新按钮位置 + m_glWidget->installEventFilter(this); + + // 连接信号 + connect(m_glWidget, &PointCloudGLWidget::pointSelected, + this, &CloudViewMainWindow::onPointSelected); + connect(m_glWidget, &PointCloudGLWidget::twoPointsSelected, + this, &CloudViewMainWindow::onTwoPointsSelected); + connect(m_glWidget, &PointCloudGLWidget::lineSelected, + this, &CloudViewMainWindow::onLineSelected); + connect(m_glWidget, &PointCloudGLWidget::viewAnglesChanged, + this, &CloudViewMainWindow::onViewAnglesChanged); + + return widget; +} + +QWidget* CloudViewMainWindow::createControlPanel() +{ + QWidget* widget = new QWidget(this); + widget->setMaximumWidth(350); + QVBoxLayout* layout = new QVBoxLayout(widget); + layout->setContentsMargins(5, 5, 5, 5); + layout->setSpacing(5); + + // 文件操作组 + layout->addWidget(createFileGroup()); + + // 创建 Tab 控件 + QTabWidget* tabWidget = new QTabWidget(widget); + tabWidget->addTab(createMeasurePage(), "选点测距"); + tabWidget->addTab(createLinePage(), "选线"); + tabWidget->addTab(createTransformPage(), "矩阵变换"); + layout->addWidget(tabWidget); + + // 点云列表组 + layout->addWidget(createCloudListGroup()); + + // 添加弹性空间 + layout->addStretch(); + + return widget; +} + +QGroupBox* CloudViewMainWindow::createFileGroup() +{ + QGroupBox* group = new QGroupBox("文件操作", this); + group->setMaximumWidth(350); + QVBoxLayout* layout = new QVBoxLayout(group); + layout->setSpacing(3); + layout->setContentsMargins(5, 5, 5, 5); + + m_btnOpenFile = new QPushButton("打开文件", group); + m_btnOpenFile->setMinimumHeight(24); + m_btnOpenFile->setMaximumHeight(24); + connect(m_btnOpenFile, &QPushButton::clicked, this, &CloudViewMainWindow::onOpenFile); + layout->addWidget(m_btnOpenFile); + + m_btnOpenPose = new QPushButton("打开姿态点 {x,y,z}-{r,p,y}", group); + m_btnOpenPose->setMinimumHeight(24); + m_btnOpenPose->setMaximumHeight(24); + connect(m_btnOpenPose, &QPushButton::clicked, this, &CloudViewMainWindow::onOpenPoseFile); + layout->addWidget(m_btnOpenPose); + + m_btnOpenBBox = new QPushButton("加载包围盒 (grasp.json)", group); + m_btnOpenBBox->setMinimumHeight(24); + m_btnOpenBBox->setMaximumHeight(24); + connect(m_btnOpenBBox, &QPushButton::clicked, this, &CloudViewMainWindow::onOpenBBoxFile); + layout->addWidget(m_btnOpenBBox); + + m_btnClearAll = new QPushButton("清除所有", group); + m_btnClearAll->setMinimumHeight(24); + m_btnClearAll->setMaximumHeight(24); + connect(m_btnClearAll, &QPushButton::clicked, this, &CloudViewMainWindow::onClearAll); + layout->addWidget(m_btnClearAll); + + return group; +} + +QWidget* CloudViewMainWindow::createViewToolbar() +{ + QWidget* widget = new QWidget(this); + QVBoxLayout* layout = new QVBoxLayout(widget); + layout->setContentsMargins(2, 2, 2, 2); + layout->setSpacing(3); + + // 视图预设：图标、提示、旋转角度 + struct ViewPreset { + const char* iconPath; + const char* tooltip; + float rotX; + float rotY; + float rotZ; + }; + + // 坐标系定义：X向右，Y向下，Z朝后 + ViewPreset presets[] = { + {":/common/resource/view_front.png", "正视 (XY面)", 180.0f, 0.0f, 0.0f}, + {":/common/resource/view_back.png", "后视", 180.0f, 180.0f, 0.0f}, + {":/common/resource/view_left.png", "左视 (YZ面)", 180.0f, 90.0f, 0.0f}, + {":/common/resource/view_right.png", "右视", 180.0f, -90.0f, 0.0f}, + {":/common/resource/view_top.png", "俯视 (XZ面)", 90.0f, 0.0f, 0.0f}, + {":/common/resource/view_bottom.png", "仰视", -90.0f, 0.0f, 0.0f}, + {":/common/resource/view_robot.png", "机械臂", -90.0f, 90.0f, 0.0f}, + }; + + int btnSize = 32; + int iconSize = 24; + + for (const auto& preset : presets) { + QToolButton* btn = new QToolButton(widget); + btn->setIcon(QIcon(preset.iconPath)); + btn->setIconSize(QSize(iconSize, iconSize)); + btn->setFixedSize(btnSize, btnSize); + btn->setToolTip(preset.tooltip); + btn->setAutoRaise(true); + + float rx = preset.rotX; + float ry = preset.rotY; + float rz = preset.rotZ; + connect(btn, &QToolButton::clicked, this, [this, rx, ry, rz]() { + m_glWidget->setViewAngles(rx, ry, rz); + m_editRotX->setText(QString::number(rx, 'f', 1)); + m_editRotY->setText(QString::number(ry, 'f', 1)); + m_editRotZ->setText(QString::number(rz, 'f', 1)); + }); + layout->addWidget(btn); + } + + layout->addStretch(); + + // 旋转角度输入（纵向紧凑布局） + m_editRotX = new QLineEdit("180.0", widget); + m_editRotY = new QLineEdit("0.0", widget); + m_editRotZ = new QLineEdit("0.0", widget); + + auto makeAngleRow = [&](const QString& label, QLineEdit* edit) { + QWidget* row = new QWidget(widget); + QVBoxLayout* rowLayout = new QVBoxLayout(row); + rowLayout->setContentsMargins(0, 0, 0, 0); + rowLayout->setSpacing(0); + QLabel* lbl = new QLabel(label, row); + lbl->setStyleSheet("font-size: 10px; color: gray;"); + rowLayout->addWidget(lbl); + edit->setFixedWidth(btnSize); + edit->setMaximumHeight(20); + edit->setStyleSheet("font-size: 10px;"); + rowLayout->addWidget(edit); + return row; + }; + + layout->addWidget(makeAngleRow("RX", m_editRotX)); + layout->addWidget(makeAngleRow("RY", m_editRotY)); + layout->addWidget(makeAngleRow("RZ", m_editRotZ)); + + // 应用按钮 + QPushButton* btnApply = new QPushButton("GO", widget); + btnApply->setFixedSize(btnSize, 20); + btnApply->setStyleSheet("font-size: 10px;"); + auto applyAngles = [this]() { + bool okX, okY, okZ; + float rotX = m_editRotX->text().toFloat(&okX); + float rotY = m_editRotY->text().toFloat(&okY); + float rotZ = m_editRotZ->text().toFloat(&okZ); + if (okX && okY && okZ) { + m_glWidget->setViewAngles(rotX, rotY, rotZ); + } + }; + connect(btnApply, &QPushButton::clicked, this, applyAngles); + connect(m_editRotX, &QLineEdit::returnPressed, this, applyAngles); + connect(m_editRotY, &QLineEdit::returnPressed, this, applyAngles); + connect(m_editRotZ, &QLineEdit::returnPressed, this, applyAngles); + layout->addWidget(btnApply); + + widget->setFixedWidth(btnSize + 8); + return widget; +} + +QWidget* CloudViewMainWindow::createMeasurePage() +{ + QWidget* page = new QWidget(this); + QVBoxLayout* layout = new QVBoxLayout(page); + layout->setContentsMargins(0, 5, 0, 0); + layout->addWidget(createMeasureGroup()); + layout->addStretch(); + return page; +} + +QWidget* CloudViewMainWindow::createLinePage() +{ + QWidget* page = new QWidget(this); + QVBoxLayout* layout = new QVBoxLayout(page); + layout->setContentsMargins(0, 5, 0, 0); + + // 选线拟合组 + layout->addWidget(createLineGroup()); + + // 输入线段组 + QGroupBox* inputLineGroup = new QGroupBox("输入线段", page); + QVBoxLayout* inputLayout = new QVBoxLayout(inputLineGroup); + + // 提示 + QLabel* lblTip = new QLabel("输入两点坐标显示线段", inputLineGroup); + lblTip->setStyleSheet("color: gray; font-size: 10px;"); + inputLayout->addWidget(lblTip); + + // 点1坐标 + QLabel* lblPoint1 = new QLabel("点1:", inputLineGroup); + lblPoint1->setStyleSheet("font-weight: bold;"); + inputLayout->addWidget(lblPoint1); + + QHBoxLayout* p1Layout = new QHBoxLayout(); + p1Layout->addWidget(new QLabel("X:", inputLineGroup)); + m_editLineX1 = new QLineEdit("0.0", inputLineGroup); + m_editLineX1->setMaximumWidth(70); + p1Layout->addWidget(m_editLineX1); + + p1Layout->addWidget(new QLabel("Y:", inputLineGroup)); + m_editLineY1 = new QLineEdit("0.0", inputLineGroup); + m_editLineY1->setMaximumWidth(70); + p1Layout->addWidget(m_editLineY1); + + p1Layout->addWidget(new QLabel("Z:", inputLineGroup)); + m_editLineZ1 = new QLineEdit("0.0", inputLineGroup); + m_editLineZ1->setMaximumWidth(70); + p1Layout->addWidget(m_editLineZ1); + inputLayout->addLayout(p1Layout); + + // 点2坐标 + QLabel* lblPoint2 = new QLabel("点2:", inputLineGroup); + lblPoint2->setStyleSheet("font-weight: bold;"); + inputLayout->addWidget(lblPoint2); + + QHBoxLayout* p2Layout = new QHBoxLayout(); + p2Layout->addWidget(new QLabel("X:", inputLineGroup)); + m_editLineX2 = new QLineEdit("100.0", inputLineGroup); + m_editLineX2->setMaximumWidth(70); + p2Layout->addWidget(m_editLineX2); + + p2Layout->addWidget(new QLabel("Y:", inputLineGroup)); + m_editLineY2 = new QLineEdit("100.0", inputLineGroup); + m_editLineY2->setMaximumWidth(70); + p2Layout->addWidget(m_editLineY2); + + p2Layout->addWidget(new QLabel("Z:", inputLineGroup)); + m_editLineZ2 = new QLineEdit("100.0", inputLineGroup); + m_editLineZ2->setMaximumWidth(70); + p2Layout->addWidget(m_editLineZ2); + inputLayout->addLayout(p2Layout); + + // 按钮 + QHBoxLayout* btnLayout = new QHBoxLayout(); + m_btnShowLine = new QPushButton("显示线段", inputLineGroup); + connect(m_btnShowLine, &QPushButton::clicked, this, &CloudViewMainWindow::onShowInputLine); + btnLayout->addWidget(m_btnShowLine); + + m_btnClearLine2 = new QPushButton("清除线段", inputLineGroup); + connect(m_btnClearLine2, &QPushButton::clicked, this, &CloudViewMainWindow::onClearInputLine); + btnLayout->addWidget(m_btnClearLine2); + inputLayout->addLayout(btnLayout); + + // 连接输入线段坐标框的回车信号 + connect(m_editLineX1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); + connect(m_editLineY1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); + connect(m_editLineZ1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); + connect(m_editLineX2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); + connect(m_editLineY2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); + connect(m_editLineZ2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowInputLine); + + layout->addWidget(inputLineGroup); + layout->addStretch(); + return page; +} + +QGroupBox* CloudViewMainWindow::createMeasureGroup() +{ + QGroupBox* group = new QGroupBox("选点测距", this); + group->setMaximumWidth(350); + QVBoxLayout* layout = new QVBoxLayout(group); + layout->setSpacing(4); + layout->setContentsMargins(5, 5, 5, 5); + + // 操作说明 + QLabel* lblTip = new QLabel("Ctrl+左键点击点云选择点", group); + lblTip->setWordWrap(true); + lblTip->setStyleSheet("color: gray; font-size: 12px;"); + layout->addWidget(lblTip); + // 测距复选框 + m_cbMeasureDistance = new QCheckBox("启用测距", group); + m_cbMeasureDistance->setChecked(false); + connect(m_cbMeasureDistance, &QCheckBox::toggled, this, [this](bool checked) { + m_glWidget->setMeasureDistanceEnabled(checked); + m_glWidget->clearSelectedPoints(); + m_lblPoint1->setText("点1: --"); + m_lblPoint2->setText("点2: --"); + m_lblDistance->setText("--"); + m_editPoint1X->setText("--"); + m_editPoint1Y->setText("--"); + m_editPoint1Z->setText("--"); + m_editPoint2X->setText("--"); + m_editPoint2Y->setText("--"); + m_editPoint2Z->setText("--"); + }); + layout->addWidget(m_cbMeasureDistance); + + // 清除选点按钮 + m_btnClearPoints = new QPushButton("清除选点", group); + m_btnClearPoints->setMinimumHeight(24); + connect(m_btnClearPoints, &QPushButton::clicked, this, &CloudViewMainWindow::onClearSelectedPoints); + layout->addWidget(m_btnClearPoints); + + // 分隔线 + QFrame* line1 = new QFrame(group); + line1->setFrameShape(QFrame::HLine); + line1->setFrameShadow(QFrame::Sunken); + layout->addWidget(line1); + + // 点1信息（坐标直接显示在标题后） + m_lblPoint1 = new QLabel("点1: --", group); + m_lblPoint1->setWordWrap(true); + m_lblPoint1->setStyleSheet("font-weight: bold; font-size: 11px;"); + layout->addWidget(m_lblPoint1); + + // 点1坐标编辑（可修改） + QHBoxLayout* coord1Layout = new QHBoxLayout(); + coord1Layout->setSpacing(5); + coord1Layout->addWidget(new QLabel("X:", group)); + m_editPoint1X = new QLineEdit("--", group); + m_editPoint1X->setMaximumWidth(70); + m_editPoint1X->setMaximumHeight(24); + coord1Layout->addWidget(m_editPoint1X); + + coord1Layout->addWidget(new QLabel("Y:", group)); + m_editPoint1Y = new QLineEdit("--", group); + m_editPoint1Y->setMaximumWidth(70); + m_editPoint1Y->setMaximumHeight(24); + coord1Layout->addWidget(m_editPoint1Y); + + coord1Layout->addWidget(new QLabel("Z:", group)); + m_editPoint1Z = new QLineEdit("--", group); + m_editPoint1Z->setMaximumWidth(70); + m_editPoint1Z->setMaximumHeight(24); + coord1Layout->addWidget(m_editPoint1Z); + coord1Layout->addStretch(); + layout->addLayout(coord1Layout); + + // 连接回车信号 + connect(m_editPoint1X, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint1CoordChanged); + connect(m_editPoint1Y, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint1CoordChanged); + connect(m_editPoint1Z, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint1CoordChanged); + + // 点1姿态输入（紧凑布局） + QHBoxLayout* pose1Layout = new QHBoxLayout(); + pose1Layout->setSpacing(5); + pose1Layout->addWidget(new QLabel("RX:", group)); + m_editRx1 = new QLineEdit("0.0", group); + m_editRx1->setMaximumWidth(50); + pose1Layout->addWidget(m_editRx1); + + pose1Layout->addWidget(new QLabel("RY:", group)); + m_editRy1 = new QLineEdit("0.0", group); + m_editRy1->setMaximumWidth(50); + pose1Layout->addWidget(m_editRy1); + + pose1Layout->addWidget(new QLabel("RZ:", group)); + m_editRz1 = new QLineEdit("0.0", group); + m_editRz1->setMaximumWidth(50); + pose1Layout->addWidget(m_editRz1); + pose1Layout->addStretch(); + layout->addLayout(pose1Layout); + + m_btnShowPose1 = new QPushButton("显示点1姿态", group); + m_btnShowPose1->setMinimumHeight(24); + connect(m_btnShowPose1, &QPushButton::clicked, this, &CloudViewMainWindow::onShowPose1); + layout->addWidget(m_btnShowPose1); + + // 连接姿态输入框的回车信号 + connect(m_editRx1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose1); + connect(m_editRy1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose1); + connect(m_editRz1, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose1); + + // 分隔线 + QFrame* line2 = new QFrame(group); + line2->setFrameShape(QFrame::HLine); + line2->setFrameShadow(QFrame::Sunken); + layout->addWidget(line2); + + // 点2信息（坐标直接显示在标题后） + m_lblPoint2 = new QLabel("点2: --", group); + m_lblPoint2->setWordWrap(true); + m_lblPoint2->setStyleSheet("font-weight: bold; font-size: 11px;"); + layout->addWidget(m_lblPoint2); + + // 点2坐标编辑（可修改） + QHBoxLayout* coord2Layout = new QHBoxLayout(); + coord2Layout->setSpacing(5); + coord2Layout->addWidget(new QLabel("X:", group)); + m_editPoint2X = new QLineEdit("--", group); + m_editPoint2X->setMaximumWidth(70); + m_editPoint2X->setMaximumHeight(24); + coord2Layout->addWidget(m_editPoint2X); + + coord2Layout->addWidget(new QLabel("Y:", group)); + m_editPoint2Y = new QLineEdit("--", group); + m_editPoint2Y->setMaximumWidth(70); + m_editPoint2Y->setMaximumHeight(24); + coord2Layout->addWidget(m_editPoint2Y); + + coord2Layout->addWidget(new QLabel("Z:", group)); + m_editPoint2Z = new QLineEdit("--", group); + m_editPoint2Z->setMaximumWidth(70); + m_editPoint2Z->setMaximumHeight(24); + coord2Layout->addWidget(m_editPoint2Z); + coord2Layout->addStretch(); + layout->addLayout(coord2Layout); + + // 连接回车信号 + connect(m_editPoint2X, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint2CoordChanged); + connect(m_editPoint2Y, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint2CoordChanged); + connect(m_editPoint2Z, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onPoint2CoordChanged); + + // 点2姿态输入（紧凑布局） + QHBoxLayout* pose2Layout = new QHBoxLayout(); + pose2Layout->setSpacing(5); + pose2Layout->addWidget(new QLabel("RX:", group)); + m_editRx2 = new QLineEdit("0.0", group); + m_editRx2->setMaximumWidth(50); + pose2Layout->addWidget(m_editRx2); + + pose2Layout->addWidget(new QLabel("RY:", group)); + m_editRy2 = new QLineEdit("0.0", group); + m_editRy2->setMaximumWidth(50); + pose2Layout->addWidget(m_editRy2); + + pose2Layout->addWidget(new QLabel("RZ:", group)); + m_editRz2 = new QLineEdit("0.0", group); + m_editRz2->setMaximumWidth(50); + pose2Layout->addWidget(m_editRz2); + pose2Layout->addStretch(); + layout->addLayout(pose2Layout); + + m_btnShowPose2 = new QPushButton("显示点2姿态", group); + m_btnShowPose2->setMinimumHeight(24); + connect(m_btnShowPose2, &QPushButton::clicked, this, &CloudViewMainWindow::onShowPose2); + layout->addWidget(m_btnShowPose2); + + // 连接姿态输入框的回车信号 + connect(m_editRx2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose2); + connect(m_editRy2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose2); + connect(m_editRz2, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onShowPose2); + + // 姿态坐标轴缩放输入 + QHBoxLayout* scaleLayout = new QHBoxLayout(); + scaleLayout->setSpacing(5); + scaleLayout->addWidget(new QLabel("坐标轴长度:", group)); + m_editPoseScale = new QLineEdit("10.0", group); + m_editPoseScale->setMaximumWidth(60); + scaleLayout->addWidget(m_editPoseScale); + scaleLayout->addStretch(); + layout->addLayout(scaleLayout); + + // 分隔线 + QFrame* line3 = new QFrame(group); + line3->setFrameShape(QFrame::HLine); + line3->setFrameShadow(QFrame::Sunken); + layout->addWidget(line3); + + // 欧拉角旋转顺序选择 + QLabel* lblEulerOrder = new QLabel("欧拉角旋转顺序:", group); + lblEulerOrder->setStyleSheet("font-weight: bold; font-size: 10px;"); + layout->addWidget(lblEulerOrder); + + m_comboEulerOrder = new QComboBox(group); + m_comboEulerOrder->addItem("ZYX (Yaw-Pitch-Roll)", static_cast(EulerRotationOrder::ZYX)); + m_comboEulerOrder->addItem("XYZ (Roll-Pitch-Yaw)", static_cast(EulerRotationOrder::XYZ)); + m_comboEulerOrder->addItem("ZXY (Yaw-Roll-Pitch)", static_cast(EulerRotationOrder::ZXY)); + m_comboEulerOrder->addItem("YXZ (Pitch-Roll-Yaw)", static_cast(EulerRotationOrder::YXZ)); + m_comboEulerOrder->addItem("XZY (Roll-Yaw-Pitch)", static_cast(EulerRotationOrder::XZY)); + m_comboEulerOrder->addItem("YZX (Pitch-Yaw-Roll)", static_cast(EulerRotationOrder::YZX)); + m_comboEulerOrder->setCurrentIndex(0); + m_comboEulerOrder->setMaximumHeight(24); + connect(m_comboEulerOrder, QOverload::of(&QComboBox::currentIndexChanged), + this, &CloudViewMainWindow::onEulerOrderChanged); + layout->addWidget(m_comboEulerOrder); + + // 分隔线 + QFrame* line4 = new QFrame(group); + line4->setFrameShape(QFrame::HLine); + line4->setFrameShadow(QFrame::Sunken); + layout->addWidget(line4); + + // 距离信息 + QHBoxLayout* distLayout = new QHBoxLayout(); + QLabel* lblDistTitle = new QLabel("距离:", group); + lblDistTitle->setStyleSheet("font-size: 10px;"); + m_lblDistance = new QLabel("--", group); + m_lblDistance->setStyleSheet("font-weight: bold; color: green; font-size: 10px;"); + distLayout->addWidget(lblDistTitle); + distLayout->addWidget(m_lblDistance, 1); + layout->addLayout(distLayout); + + return group; +} + +QGroupBox* CloudViewMainWindow::createLineGroup() +{ + QGroupBox* group = new QGroupBox("选线", this); + group->setMaximumWidth(400); + QVBoxLayout* layout = new QVBoxLayout(group); + layout->setSpacing(3); + layout->setContentsMargins(5, 5, 5, 5); + + // 操作说明 + QLabel* lblTip = new QLabel("Shift+左键点击点云选择线", group); + lblTip->setWordWrap(true); + lblTip->setStyleSheet("color: gray; font-size: 9px;"); + layout->addWidget(lblTip); + + // 选线模式选择 + QHBoxLayout* modeLayout = new QHBoxLayout(); + modeLayout->setSpacing(5); + m_rbVertical = new QRadioButton("纵向", group); + m_rbHorizontal = new QRadioButton("横向", group); + m_rbVertical->setChecked(true); + connect(m_rbVertical, &QRadioButton::toggled, this, &CloudViewMainWindow::onLineSelectModeChanged); + modeLayout->addWidget(m_rbVertical); + modeLayout->addWidget(m_rbHorizontal); + modeLayout->addStretch(); + layout->addLayout(modeLayout); + + // 输入索引选择 + QHBoxLayout* inputLayout = new QHBoxLayout(); + inputLayout->setSpacing(5); + m_lineNumberInput = new QLineEdit(group); + m_lineNumberInput->setPlaceholderText("输入索引"); + m_lineNumberInput->setMaximumHeight(24); + m_btnSelectByNumber = new QPushButton("选择", group); + m_btnSelectByNumber->setMaximumHeight(24); + m_btnSelectByNumber->setMaximumWidth(50); + connect(m_btnSelectByNumber, &QPushButton::clicked, this, &CloudViewMainWindow::onSelectLineByNumber); + connect(m_lineNumberInput, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onSelectLineByNumber); + inputLayout->addWidget(m_lineNumberInput, 1); + inputLayout->addWidget(m_btnSelectByNumber); + layout->addLayout(inputLayout); + + // 清除选线按钮 + m_btnClearLine = new QPushButton("清除选线", group); + m_btnClearLine->setMinimumHeight(24); + connect(m_btnClearLine, &QPushButton::clicked, this, &CloudViewMainWindow::onClearLinePoints); + layout->addWidget(m_btnClearLine); + + // 显示线上点按钮 + m_btnShowLinePoints = new QPushButton("显示线上点", group); + m_btnShowLinePoints->setMinimumHeight(24); + connect(m_btnShowLinePoints, &QPushButton::clicked, this, &CloudViewMainWindow::onShowLinePoints); + layout->addWidget(m_btnShowLinePoints); + + // 线索引信息 + QHBoxLayout* indexLayout = new QHBoxLayout(); + QLabel* lblIndexTitle = new QLabel("索引:", group); + lblIndexTitle->setStyleSheet("font-size: 10px;"); + m_lblLineIndex = new QLabel("--", group); + m_lblLineIndex->setStyleSheet("font-size: 10px;"); + indexLayout->addWidget(lblIndexTitle); + indexLayout->addWidget(m_lblLineIndex, 1); + layout->addLayout(indexLayout); + + // 点数信息 + QHBoxLayout* countLayout = new QHBoxLayout(); + QLabel* lblCountTitle = new QLabel("点数:", group); + lblCountTitle->setStyleSheet("font-size: 10px;"); + m_lblLinePointCount = new QLabel("--", group); + m_lblLinePointCount->setStyleSheet("font-size: 10px;"); + countLayout->addWidget(lblCountTitle); + countLayout->addWidget(m_lblLinePointCount, 1); + layout->addLayout(countLayout); + + return group; +} + +QGroupBox* CloudViewMainWindow::createCloudListGroup() +{ + QGroupBox* group = new QGroupBox("点云列表", this); + group->setMaximumWidth(350); + QVBoxLayout* layout = new QVBoxLayout(group); + layout->setSpacing(3); + layout->setContentsMargins(5, 5, 5, 5); + + m_cloudList = new QListWidget(group); + m_cloudList->setMinimumHeight(70); + layout->addWidget(m_cloudList); + + return group; +} + +void CloudViewMainWindow::onOpenFile() +{ + // 如果已有打开的文件，提示是否清除 + if (m_cloudList->count() > 0) { + auto ret = QMessageBox::question(this, "提示", + "当前已有打开的文件，是否清除？", + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); + if (ret == QMessageBox::Cancel) { + return; + } + if (ret == QMessageBox::Yes) { + onClearAll(); + } + } + + QString fileName = QFileDialog::getOpenFileName( + this, + "打开文件", + QString(), + "所有支持格式 (*.pcd *.ply *.txt);;PCD 文件 (*.pcd);;PLY 文件 (*.ply);;TXT 文件 (*.txt);;所有文件 (*.*)" + ); + + if (fileName.isEmpty()) { + return; + } + + // 同一文件中可能同时包含点云和线段，两者都尝试加载 + bool cloudOk = loadPointCloudFile(fileName); + bool segmentOk = loadSegmentFile(fileName); + + if (!cloudOk && !segmentOk) { + QMessageBox::critical(this, "错误", "无法从文件中加载点云或线段数据"); + statusBar()->showMessage("加载失败"); + } +} + +bool CloudViewMainWindow::loadPointCloudFile(const QString& fileName) +{ + statusBar()->showMessage("正在加载点云..."); + + QFileInfo fileInfo(fileName); + QString ext = fileInfo.suffix().toLower(); + QString cloudName = QString("Cloud_%1 (%2)").arg(++m_cloudCount).arg(fileInfo.fileName()); + + // 统一使用 PointCloudXYZRGB 加载，支持带颜色和不带颜色的文件 + PointCloudXYZRGB rgbCloud; + int result = m_converter->loadFromFile(fileName.toStdString(), rgbCloud); + + if (result != 0) { + LOG_INFO("[CloudView] Load point cloud failed: %s\n", m_converter->getLastError().c_str()); + return false; + } + + // 保存原始完整点云 XYZ（用于旋转/线上点等功能） + m_originalCloud.clear(); + m_originalCloud.reserve(rgbCloud.size()); + for (size_t i = 0; i < rgbCloud.points.size(); ++i) { + const auto& pt = rgbCloud.points[i]; + Point3D xyzPt(pt.x, pt.y, pt.z); + int lineIdx = (i < rgbCloud.lineIndices.size()) ? rgbCloud.lineIndices[i] : 0; + m_originalCloud.push_back(xyzPt, lineIdx); + } + + // 根据是否有颜色选择显示方式 + bool hadColor = m_converter->lastLoadHadColor(); + + if (ext == "pcd" || ext == "ply") { + // PCD/PLY 文件始终走 XYZRGB 路径，避免被颜色轮换表染成蓝色等 + if (!hadColor) { + // 无 rgb 字段：默认灰色显示 + for (size_t i = 0; i < rgbCloud.points.size(); ++i) { + rgbCloud.points[i].r = 192; + rgbCloud.points[i].g = 192; + rgbCloud.points[i].b = 192; + } + } + m_glWidget->addPointCloud(rgbCloud, cloudName); + LOG_INFO("[CloudView] PCD/PLY loaded with XYZRGB path (hasRgbField=%d), points: %zu\n", + m_converter->lastLoadHadColor(), rgbCloud.size()); + } else if (hadColor) { + m_glWidget->addPointCloud(rgbCloud, cloudName); + LOG_INFO("[CloudView] Loaded with original color, points: %zu\n", rgbCloud.size()); + } else { + m_glWidget->addPointCloud(m_originalCloud, cloudName); + LOG_INFO("[CloudView] Loaded without color (color table), points: %zu\n", m_originalCloud.size()); + } + + // 保存线信息（用于旋转功能） + int lineCount = m_converter->getLoadedLineCount(); + if (lineCount > 0) { + m_currentLineNum = lineCount; + m_currentLinePtNum = static_cast(m_converter->getLoadedPointCount()) / lineCount; + } else { + m_currentLineNum = 0; + m_currentLinePtNum = 0; + } + + // 添加到列表 + QString itemText; + if (lineCount > 0) { + itemText = QString("%1 - %2 点, %3 线").arg(cloudName).arg(m_converter->getLoadedPointCount()).arg(lineCount); + } else { + itemText = QString("%1 - %2 点").arg(cloudName).arg(m_converter->getLoadedPointCount()); + } + if (hadColor) { + itemText += " [彩色]"; + } + m_cloudList->addItem(itemText); + + statusBar()->showMessage(QString("已加载 %1 个点%2").arg(m_converter->getLoadedPointCount()).arg(hadColor ? " (彩色)" : "")); + return true; +} + +bool CloudViewMainWindow::loadSegmentFile(const QString& fileName) +{ + statusBar()->showMessage("正在加载线段..."); + + LaserDataLoader loader; + std::vector> polyLines; + int result = loader.LoadPolySegments(fileName.toStdString(), polyLines); + + if (result != 0) { + LOG_INFO("[CloudView] Load segments failed: %s\n", loader.GetLastError().c_str()); + return false; + } + + if (polyLines.empty()) { + return false; + } + + // 将折线点转换为线段（使用点自带的颜色） + QVector segments; + for (const auto& polyLine : polyLines) { + for (size_t i = 0; i + 1 < polyLine.size(); ++i) { + const auto& p1 = polyLine[i]; + const auto& p2 = polyLine[i + 1]; + // 取起点颜色作为线段颜色，nRGB 为 (A<<24)|BGR 打包格式 + float r = ((p1.nRGB >> 0) & 0xFF) / 255.0f; + float g = ((p1.nRGB >> 8) & 0xFF) / 255.0f; + float b = ((p1.nRGB >> 16) & 0xFF) / 255.0f; + uint8_t a = static_cast((p1.nRGB >> 24) & 0xFF); + // A > 1 时作为线宽使用 + float lineWidth = (a > 1) ? static_cast(a) : 0.0f; + segments.append(LineSegment( + p1.x, p1.y, p1.z, + p2.x, p2.y, p2.z, + r, g, b, lineWidth)); + } + } + + if (segments.isEmpty()) { + return false; + } + + m_glWidget->addLineSegments(segments); + + // 添加到列表 + QFileInfo fileInfo(fileName); + int totalPolyCount = static_cast(polyLines.size()); + QString itemName = QString("Segments (%1)").arg(fileInfo.fileName()); + m_cloudList->addItem(QString("%1 - %2 条折线, %3 条线段") + .arg(itemName).arg(totalPolyCount).arg(segments.size())); + + statusBar()->showMessage(QString("已加载 %1 条折线, %2 条线段").arg(totalPolyCount).arg(segments.size())); + LOG_INFO("[CloudView] Loaded %d polylines, %d segments from %s\n", + totalPolyCount, segments.size(), fileName.toStdString().c_str()); + return true; +} + +void CloudViewMainWindow::onOpenPoseFile() +{ + QString fileName = QFileDialog::getOpenFileName( + this, + "打开姿态点文件", + QString(), + "文本文件 (*.txt);;所有文件 (*.*)" + ); + + if (fileName.isEmpty()) { + return; + } + + statusBar()->showMessage("正在加载姿态点..."); + + QFile file(fileName); + #if 0 + if (false) { + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + QMessageBox::critical(this, "错误", QString("无法打开文件: %1").arg(fileName)); + return; + } + + QTextStream in(&file); + const QString matrixText = in.readAll(); + file.close(); + + QMatrix4x4 matrix; + QString errorMessage; + if (!parseMatrixText(matrixText, matrix, &errorMessage)) { + QMessageBox::warning(this, "格式错误", errorMessage); + return; + } + + m_matrixEdit->setPlainText(formatMatrixText(matrix)); + statusBar()->showMessage(QString("已从 %1 加载矩阵").arg(QFileInfo(fileName).fileName())); + LOG_INFO("[CloudView] Loaded matrix from %s\n", fileName.toStdString().c_str()); + return; + } + #endif + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + QMessageBox::critical(this, "错误", QString("无法打开文件: %1").arg(fileName)); + statusBar()->showMessage("加载失败"); + return; + } + + QVector poses; + QTextStream in(&file); + int lineNum = 0; + int validCount = 0; + + while (!in.atEnd()) { + QString line = in.readLine().trimmed(); + lineNum++; + + // 跳过空行和注释 + if (line.isEmpty() || line.startsWith('#')) { + continue; + } + + // 解析格式：{x,y,z}-{r,p,y} + QRegExp regex("\\{([^}]+)\\}-\\{([^}]+)\\}"); + if (regex.indexIn(line) == -1) { + LOG_WARN("[CloudView] Line %d: Invalid format, expected {x,y,z}-{r,p,y}\n", lineNum); + continue; + } + + QString posStr = regex.cap(1); + QString rotStr = regex.cap(2); + + QStringList pos = posStr.split(','); + QStringList rot = rotStr.split(','); + + if (pos.size() != 3 || rot.size() != 3) { + LOG_WARN("[CloudView] Line %d: Invalid point format\n", lineNum); + continue; + } + + bool ok = true; + float x = pos[0].toFloat(&ok); if (!ok) continue; + float y = pos[1].toFloat(&ok); if (!ok) continue; + float z = pos[2].toFloat(&ok); if (!ok) continue; + float roll = rot[0].toFloat(&ok); if (!ok) continue; + float pitch = rot[1].toFloat(&ok); if (!ok) continue; + float yaw = rot[2].toFloat(&ok); if (!ok) continue; + + // 固定大小为10 + float scale = 10.0f; + + poses.append(PosePoint(x, y, z, roll, pitch, yaw, scale)); + validCount++; + } + + file.close(); + + if (poses.isEmpty()) { + QMessageBox::warning(this, "警告", "文件中没有有效的姿态点数据"); + statusBar()->showMessage("加载失败"); + return; + } + + m_glWidget->addPosePoints(poses); + statusBar()->showMessage(QString("已加载 %1 个姿态点").arg(validCount)); + LOG_INFO("[CloudView] Loaded %d pose points from %s\n", validCount, fileName.toStdString().c_str()); +} + +void CloudViewMainWindow::onOpenBBoxFile() +{ + QString fileName = QFileDialog::getOpenFileName( + this, + "打开包围盒文件", + QString(), + "JSON 文件 (*.json);;所有文件 (*.*)" + ); + + if (fileName.isEmpty()) { + return; + } + + if (!loadBoundingBoxFile(fileName)) { + QMessageBox::critical(this, "错误", "无法加载包围盒数据"); + statusBar()->showMessage("加载失败"); + } +} + +bool CloudViewMainWindow::loadBoundingBoxFile(const QString& fileName) +{ + QFile file(fileName); + if (!file.open(QIODevice::ReadOnly)) { + LOG_INFO("[CloudView] Cannot open bbox file: %s\n", fileName.toStdString().c_str()); + return false; + } + + QByteArray data = file.readAll(); + file.close(); + + QJsonParseError parseError; + QJsonDocument doc = QJsonDocument::fromJson(data, &parseError); + if (doc.isNull()) { + LOG_INFO("[CloudView] JSON parse error: %s\n", parseError.errorString().toStdString().c_str()); + return false; + } + + QJsonObject root = doc.object(); + QJsonArray records = root["records"].toArray(); + if (records.isEmpty()) { + LOG_INFO("[CloudView] No records in bbox file\n"); + return false; + } + + // 包围盒8个顶点的12条边索引 + // 底面: 0-1, 1-2, 2-3, 3-0 + // 顶面: 4-5, 5-6, 6-7, 7-4 + // 立柱: 0-4, 1-5, 2-6, 3-7 + static const int edgeIndices[][2] = { + {0, 1}, {1, 2}, {2, 3}, {3, 0}, // 底面 + {4, 5}, {5, 6}, {6, 7}, {7, 4}, // 顶面 + {0, 4}, {1, 5}, {2, 6}, {3, 7} // 立柱 + }; + + QVector segments; + QVector graspPoses; + int bboxCount = 0; + + // 为每个 record 使用不同的颜色 + const float colors[][3] = { + {0.0f, 1.0f, 0.0f}, // 绿色 + {1.0f, 1.0f, 0.0f}, // 黄色 + {0.0f, 1.0f, 1.0f}, // 青色 + {1.0f, 0.5f, 0.0f}, // 橙色 + {1.0f, 0.0f, 1.0f}, // 紫色 + {0.5f, 1.0f, 0.5f}, // 浅绿 + }; + const int colorCount = sizeof(colors) / sizeof(colors[0]); + + for (int i = 0; i < records.size(); ++i) { + QJsonObject record = records[i].toObject(); + QJsonObject bbox = record["bounding_box"].toObject(); + QJsonArray vertices = bbox["vertices"].toArray(); + + if (vertices.size() != 8) { + LOG_WARN("[CloudView] Record %d: vertices count != 8, skip\n", i); + continue; + } + + // 解析8个顶点 + float vx[8], vy[8], vz[8]; + for (int j = 0; j < 8; ++j) { + QJsonObject v = vertices[j].toObject(); + vx[j] = static_cast(v["x"].toDouble()); + vy[j] = static_cast(v["y"].toDouble()); + vz[j] = static_cast(v["z"].toDouble()); + } + + // 选择颜色 + float r = colors[i % colorCount][0]; + float g = colors[i % colorCount][1]; + float b = colors[i % colorCount][2]; + + // 生成12条边的线段 + for (const auto& edge : edgeIndices) { + int a = edge[0]; + int b_idx = edge[1]; + segments.append(LineSegment( + vx[a], vy[a], vz[a], + vx[b_idx], vy[b_idx], vz[b_idx], + r, g, b, 2.0f)); + } + + bboxCount++; + + // 解析抓取点（右抓取点） + if (record.contains("has_right_point") && record["has_right_point"].toBool()) { + QJsonObject graspPt = record["right_grasp_point"].toObject(); + QJsonObject pos = graspPt["position"].toObject(); + QJsonObject ori = graspPt["orientation"].toObject(); + + float px = static_cast(pos["x"].toDouble()); + float py = static_cast(pos["y"].toDouble()); + float pz = static_cast(pos["z"].toDouble()); + float roll = static_cast(ori["roll"].toDouble()); + float pitch = static_cast(ori["pitch"].toDouble()); + float yaw = static_cast(ori["yaw"].toDouble()); + + graspPoses.append(PosePoint(px, py, pz, roll, pitch, yaw, 15.0f)); + } + + // 解析左抓取点 + if (record.contains("has_left_point") && record["has_left_point"].toBool()) { + QJsonObject graspPt = record["left_grasp_point"].toObject(); + QJsonObject pos = graspPt["position"].toObject(); + QJsonObject ori = graspPt["orientation"].toObject(); + + float px = static_cast(pos["x"].toDouble()); + float py = static_cast(pos["y"].toDouble()); + float pz = static_cast(pos["z"].toDouble()); + float roll = static_cast(ori["roll"].toDouble()); + float pitch = static_cast(ori["pitch"].toDouble()); + float yaw = static_cast(ori["yaw"].toDouble()); + + graspPoses.append(PosePoint(px, py, pz, roll, pitch, yaw, 15.0f)); + } + } + + if (segments.isEmpty()) { + return false; + } + + m_glWidget->addLineSegments(segments); + + if (!graspPoses.isEmpty()) { + m_glWidget->addPosePoints(graspPoses); + } + + // 添加到列表 + QFileInfo fileInfo(fileName); + QString itemName = QString("BBox (%1)").arg(fileInfo.fileName()); + QString itemText = QString("%1 - %2 个包围盒, %3 条边") + .arg(itemName).arg(bboxCount).arg(segments.size()); + if (!graspPoses.isEmpty()) { + itemText += QString(", %1 个抓取点").arg(graspPoses.size()); + } + m_cloudList->addItem(itemText); + + statusBar()->showMessage(QString("已加载 %1 个包围盒").arg(bboxCount)); + LOG_INFO("[CloudView] Loaded %d bounding boxes, %d grasp points from %s\n", + bboxCount, graspPoses.size(), fileName.toStdString().c_str()); + return true; +} + +void CloudViewMainWindow::applyTransformToAllClouds(const QMatrix4x4& matrix) +{ + m_glWidget->transformAllClouds(matrix); + transformPointCloud(m_originalCloud, matrix); + + m_glWidget->clearSelectedPoints(); + m_glWidget->clearSelectedLine(); + m_glWidget->clearListHighlightPoint(); + m_glWidget->clearLineSegments(); + m_glWidget->clearPosePoints(); + + m_lblPoint1->setText("点1: --"); + m_lblPoint2->setText("点2: --"); + m_lblDistance->setText("--"); + m_editPoint1X->setText("--"); + m_editPoint1Y->setText("--"); + m_editPoint1Z->setText("--"); + m_editPoint2X->setText("--"); + m_editPoint2Y->setText("--"); + m_editPoint2Z->setText("--"); + m_lblLineIndex->setText("--"); + m_lblLinePointCount->setText("--"); + m_currentLinePoints.clear(); + + if (m_linePointsDialog && m_linePointsTable) { + updateLinePointsDialog(); + } +} + +void CloudViewMainWindow::addGeneratedCloud(const PointCloudXYZ& cloud, const QString& name) +{ + m_glWidget->clearSelectedPoints(); + m_glWidget->clearSelectedLine(); + m_glWidget->clearListHighlightPoint(); + + const QString cloudName = QString("Cloud_%1 (%2)").arg(++m_cloudCount).arg(name); + m_glWidget->addPointCloud(cloud, cloudName); + + m_originalCloud = cloud; + m_currentLineNum = 0; + m_currentLinePtNum = 0; + + m_cloudList->addItem(QString("%1 - %2 点 [生成]").arg(cloudName).arg(cloud.size())); + statusBar()->showMessage(QString("已生成点云: %1，点数 %2").arg(name).arg(cloud.size())); + LOG_INFO("[CloudView] Generated synthetic cloud: %s, points=%zu\n", + cloudName.toStdString().c_str(), cloud.size()); +} + +void CloudViewMainWindow::onClearAll() +{ + m_glWidget->clearPointClouds(); + m_glWidget->clearLineSegments(); + m_glWidget->clearPosePoints(); + m_cloudList->clear(); + m_cloudCount = 0; + m_currentLineNum = 0; + m_currentLinePtNum = 0; + m_originalCloud.clear(); + + // 清除选点信息 + m_lblPoint1->setText("点1: --"); + m_lblPoint2->setText("点2: --"); + m_lblDistance->setText("--"); + m_editPoint1X->setText("--"); + m_editPoint1Y->setText("--"); + m_editPoint1Z->setText("--"); + m_editPoint2X->setText("--"); + m_editPoint2Y->setText("--"); + m_editPoint2Z->setText("--"); + + // 清除选线信息 + m_lblLineIndex->setText("--"); + m_lblLinePointCount->setText("--"); + + statusBar()->showMessage("已清除所有数据"); +} + +void CloudViewMainWindow::onResetView() +{ + m_glWidget->resetView(); + statusBar()->showMessage("视图已重置"); +} + +void CloudViewMainWindow::onClearSelectedPoints() +{ + m_glWidget->clearSelectedPoints(); + m_glWidget->clearPosePoints(); // 清除选点时也清除姿态 + m_lblPoint1->setText("点1: --"); + m_lblPoint2->setText("点2: --"); + m_lblDistance->setText("--"); + m_editPoint1X->setText("--"); + m_editPoint1Y->setText("--"); + m_editPoint1Z->setText("--"); + m_editPoint2X->setText("--"); + m_editPoint2Y->setText("--"); + m_editPoint2Z->setText("--"); + statusBar()->showMessage("已清除选中的点"); +} + +void CloudViewMainWindow::onPointSelected(const SelectedPointInfo& point) +{ + if (!point.valid) { + return; + } + + // 选择新点时清除之前的姿态显示 + m_glWidget->clearPosePoints(); + + updateSelectedPointsDisplay(); + + // 状态栏显示：坐标、线号、索引号 + QString statusMsg = QString("选中点: (%1, %2, %3)") + .arg(point.x, 0, 'f', 3) + .arg(point.y, 0, 'f', 3) + .arg(point.z, 0, 'f', 3); + if (point.lineIndex >= 0) { + statusMsg += QString(" | 线号: %1").arg(point.lineIndex); + if (point.pointIndexInLine >= 0) { + statusMsg += QString(" | 索引号: %1").arg(point.pointIndexInLine); + } + } + statusBar()->showMessage(statusMsg); +} + +void CloudViewMainWindow::onTwoPointsSelected(const SelectedPointInfo& p1, const SelectedPointInfo& p2, float distance) +{ + updateSelectedPointsDisplay(); + m_lblDistance->setText(QString("%1 mm").arg(distance, 0, 'f', 3)); + statusBar()->showMessage(QString("测量距离: %1 mm").arg(distance, 0, 'f', 3)); +} + +void CloudViewMainWindow::updateSelectedPointsDisplay() +{ + auto selectedPoints = m_glWidget->getSelectedPoints(); + + if (selectedPoints.size() >= 1 && selectedPoints[0].valid) { + QString text; + if (selectedPoints[0].lineIndex >= 0) { + text = QString("点1: 线号:%1 点序:%2") + .arg(selectedPoints[0].lineIndex) + .arg(selectedPoints[0].pointIndexInLine); + } else { + text = QString("点1:"); + } + m_lblPoint1->setText(text); + + // 更新坐标编辑框 + m_editPoint1X->setText(QString::number(selectedPoints[0].x, 'f', 3)); + m_editPoint1Y->setText(QString::number(selectedPoints[0].y, 'f', 3)); + m_editPoint1Z->setText(QString::number(selectedPoints[0].z, 'f', 3)); + } else { + m_lblPoint1->setText("点1: --"); + m_editPoint1X->setText("--"); + m_editPoint1Y->setText("--"); + m_editPoint1Z->setText("--"); + } + + if (selectedPoints.size() >= 2 && selectedPoints[1].valid) { + QString text; + if (selectedPoints[1].lineIndex >= 0) { + text = QString("点2: 线号:%1 点序:%2") + .arg(selectedPoints[1].lineIndex) + .arg(selectedPoints[1].pointIndexInLine); + } else { + text = QString("点2:"); + } + m_lblPoint2->setText(text); + + // 更新坐标编辑框 + m_editPoint2X->setText(QString::number(selectedPoints[1].x, 'f', 3)); + m_editPoint2Y->setText(QString::number(selectedPoints[1].y, 'f', 3)); + m_editPoint2Z->setText(QString::number(selectedPoints[1].z, 'f', 3)); + } else { + m_lblPoint2->setText("点2: --"); + m_editPoint2X->setText("--"); + m_editPoint2Y->setText("--"); + m_editPoint2Z->setText("--"); + } +} + +void CloudViewMainWindow::onLineSelectModeChanged(bool checked) +{ + if (checked) { + // 纵向模式 + m_glWidget->setLineSelectMode(LineSelectMode::Vertical); + } else { + // 横向模式 + m_glWidget->setLineSelectMode(LineSelectMode::Horizontal); + } + m_lineNumberInput->setPlaceholderText("输入索引"); +} + +void CloudViewMainWindow::onClearLinePoints() +{ + m_glWidget->clearSelectedLine(); + m_glWidget->clearListHighlightPoint(); // 清除列表选中的高亮点 + m_lblLineIndex->setText("--"); + m_lblLinePointCount->setText("--"); + statusBar()->showMessage("已清除选线"); +} + +void CloudViewMainWindow::onLineSelected(const SelectedLineInfo& line) +{ + // 重新选线时清除列表高亮点 + m_glWidget->clearListHighlightPoint(); + + if (!line.valid) { + m_lblLineIndex->setText("--"); + m_lblLinePointCount->setText("--"); + return; + } + + // 状态栏显示：线号/索引号、线点数 + if (line.mode == LineSelectMode::Vertical) { + m_lblLineIndex->setText(QString::number(line.lineIndex)); + statusBar()->showMessage(QString("选中线 | 线号: %1 | 线点数: %2") + .arg(line.lineIndex) + .arg(line.pointCount)); + } else { + // 横向选线：显示索引号 + m_lblLineIndex->setText(QString::number(line.pointIndex)); + statusBar()->showMessage(QString("选中横向线 | 索引号: %1 | 线点数: %2") + .arg(line.pointIndex) + .arg(line.pointCount)); + } + m_lblLinePointCount->setText(QString::number(line.pointCount)); + + // 如果线上点对话框已打开，刷新内容 + updateLinePointsDialog(); +} + +void CloudViewMainWindow::onSelectLineByNumber() +{ + if (m_glWidget->getCloudCount() == 0) { + QMessageBox::warning(this, "提示", "请先加载点云"); + return; + } + + QString text = m_lineNumberInput->text().trimmed(); + if (text.isEmpty()) { + QMessageBox::warning(this, "提示", "请输入索引"); + return; + } + + bool ok; + int index = text.toInt(&ok); + if (!ok || index < 0) { + QMessageBox::warning(this, "提示", "请输入有效的索引（非负整数）"); + return; + } + + bool success = false; + if (m_rbVertical->isChecked()) { + // 纵向选线：直接使用索引 + success = m_glWidget->selectLineByIndex(index); + if (!success) { + QMessageBox::warning(this, "提示", QString("索引 %1 不存在").arg(index)); + } + } else { + // 横向选线：直接使用索引 + success = m_glWidget->selectHorizontalLineByIndex(index); + if (!success) { + QMessageBox::warning(this, "提示", QString("索引 %1 不存在").arg(index)); + } + } +} + +QVector CloudViewMainWindow::getOriginalLinePoints(const SelectedLineInfo& lineInfo) +{ + QVector points; + + if (!lineInfo.valid || m_originalCloud.empty()) { + return points; + } + + if (lineInfo.mode == LineSelectMode::Vertical) { + // 纵向选线：获取同一条扫描线上的所有点 + for (size_t i = 0; i < m_originalCloud.points.size(); ++i) { + if (i < m_originalCloud.lineIndices.size() && + m_originalCloud.lineIndices[i] == lineInfo.lineIndex) { + const auto& pt = m_originalCloud.points[i]; + points.append(QVector3D(pt.x, pt.y, pt.z)); + } + } + } else { + // 横向选线：获取所有线的相同索引点 + if (m_currentLinePtNum > 0 && lineInfo.pointIndex >= 0) { + for (size_t i = 0; i < m_originalCloud.points.size(); ++i) { + int originalIdx = static_cast(i); + if (originalIdx % m_currentLinePtNum == lineInfo.pointIndex) { + const auto& pt = m_originalCloud.points[i]; + points.append(QVector3D(pt.x, pt.y, pt.z)); + } + } + } + } + + return points; +} + +void CloudViewMainWindow::updateLinePointsDialog() +{ + if (!m_linePointsDialog || !m_linePointsTable) { + return; + } + + SelectedLineInfo lineInfo = m_glWidget->getSelectedLine(); + if (!lineInfo.valid) { + m_linePointsTable->setRowCount(0); + m_linePointsDialog->setWindowTitle("线上点坐标"); + m_currentLinePoints.clear(); + return; + } + + // 从原始数据获取线上点（包含0,0,0） + m_currentLinePoints = getOriginalLinePoints(lineInfo); + + // 更新标题 + QString title; + if (lineInfo.mode == LineSelectMode::Vertical) { + title = QString("线上点坐标 - 线号: %1 (共 %2 个点)") + .arg(lineInfo.lineIndex) + .arg(m_currentLinePoints.size()); + } else { + title = QString("线上点坐标 - 索引号: %1 (共 %2 个点)") + .arg(lineInfo.pointIndex) + .arg(m_currentLinePoints.size()); + } + m_linePointsDialog->setWindowTitle(title); + + // 更新表格 + m_linePointsTable->setRowCount(m_currentLinePoints.size()); + + // 斑马线颜色 + QColor evenColor(245, 245, 245); // 浅灰色 + QColor oddColor(255, 255, 255); // 白色 + + for (int i = 0; i < m_currentLinePoints.size(); ++i) { + const QVector3D& pt = m_currentLinePoints[i]; + QColor rowColor = (i % 2 == 0) ? evenColor : oddColor; + + // 序号 + QTableWidgetItem* indexItem = new QTableWidgetItem(QString::number(i)); + indexItem->setTextAlignment(Qt::AlignCenter); + indexItem->setBackground(rowColor); + indexItem->setFlags(indexItem->flags() & ~Qt::ItemIsEditable); + m_linePointsTable->setItem(i, 0, indexItem); + + // X + QTableWidgetItem* xItem = new QTableWidgetItem(QString::number(pt.x(), 'f', 3)); + xItem->setTextAlignment(Qt::AlignCenter); + xItem->setBackground(rowColor); + xItem->setFlags(xItem->flags() & ~Qt::ItemIsEditable); + m_linePointsTable->setItem(i, 1, xItem); + + // Y + QTableWidgetItem* yItem = new QTableWidgetItem(QString::number(pt.y(), 'f', 3)); + yItem->setTextAlignment(Qt::AlignCenter); + yItem->setBackground(rowColor); + yItem->setFlags(yItem->flags() & ~Qt::ItemIsEditable); + m_linePointsTable->setItem(i, 2, yItem); + + // Z + QTableWidgetItem* zItem = new QTableWidgetItem(QString::number(pt.z(), 'f', 3)); + zItem->setTextAlignment(Qt::AlignCenter); + zItem->setBackground(rowColor); + zItem->setFlags(zItem->flags() & ~Qt::ItemIsEditable); + m_linePointsTable->setItem(i, 3, zItem); + } +} + +void CloudViewMainWindow::onShowLinePoints() +{ + SelectedLineInfo lineInfo = m_glWidget->getSelectedLine(); + if (!lineInfo.valid) { + QMessageBox::warning(this, "提示", "请先选择一条线"); + return; + } + + // 如果对话框已存在，刷新内容并显示 + if (m_linePointsDialog) { + updateLinePointsDialog(); + m_linePointsDialog->raise(); + m_linePointsDialog->activateWindow(); + return; + } + + // 创建对话框 + m_linePointsDialog = new QDialog(this); + m_linePointsDialog->resize(450, 500); + m_linePointsDialog->setAttribute(Qt::WA_DeleteOnClose); + + // 对话框关闭时清理指针 + connect(m_linePointsDialog, &QDialog::destroyed, this, [this]() { + m_linePointsDialog = nullptr; + m_linePointsTable = nullptr; + m_currentLinePoints.clear(); + m_glWidget->clearListHighlightPoint(); + }); + + QVBoxLayout* layout = new QVBoxLayout(m_linePointsDialog); + + // 提示标签 + QLabel* lblTip = new QLabel("点击行在3D视图中高亮显示", m_linePointsDialog); + lblTip->setStyleSheet("color: gray; font-size: 10px;"); + layout->addWidget(lblTip); + + // 创建表格控件 + m_linePointsTable = new QTableWidget(m_linePointsDialog); + m_linePointsTable->setColumnCount(4); + m_linePointsTable->setHorizontalHeaderLabels({"序号", "X", "Y", "Z"}); + m_linePointsTable->setFont(QFont("Consolas", 9)); + m_linePointsTable->setSelectionBehavior(QAbstractItemView::SelectRows); + m_linePointsTable->setSelectionMode(QAbstractItemView::SingleSelection); + m_linePointsTable->verticalHeader()->setVisible(false); + + // 设置列宽 + m_linePointsTable->setColumnWidth(0, 60); // 序号 + m_linePointsTable->setColumnWidth(1, 110); // X + m_linePointsTable->setColumnWidth(2, 110); // Y + m_linePointsTable->setColumnWidth(3, 110); // Z + + // 表头样式 + m_linePointsTable->horizontalHeader()->setStretchLastSection(true); + m_linePointsTable->horizontalHeader()->setDefaultAlignment(Qt::AlignCenter); + + connect(m_linePointsTable, &QTableWidget::cellClicked, + this, &CloudViewMainWindow::onLinePointTableClicked); + layout->addWidget(m_linePointsTable); + + // 关闭按钮 + QPushButton* btnClose = new QPushButton("关闭", m_linePointsDialog); + connect(btnClose, &QPushButton::clicked, m_linePointsDialog, &QDialog::close); + layout->addWidget(btnClose); + + // 填充数据 + updateLinePointsDialog(); + + m_linePointsDialog->show(); +} + +void CloudViewMainWindow::onLinePointTableClicked(int row, int column) +{ + Q_UNUSED(column); + + if (row >= 0 && row < m_currentLinePoints.size()) { + const QVector3D& pt = m_currentLinePoints[row]; + m_glWidget->setListHighlightPoint(pt); + + // 在状态栏显示选中点信息 + statusBar()->showMessage(QString("列表选中点 %1: (%2, %3, %4)") + .arg(row) + .arg(pt.x(), 0, 'f', 3) + .arg(pt.y(), 0, 'f', 3) + .arg(pt.z(), 0, 'f', 3)); + } +} + +void CloudViewMainWindow::onShowPose1() +{ + // 从输入框读取点1坐标 + bool ok = true; + float x1 = m_editPoint1X->text().toFloat(&ok); + if (!ok) { QMessageBox::warning(this, "错误", "点1 X 值无效"); return; } + float y1 = m_editPoint1Y->text().toFloat(&ok); + if (!ok) { QMessageBox::warning(this, "错误", "点1 Y 值无效"); return; } + float z1 = m_editPoint1Z->text().toFloat(&ok); + if (!ok) { QMessageBox::warning(this, "错误", "点1 Z 值无效"); return; } + + // 设置选中点（不存在则创建） + m_glWidget->setSelectedPointCoord(0, x1, y1, z1); + + // 读取姿态参数 + float rx = m_editRx1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 RX 值无效"); + return; + } + + float ry = m_editRy1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 RY 值无效"); + return; + } + + float rz = m_editRz1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 RZ 值无效"); + return; + } + + // 读取坐标轴缩放 + float scale = m_editPoseScale->text().toFloat(&ok); + if (!ok || scale <= 0) scale = 25.0f; + + // 清除之前的姿态点 + m_glWidget->clearPosePoints(); + + // 创建点1的姿态点 + PosePoint pose1(x1, y1, z1, rx, ry, rz, scale); + QVector poses; + poses.append(pose1); + + // 如果点2输入框有值，也一起显示 + bool ok2 = true; + float x2 = m_editPoint2X->text().toFloat(&ok2); + float y2 = ok2 ? m_editPoint2Y->text().toFloat(&ok2) : 0; + float z2 = ok2 ? m_editPoint2Z->text().toFloat(&ok2) : 0; + if (ok2 && !(x2 == 0 && y2 == 0 && z2 == 0 && m_editPoint2X->text().isEmpty())) { + m_glWidget->setSelectedPointCoord(1, x2, y2, z2); + + bool okR = true; + float rx2 = m_editRx2->text().toFloat(&okR); + float ry2 = okR ? m_editRy2->text().toFloat(&okR) : 0; + float rz2 = okR ? m_editRz2->text().toFloat(&okR) : 0; + if (okR) { + PosePoint pose2(x2, y2, z2, rx2, ry2, rz2, scale); + poses.append(pose2); + } + } + + // 添加到显示 + m_glWidget->addPosePoints(poses); + + statusBar()->showMessage(QString("已显示点1姿态 (%1, %2, %3) 旋转(%4°, %5°, %6°)") + .arg(x1, 0, 'f', 3).arg(y1, 0, 'f', 3).arg(z1, 0, 'f', 3) + .arg(rx, 0, 'f', 1).arg(ry, 0, 'f', 1).arg(rz, 0, 'f', 1)); + + LOG_INFO("[CloudView] Show pose1 at (%.3f, %.3f, %.3f) with rotation (%.1f, %.1f, %.1f)\n", + x1, y1, z1, rx, ry, rz); +} + +void CloudViewMainWindow::onShowPose2() +{ + // 从输入框读取点2坐标 + bool ok = true; + float x2 = m_editPoint2X->text().toFloat(&ok); + if (!ok) { QMessageBox::warning(this, "错误", "点2 X 值无效"); return; } + float y2 = m_editPoint2Y->text().toFloat(&ok); + if (!ok) { QMessageBox::warning(this, "错误", "点2 Y 值无效"); return; } + float z2 = m_editPoint2Z->text().toFloat(&ok); + if (!ok) { QMessageBox::warning(this, "错误", "点2 Z 值无效"); return; } + + // 设置选中点（不存在则创建） + m_glWidget->setSelectedPointCoord(1, x2, y2, z2); + + // 读取姿态参数 + float rx = m_editRx2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 RX 值无效"); + return; + } + + float ry = m_editRy2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 RY 值无效"); + return; + } + + float rz = m_editRz2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 RZ 值无效"); + return; + } + + // 读取坐标轴缩放 + float scale = m_editPoseScale->text().toFloat(&ok); + if (!ok || scale <= 0) scale = 25.0f; + + // 清除之前的姿态点 + m_glWidget->clearPosePoints(); + + QVector poses; + + // 如果点1输入框有值，也一起显示 + bool ok1 = true; + float x1 = m_editPoint1X->text().toFloat(&ok1); + float y1 = ok1 ? m_editPoint1Y->text().toFloat(&ok1) : 0; + float z1 = ok1 ? m_editPoint1Z->text().toFloat(&ok1) : 0; + if (ok1 && !(x1 == 0 && y1 == 0 && z1 == 0 && m_editPoint1X->text().isEmpty())) { + m_glWidget->setSelectedPointCoord(0, x1, y1, z1); + + bool okR = true; + float rx1 = m_editRx1->text().toFloat(&okR); + float ry1 = okR ? m_editRy1->text().toFloat(&okR) : 0; + float rz1 = okR ? m_editRz1->text().toFloat(&okR) : 0; + if (okR) { + PosePoint pose1(x1, y1, z1, rx1, ry1, rz1, scale); + poses.append(pose1); + } + } + + // 创建点2的姿态点 + PosePoint pose2(x2, y2, z2, rx, ry, rz, scale); + poses.append(pose2); + + // 添加到显示 + m_glWidget->addPosePoints(poses); + + statusBar()->showMessage(QString("已显示点2姿态 (%1, %2, %3) 旋转(%4°, %5°, %6°)") + .arg(x2, 0, 'f', 3).arg(y2, 0, 'f', 3).arg(z2, 0, 'f', 3) + .arg(rx, 0, 'f', 1).arg(ry, 0, 'f', 1).arg(rz, 0, 'f', 1)); + + LOG_INFO("[CloudView] Show pose2 at (%.3f, %.3f, %.3f) with rotation (%.1f, %.1f, %.1f)\n", + x2, y2, z2, rx, ry, rz); +} + +void CloudViewMainWindow::onShowInputLine() +{ + // 读取点1坐标 + bool ok = true; + float x1 = m_editLineX1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 X 值无效"); + return; + } + + float y1 = m_editLineY1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 Y 值无效"); + return; + } + + float z1 = m_editLineZ1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 Z 值无效"); + return; + } + + // 读取点2坐标 + float x2 = m_editLineX2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 X 值无效"); + return; + } + + float y2 = m_editLineY2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 Y 值无效"); + return; + } + + float z2 = m_editLineZ2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 Z 值无效"); + return; + } + + // 清除之前的线段 + m_glWidget->clearLineSegments(); + + // 创建线段（红色） + LineSegment segment(x1, y1, z1, x2, y2, z2, 1.0f, 0.0f, 0.0f); + QVector segments; + segments.append(segment); + + // 添加到显示 + m_glWidget->addLineSegments(segments); + + // 计算距离 + float dx = x2 - x1; + float dy = y2 - y1; + float dz = z2 - z1; + float distance = std::sqrt(dx * dx + dy * dy + dz * dz); + + statusBar()->showMessage(QString("已显示线段 (%1,%2,%3) → (%4,%5,%6) 长度: %7") + .arg(x1, 0, 'f', 1).arg(y1, 0, 'f', 1).arg(z1, 0, 'f', 1) + .arg(x2, 0, 'f', 1).arg(y2, 0, 'f', 1).arg(z2, 0, 'f', 1) + .arg(distance, 0, 'f', 3)); + + LOG_INFO("[CloudView] Show input line from (%.3f, %.3f, %.3f) to (%.3f, %.3f, %.3f) length=%.3f\n", + x1, y1, z1, x2, y2, z2, distance); +} + +void CloudViewMainWindow::onClearInputLine() +{ + m_glWidget->clearLineSegments(); + statusBar()->showMessage("已清除输入的线段"); +} + +#if 0 +QWidget* CloudViewMainWindow::createTransformPage() +{ + QWidget* page = new QWidget(this); + QVBoxLayout* layout = new QVBoxLayout(page); + layout->setContentsMargins(0, 5, 0, 0); + + QGroupBox* group = new QGroupBox("矩阵变换", page); + group->setMaximumWidth(400); + QVBoxLayout* groupLayout = new QVBoxLayout(group); + + // 操作说明 + QLabel* lblTip = new QLabel("输入或从文件加载 4x4 变换矩阵，应用到所有点云", group); + lblTip->setWordWrap(true); + lblTip->setStyleSheet("color: gray; font-size: 10px;"); + groupLayout->addWidget(lblTip); + + // 矩阵编辑区域 + QLabel* lblMatrix = new QLabel("变换矩阵 (4x4):", group); + lblMatrix->setStyleSheet("font-weight: bold;"); + groupLayout->addWidget(lblMatrix); + + m_matrixEdit = new QTextEdit(group); + m_matrixEdit->setFont(QFont("Consolas", 10)); + m_matrixEdit->setMinimumHeight(100); + m_matrixEdit->setMaximumHeight(120); + // 初始化为单位矩阵 + m_matrixEdit->setPlainText( + "1.0 0.0 0.0 0.0\n" + "0.0 1.0 0.0 0.0\n" + "0.0 0.0 1.0 0.0\n" + "0.0 0.0 0.0 1.0" + ); + groupLayout->addWidget(m_matrixEdit); + + // 从文件加载按钮 + m_btnLoadMatrix = new QPushButton("从文件加载矩阵", group); + m_btnLoadMatrix->setMinimumHeight(30); + connect(m_btnLoadMatrix, &QPushButton::clicked, this, &CloudViewMainWindow::onLoadMatrix); + groupLayout->addWidget(m_btnLoadMatrix); + + // 按钮行 + QHBoxLayout* btnLayout = new QHBoxLayout(); + m_btnApplyMatrix = new QPushButton("应用变换", group); + m_btnApplyMatrix->setMinimumHeight(30); + m_btnApplyMatrix->setStyleSheet("QPushButton { background-color: #4CAF50; color: white; font-weight: bold; }" + "QPushButton:hover { background-color: #45a049; }"); + connect(m_btnApplyMatrix, &QPushButton::clicked, this, &CloudViewMainWindow::onApplyMatrix); + btnLayout->addWidget(m_btnApplyMatrix); + + m_btnResetMatrix = new QPushButton("重置矩阵", group); + connect(m_btnResetMatrix, &QPushButton::clicked, this, &CloudViewMainWindow::onResetMatrix); + btnLayout->addWidget(m_btnResetMatrix); + groupLayout->addLayout(btnLayout); + + // 文件格式说明 + QLabel* lblFormat = new QLabel( + "矩阵文件格式：4行，每行4个数值\n" + "分隔符：空格/Tab/逗号\n" + "#开头的行为注释", group); + lblFormat->setWordWrap(true); + lblFormat->setStyleSheet("color: gray; font-size: 9px;"); + groupLayout->addWidget(lblFormat); + + layout->addWidget(group); + + QGroupBox* eyeGroup = new QGroupBox("眼在手上转换", page); + eyeGroup->setMaximumWidth(400); + QVBoxLayout* eyeLayout = new QVBoxLayout(eyeGroup); + + QLabel* lblEyeTip = new QLabel(QString(), eyeGroup); /* + "使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼标定矩阵。
" + "输入当前机械臂位姿（mm / deg）后，可将所有点云转换到机械臂基坐标系。
" + "角度组合方式：ZYX（Rz-Ry-Rx）。", + eyeGroup); + */ lblEyeTip->setWordWrap(true); + /* lblEyeTip->setText( + "使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼标定矩阵。
" + "输入当前机械臂位姿（mm / deg）后，可将所有点云转换到机械臂基坐标系。
" + "角度组合方式：ZYX（Rz-Ry-Rx）。"); + lblEyeTip->setText( + "使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼标定矩阵。
" + "输入当前机械臂位姿（mm / deg）后，可将所有点云转换到机械臂基坐标系。
" + "角度组合方式：ZYX（Rz-Ry-Rx）。"); + */ lblEyeTip->setTextFormat(Qt::RichText); + lblEyeTip->setText( + "使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼标定矩阵。
" + "输入当前机械臂位姿（mm / deg）后，可将所有点云转换到机械臂基坐标系。
" + "角度组合方式：ZYX（Rz-Ry-Rx）。"); + lblEyeTip->setStyleSheet("color: gray; font-size: 10px;"); + eyeLayout->addWidget(lblEyeTip); + + QGridLayout* poseLayout = new QGridLayout(); + poseLayout->setHorizontalSpacing(8); + poseLayout->setVerticalSpacing(6); + + auto addPoseEdit = [&](const QString& label, QLineEdit*& edit, int row, int col) { + QLabel* lbl = new QLabel(label, eyeGroup); + edit = new QLineEdit("0.0", eyeGroup); + edit->setAlignment(Qt::AlignRight); + edit->setFont(QFont("Consolas", 10)); + poseLayout->addWidget(lbl, row, col * 2); + poseLayout->addWidget(edit, row, col * 2 + 1); + }; + + addPoseEdit("X:", m_editRobotX, 0, 0); + addPoseEdit("Y:", m_editRobotY, 0, 1); + addPoseEdit("Z:", m_editRobotZ, 0, 2); + addPoseEdit("RX:", m_editRobotRx, 1, 0); + addPoseEdit("RY:", m_editRobotRy, 1, 1); + addPoseEdit("RZ:", m_editRobotRz, 1, 2); + eyeLayout->addLayout(poseLayout); + + m_btnApplyEyeInHand = new QPushButton("应用眼在手上转换", eyeGroup); + m_btnApplyEyeInHand->setMinimumHeight(30); + m_btnApplyEyeInHand->setStyleSheet("QPushButton { background-color: #1976D2; color: white; font-weight: bold; }" + "QPushButton:hover { background-color: #1565C0; }"); + connect(m_btnApplyEyeInHand, &QPushButton::clicked, this, &CloudViewMainWindow::onApplyEyeInHandTransform); + eyeLayout->addWidget(m_btnApplyEyeInHand); + + for (QLineEdit* edit : {m_editRobotX, m_editRobotY, m_editRobotZ, m_editRobotRx, m_editRobotRy, m_editRobotRz}) { + connect(edit, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onApplyEyeInHandTransform); + } + + layout->addWidget(eyeGroup); + layout->addStretch(); + return page; +} + +void CloudViewMainWindow::onLoadMatrix() +{ + QString fileName = QFileDialog::getOpenFileName( + this, + "打开矩阵文件", + QString(), + "文本文件 (*.txt);;所有文件 (*.*)" + ); + + if (fileName.isEmpty()) { + return; + } + + QFile file(fileName); + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + QMessageBox::critical(this, "错误", QString("无法打开文件: %1").arg(fileName)); + return; + } + + QTextStream matrixStream(&file); + const QString loadedMatrixText = matrixStream.readAll(); + file.close(); + + QMatrix4x4 matrix; + QString errorMessage; + if (!parseMatrixText(loadedMatrixText, matrix, &errorMessage)) { + QMessageBox::warning(this, "格式错误", errorMessage); + return; + } + + m_matrixEdit->setPlainText(formatMatrixText(matrix)); + statusBar()->showMessage(QString("已从 %1 加载矩阵").arg(QFileInfo(fileName).fileName())); + LOG_INFO("[CloudView] Loaded matrix from %s\n", fileName.toStdString().c_str()); + return; + + QTextStream in(&file); + QVector> rows; + + while (!in.atEnd()) { + QString line = in.readLine().trimmed(); + // 跳过空行和注释 + if (line.isEmpty() || line.startsWith('#')) { + continue; + } + + // 将逗号替换为空格，统一分隔符 + line.replace(',', ' '); + line.replace('\t', ' '); + QStringList parts = line.split(' ', QString::SkipEmptyParts); + + QVector row; + bool ok = true; + for (const QString& part : parts) { + float val = part.toFloat(&ok); + if (!ok) break; + row.append(val); + } + + if (!ok || row.size() != 4) { + QMessageBox::warning(this, "格式错误", + QString("第 %1 行格式无效，需要4个数值").arg(rows.size() + 1)); + file.close(); + return; + } + rows.append(row); + + if (rows.size() == 4) { + break; + } + } + file.close(); + + if (rows.size() != 4) { + QMessageBox::warning(this, "格式错误", + QString("矩阵需要4行数据，当前只有 %1 行").arg(rows.size())); + return; + } + + // 将矩阵显示到编辑区域 + QString matrixText; + for (int r = 0; r < 4; ++r) { + QStringList vals; + for (int c = 0; c < 4; ++c) { + vals.append(QString::number(static_cast(rows[r][c]), 'f', 6)); + } + matrixText += vals.join(" "); + if (r < 3) matrixText += "\n"; + } + m_matrixEdit->setPlainText(matrixText); + + statusBar()->showMessage(QString("已从 %1 加载矩阵").arg(QFileInfo(fileName).fileName())); + LOG_INFO("[CloudView] Loaded matrix from %s\n", fileName.toStdString().c_str()); +} + +void CloudViewMainWindow::onApplyMatrix() +{ + if (m_glWidget->getCloudCount() == 0) { + QMessageBox::warning(this, "提示", "请先加载点云"); + return; + } + + // 解析编辑区域中的矩阵 + QMatrix4x4 parsedMatrix; + QString errorMessage; + if (!parseMatrixText(m_matrixEdit->toPlainText().trimmed(), parsedMatrix, &errorMessage)) { + QMessageBox::warning(this, "格式错误", errorMessage); + return; + } + + if (parsedMatrix.isIdentity()) { + QMessageBox::information(this, "提示", "当前矩阵为单位矩阵，无需变换"); + return; + } + + applyTransformToAllClouds(parsedMatrix); + statusBar()->showMessage("已应用矩阵变换到所有点云"); + LOG_INFO("[CloudView] Applied matrix transform to all point clouds\n"); + return; + + QString text = m_matrixEdit->toPlainText().trimmed(); + QStringList lines = text.split('\n', QString::SkipEmptyParts); + + QVector> rows; + for (const QString& line : lines) { + QString cleaned = line.trimmed(); + if (cleaned.isEmpty() || cleaned.startsWith('#')) { + continue; + } + cleaned.replace(',', ' '); + cleaned.replace('\t', ' '); + QStringList parts = cleaned.split(' ', QString::SkipEmptyParts); + + QVector row; + bool ok = true; + for (const QString& part : parts) { + float val = part.toFloat(&ok); + if (!ok) break; + row.append(val); + } + + if (!ok || row.size() != 4) { + QMessageBox::warning(this, "格式错误", "矩阵格式无效，需要4行4列数值"); + return; + } + rows.append(row); + } + + if (rows.size() != 4) { + QMessageBox::warning(this, "格式错误", + QString("矩阵需要4行数据，当前 %1 行").arg(rows.size())); + return; + } + + // 构造 QMatrix4x4（按行优先存储） + float values[16]; + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + values[r * 4 + c] = rows[r][c]; + } + } + QMatrix4x4 matrix(values); + + // 检查是否为单位矩阵 + if (matrix.isIdentity()) { + QMessageBox::information(this, "提示", "当前矩阵为单位矩阵，无需变换"); + return; + } + + // 应用变换 + m_glWidget->transformAllClouds(matrix); + + statusBar()->showMessage("已应用矩阵变换到所有点云"); + LOG_INFO("[CloudView] Applied matrix transform to all point clouds\n"); +} + +void CloudViewMainWindow::onApplyEyeInHandTransform() +{ + if (m_glWidget->getCloudCount() == 0) { + QMessageBox::warning(this, "提示", "请先加载点云"); + return; + } + + QMatrix4x4 handEyeMatrix; + QString errorMessage; + if (!parseMatrixText(m_matrixEdit->toPlainText().trimmed(), handEyeMatrix, &errorMessage)) { QMessageBox::warning(this, "格式错误", QString("Hand-eye matrix is invalid: %1").arg(errorMessage)); return; /* + QMessageBox::warning(this, "格式错误", QString("手眼矩阵无效：%1").arg(errorMessage)); + return; + QMessageBox::warning(this, "格式错误", QString("手眼矩阵无效：%1").arg(errorMessage)); + return; + QMessageBox::warning(this, "格式错误", QString("手眼矩阵无效：%1").arg(errorMessage)); + return; + */ } + + bool ok = false; + const float x = m_editRobotX->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 X 位姿无效"); + return; + } + + const float y = m_editRobotY->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 Y 位姿无效"); + return; + } + + const float z = m_editRobotZ->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 Z 位姿无效"); + return; + } + + const float rx = m_editRobotRx->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 RX 位姿无效"); + return; + } + + const float ry = m_editRobotRy->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 RY 位姿无效"); + return; + } + + const float rz = m_editRobotRz->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 RZ 位姿无效"); + return; + } + + const QMatrix4x4 robotPoseMatrix = buildRobotPoseMatrix(x, y, z, rx, ry, rz); + const QMatrix4x4 totalMatrix = robotPoseMatrix * handEyeMatrix; + + if (totalMatrix.isIdentity()) { + QMessageBox::information(this, "提示", "当前位姿和手眼矩阵组合为单位矩阵，无需变换"); + return; + } + + applyTransformToAllClouds(totalMatrix); + + statusBar()->showMessage( + QString("已应用眼在手上转换，当前机械臂位姿: [%1, %2, %3, %4, %5, %6]") + .arg(x, 0, 'f', 3) + .arg(y, 0, 'f', 3) + .arg(z, 0, 'f', 3) + .arg(rx, 0, 'f', 3) + .arg(ry, 0, 'f', 3) + .arg(rz, 0, 'f', 3)); + LOG_INFO("[CloudView] Applied eye-in-hand transform, robot pose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n", + x, y, z, rx, ry, rz); +} + +void CloudViewMainWindow::onResetMatrix() +{ + m_matrixEdit->setPlainText( + "1.0 0.0 0.0 0.0\n" + "0.0 1.0 0.0 0.0\n" + "0.0 0.0 1.0 0.0\n" + "0.0 0.0 0.0 1.0" + ); + statusBar()->showMessage("矩阵已重置为单位矩阵"); +} + +#endif + +QWidget* CloudViewMainWindow::createTransformPage() +{ + QWidget* page = new QWidget(this); + QVBoxLayout* layout = new QVBoxLayout(page); + layout->setContentsMargins(0, 5, 0, 0); + + QGroupBox* group = new QGroupBox("矩阵变换", page); + group->setMaximumWidth(400); + QVBoxLayout* groupLayout = new QVBoxLayout(group); + + QLabel* lblTip = new QLabel(group); /* + "输入或从文件加载 4x4 变换矩阵，可直接应用到所有点云，也可作为下方眼在手上转换的手眼矩阵。", + group); */ + lblTip->setText("输入或从文件加载 4x4 变换矩阵，可直接应用到所有点云，也可作为下方眼在手上转换的手眼矩阵。"); + lblTip->setText("输入或从文件加载 4x4 变换矩阵，可直接应用到所有点云，也可作为下方眼在手上转换的手眼矩阵。"); + lblTip->setWordWrap(true); + lblTip->setStyleSheet("color: gray; font-size: 10px;"); + groupLayout->addWidget(lblTip); + + QLabel* lblMatrix = new QLabel("变换矩阵 (4x4):", group); + lblMatrix->setStyleSheet("font-weight: bold;"); + groupLayout->addWidget(lblMatrix); + + m_matrixEdit = new QTextEdit(group); + m_matrixEdit->setFont(QFont("Consolas", 10)); + m_matrixEdit->setMinimumHeight(100); + m_matrixEdit->setMaximumHeight(120); + m_matrixEdit->setPlainText( + "1.0 0.0 0.0 0.0\n" + "0.0 1.0 0.0 0.0\n" + "0.0 0.0 1.0 0.0\n" + "0.0 0.0 0.0 1.0"); + groupLayout->addWidget(m_matrixEdit); + + m_btnLoadMatrix = new QPushButton("从文件加载矩阵", group); + m_btnLoadMatrix->setMinimumHeight(30); + connect(m_btnLoadMatrix, &QPushButton::clicked, this, &CloudViewMainWindow::onLoadMatrix); + groupLayout->addWidget(m_btnLoadMatrix); + + QHBoxLayout* btnLayout = new QHBoxLayout(); + m_btnApplyMatrix = new QPushButton("应用变换", group); + m_btnApplyMatrix->setMinimumHeight(30); + m_btnApplyMatrix->setStyleSheet("QPushButton { background-color: #4CAF50; color: white; font-weight: bold; }" + "QPushButton:hover { background-color: #45a049; }"); + connect(m_btnApplyMatrix, &QPushButton::clicked, this, &CloudViewMainWindow::onApplyMatrix); + btnLayout->addWidget(m_btnApplyMatrix); + + m_btnResetMatrix = new QPushButton("重置矩阵", group); + connect(m_btnResetMatrix, &QPushButton::clicked, this, &CloudViewMainWindow::onResetMatrix); + btnLayout->addWidget(m_btnResetMatrix); + groupLayout->addLayout(btnLayout); + + QLabel* lblFormat = new QLabel(group); /* + "矩阵文件格式：4 行，每行 4 个数值\n" + "分隔符：空格 / Tab / 逗号\n" + "# 开头的行会被视为注释", + group); */ + lblFormat->setText("矩阵文件格式：4 行，每行 4 个数值\n分隔符：空格 / Tab / 逗号\n# 开头的行会被视为注释"); + lblFormat->setText("矩阵文件格式：4 行，每行 4 个数值\n分隔符：空格 / Tab / 逗号\n# 开头的行会被视为注释"); + lblFormat->setWordWrap(true); + lblFormat->setStyleSheet("color: gray; font-size: 9px;"); + groupLayout->addWidget(lblFormat); + + layout->addWidget(group); + + QGroupBox* eyeGroup = new QGroupBox("眼在手上转换", page); + eyeGroup->setMaximumWidth(400); + QVBoxLayout* eyeLayout = new QVBoxLayout(eyeGroup); + + QLabel* lblEyeTip = new QLabel(eyeGroup); /* + "使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼矩阵。
" + "输入当前机械臂位姿（单位：mm / deg）后，可将所有点云转换到机械臂基坐标系。
" + "角度组合方式：ZYX（Rz-Ry-Rx）。", + eyeGroup); */ + lblEyeTip->setText("使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼矩阵。
输入当前机械臂位姿（单位：mm / deg）后，可将所有点云转换到机械臂基坐标系。
角度组合方式：ZYX（Rz-Ry-Rx）。"); + lblEyeTip->setText("使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼矩阵。
输入当前机械臂位姿（单位：mm / deg）后，可将所有点云转换到机械臂基坐标系。
角度组合方式：ZYX（Rz-Ry-Rx）。"); + lblEyeTip->setWordWrap(true); + lblEyeTip->setTextFormat(Qt::RichText); + lblEyeTip->setStyleSheet("color: gray; font-size: 10px;"); + eyeLayout->addWidget(lblEyeTip); + + QGridLayout* poseLayout = new QGridLayout(); + poseLayout->setHorizontalSpacing(8); + poseLayout->setVerticalSpacing(6); + + auto addPoseEdit = [&](const QString& label, QLineEdit*& edit, int row, int col) { + QLabel* lbl = new QLabel(label, eyeGroup); + edit = new QLineEdit("0.0", eyeGroup); + edit->setAlignment(Qt::AlignRight); + edit->setFont(QFont("Consolas", 10)); + poseLayout->addWidget(lbl, row, col * 2); + poseLayout->addWidget(edit, row, col * 2 + 1); + }; + + addPoseEdit("X:", m_editRobotX, 0, 0); + addPoseEdit("Y:", m_editRobotY, 0, 1); + addPoseEdit("Z:", m_editRobotZ, 0, 2); + addPoseEdit("RX:", m_editRobotRx, 1, 0); + addPoseEdit("RY:", m_editRobotRy, 1, 1); + addPoseEdit("RZ:", m_editRobotRz, 1, 2); + eyeLayout->addLayout(poseLayout); + + m_btnApplyEyeInHand = new QPushButton("应用眼在手上转换", eyeGroup); + m_btnApplyEyeInHand->setMinimumHeight(30); + m_btnApplyEyeInHand->setStyleSheet("QPushButton { background-color: #1976D2; color: white; font-weight: bold; }" + "QPushButton:hover { background-color: #1565C0; }"); + connect(m_btnApplyEyeInHand, &QPushButton::clicked, this, &CloudViewMainWindow::onApplyEyeInHandTransform); + eyeLayout->addWidget(m_btnApplyEyeInHand); + + for (QLineEdit* edit : {m_editRobotX, m_editRobotY, m_editRobotZ, m_editRobotRx, m_editRobotRy, m_editRobotRz}) { + connect(edit, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onApplyEyeInHandTransform); + } + + layout->addWidget(eyeGroup); + layout->addStretch(); + return page; +} + +void CloudViewMainWindow::onLoadMatrix() +{ + const QString fileName = QFileDialog::getOpenFileName( + this, + "打开矩阵文件", + QString(), + "标定矩阵文件 (*.ini *.txt);;INI文件 (*.ini);;文本文件 (*.txt);;所有文件 (*.*)"); + + if (fileName.isEmpty()) { + return; + } + + // INI 格式：EyeHandCalibMatrixInfo.ini 标定矩阵 + if (fileName.endsWith(".ini", Qt::CaseInsensitive)) { + QVector> matrixList; + QString errorMessage; + if (!parseCalibIniFile(fileName, matrixList, &errorMessage)) { + QMessageBox::warning(this, "格式错误", errorMessage); + return; + } + + // 只有一个矩阵时直接加载 + if (matrixList.size() == 1) { + m_matrixEdit->setPlainText(formatMatrixText(matrixList[0].second)); + statusBar()->showMessage(QString("已从 %1 加载 %2").arg(QFileInfo(fileName).fileName(), matrixList[0].first)); + LOG_INFO("[CloudView] Loaded %s from %s\n", matrixList[0].first.toStdString().c_str(), fileName.toStdString().c_str()); + return; + } + + // 多个矩阵时弹出选择对话框 + QDialog dlg(this); + dlg.setWindowTitle("选择标定矩阵"); + QVBoxLayout* dlgLayout = new QVBoxLayout(&dlg); + dlgLayout->addWidget(new QLabel(QString("文件中包含 %1 个标定矩阵，请选择：").arg(matrixList.size()), &dlg)); + + QListWidget* listWidget = new QListWidget(&dlg); + for (int i = 0; i < matrixList.size(); ++i) { + listWidget->addItem(matrixList[i].first); + } + listWidget->setCurrentRow(0); + dlgLayout->addWidget(listWidget); + + QDialogButtonBox* btnBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, &dlg); + dlgLayout->addWidget(btnBox); + connect(btnBox, &QDialogButtonBox::accepted, &dlg, &QDialog::accept); + connect(btnBox, &QDialogButtonBox::rejected, &dlg, &QDialog::reject); + + if (dlg.exec() != QDialog::Accepted) return; + + const int selected = listWidget->currentRow(); + if (selected < 0 || selected >= matrixList.size()) return; + + m_matrixEdit->setPlainText(formatMatrixText(matrixList[selected].second)); + statusBar()->showMessage(QString("已从 %1 加载 %2").arg(QFileInfo(fileName).fileName(), matrixList[selected].first)); + LOG_INFO("[CloudView] Loaded %s from %s\n", matrixList[selected].first.toStdString().c_str(), fileName.toStdString().c_str()); + return; + } + + // 纯文本格式 + QFile file(fileName); + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + QMessageBox::critical(this, "错误", QString("无法打开文件: %1").arg(fileName)); + return; + } + + QTextStream in(&file); + const QString text = in.readAll(); + file.close(); + + QMatrix4x4 matrix; + QString errorMessage; + if (!parseMatrixText(text, matrix, &errorMessage)) { + QMessageBox::warning(this, "格式错误", errorMessage); + return; + } + + m_matrixEdit->setPlainText(formatMatrixText(matrix)); + statusBar()->showMessage(QString("已从 %1 加载矩阵").arg(QFileInfo(fileName).fileName())); + LOG_INFO("[CloudView] Loaded matrix from %s\n", fileName.toStdString().c_str()); +} + +void CloudViewMainWindow::onApplyMatrix() +{ + if (m_glWidget->getCloudCount() == 0) { + QMessageBox::warning(this, "提示", "请先加载点云"); + return; + } + + QMatrix4x4 matrix; + QString errorMessage; + if (!parseMatrixText(m_matrixEdit->toPlainText().trimmed(), matrix, &errorMessage)) { + QMessageBox::warning(this, "格式错误", errorMessage); + return; + } + + if (matrix.isIdentity()) { + QMessageBox::information(this, "提示", "当前矩阵为单位矩阵，无需变换"); + return; + } + + applyTransformToAllClouds(matrix); + statusBar()->showMessage("已将矩阵变换应用到所有点云"); + LOG_INFO("[CloudView] Applied matrix transform to all point clouds\n"); +} + +void CloudViewMainWindow::onApplyEyeInHandTransform() +{ + if (m_glWidget->getCloudCount() == 0) { + QMessageBox::warning(this, "提示", "请先加载点云"); + return; + } + + QMatrix4x4 handEyeMatrix; + QString errorMessage; + if (!parseMatrixText(m_matrixEdit->toPlainText().trimmed(), handEyeMatrix, &errorMessage)) { + QMessageBox::warning(this, "格式错误", QString("手眼矩阵无效：%1").arg(errorMessage)); + return; + QMessageBox::warning(this, "格式错误", QString("手眼矩阵无效：%1").arg(errorMessage)); + return; + QMessageBox::warning(this, "格式错误", QString("手眼矩阵无效：%1").arg(errorMessage)); + return; + } + + bool ok = false; + const float x = m_editRobotX->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 X 位姿无效"); + return; + } + + const float y = m_editRobotY->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 Y 位姿无效"); + return; + } + + const float z = m_editRobotZ->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 Z 位姿无效"); + return; + } + + const float rx = m_editRobotRx->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 RX 位姿无效"); + return; + } + + const float ry = m_editRobotRy->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 RY 位姿无效"); + return; + } + + const float rz = m_editRobotRz->text().trimmed().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "格式错误", "机械臂 RZ 位姿无效"); + return; + } + + const QMatrix4x4 totalMatrix = buildRobotPoseMatrix(x, y, z, rx, ry, rz) * handEyeMatrix; + if (totalMatrix.isIdentity()) { + QMessageBox::information(this, "提示", "当前位姿和手眼矩阵组合为单位矩阵，无需变换"); + return; + } + + applyTransformToAllClouds(totalMatrix); + statusBar()->showMessage( + QString("已应用眼在手上转换，当前机械臂位姿: [%1, %2, %3, %4, %5, %6]") + .arg(x, 0, 'f', 3) + .arg(y, 0, 'f', 3) + .arg(z, 0, 'f', 3) + .arg(rx, 0, 'f', 3) + .arg(ry, 0, 'f', 3) + .arg(rz, 0, 'f', 3)); + LOG_INFO("[CloudView] Applied eye-in-hand transform, robot pose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n", + x, y, z, rx, ry, rz); +} + +void CloudViewMainWindow::onResetMatrix() +{ + m_matrixEdit->setPlainText( + "1.0 0.0 0.0 0.0\n" + "0.0 1.0 0.0 0.0\n" + "0.0 0.0 1.0 0.0\n" + "0.0 0.0 0.0 1.0"); + + if (m_editRobotX) m_editRobotX->setText("0.0"); + if (m_editRobotY) m_editRobotY->setText("0.0"); + if (m_editRobotZ) m_editRobotZ->setText("0.0"); + if (m_editRobotRx) m_editRobotRx->setText("0.0"); + if (m_editRobotRy) m_editRobotRy->setText("0.0"); + if (m_editRobotRz) m_editRobotRz->setText("0.0"); + + statusBar()->showMessage("矩阵已重置为单位矩阵"); +} + +void CloudViewMainWindow::onSavePointCloud() +{ + if (m_glWidget->getCloudCount() == 0) { + QMessageBox::warning(this, "提示", "请先加载点云"); + return; + } + + const QString fileName = QFileDialog::getSaveFileName( + this, "保存点云", QString(), "文本文件 (*.txt);;所有文件 (*.*)"); + if (fileName.isEmpty()) return; + + std::vector> scanLines; + const size_t totalPoints = m_glWidget->getAllCloudsByLines(scanLines); + if (scanLines.empty() || totalPoints == 0) { + QMessageBox::warning(this, "提示", "当前无可保存的点云数据"); + return; + } + + // 进度对话框 + QProgressDialog progress("正在保存点云...", "取消", 0, 100, this); + progress.setWindowTitle("保存点云"); + progress.setWindowModality(Qt::WindowModal); + progress.setMinimumDuration(0); + progress.setAutoClose(true); + progress.setAutoReset(true); + + // 回调实现 + class SaveProgressCallback : public ISaveProgressCallback + { + public: + SaveProgressCallback(QProgressDialog& dlg) : m_dlg(dlg) {} + void onSaveProgress(float progress) override + { + m_dlg.setValue(static_cast(progress * 100)); + QCoreApplication::processEvents(); + } + bool isSaveCancelled() const override + { + return m_dlg.wasCanceled(); + } + private: + QProgressDialog& m_dlg; + }; + + SaveProgressCallback callback(progress); + + LaserDataLoader loader; + const int ret = loader.DebugSaveLaser(fileName.toStdString(), scanLines, &callback); + progress.setValue(100); + + if (ret != 0) { + if (callback.isSaveCancelled()) { + statusBar()->showMessage("保存已取消"); + } else { + QMessageBox::critical(this, "保存失败", + QString("保存点云失败: %1").arg(QString::fromStdString(loader.GetLastError()))); + } + return; + } + + statusBar()->showMessage( + QString("已保存点云: %1 条线, %2 个点 -> %3") + .arg(scanLines.size()).arg(totalPoints).arg(QFileInfo(fileName).fileName())); + LOG_INFO("[CloudView] Saved %zu lines, %zu points to %s\n", + scanLines.size(), totalPoints, fileName.toStdString().c_str()); +} + +void CloudViewMainWindow::onRotateCloudByZ() +{ + if (m_glWidget->getCloudCount() == 0) { + QMessageBox::warning(this, "提示", "请先加载点云"); + return; + } + + QDialog dialog(this); + dialog.setWindowTitle("点云旋转"); + dialog.setModal(true); + + QVBoxLayout* layout = new QVBoxLayout(&dialog); + + QLabel* tip = new QLabel("对 Z 值小于阈值的点绕指定轴旋转指定角度。", &dialog); + tip->setWordWrap(true); + tip->setStyleSheet("color: gray;"); + layout->addWidget(tip); + + auto makeSpinBox = [&](double minValue, double maxValue, double value, double step = 1.0) { + QDoubleSpinBox* spin = new QDoubleSpinBox(&dialog); + spin->setDecimals(3); + spin->setRange(minValue, maxValue); + spin->setValue(value); + spin->setSingleStep(step); + spin->setAlignment(Qt::AlignRight); + return spin; + }; + + QGroupBox* thresholdGroup = new QGroupBox("Z 轴阈值", &dialog); + QFormLayout* thresholdLayout = new QFormLayout(thresholdGroup); + QDoubleSpinBox* zThreshold = makeSpinBox(-100000.0, 100000.0, 0.0, 10.0); + thresholdLayout->addRow("Z < ", zThreshold); + layout->addWidget(thresholdGroup); + + QGroupBox* rotateGroup = new QGroupBox("旋转角度 (度)", &dialog); + QFormLayout* rotateLayout = new QFormLayout(rotateGroup); + QDoubleSpinBox* rotX = makeSpinBox(-360.0, 360.0, 0.0, 5.0); + QDoubleSpinBox* rotY = makeSpinBox(-360.0, 360.0, 0.0, 5.0); + QDoubleSpinBox* rotZ = makeSpinBox(-360.0, 360.0, 0.0, 5.0); + rotateLayout->addRow("绕 X 轴:", rotX); + rotateLayout->addRow("绕 Y 轴:", rotY); + rotateLayout->addRow("绕 Z 轴:", rotZ); + layout->addWidget(rotateGroup); + + QDialogButtonBox* buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, &dialog); + connect(buttonBox, &QDialogButtonBox::accepted, &dialog, &QDialog::accept); + connect(buttonBox, &QDialogButtonBox::rejected, &dialog, &QDialog::reject); + layout->addWidget(buttonBox); + + if (dialog.exec() != QDialog::Accepted) { + return; + } + + // 构建旋转矩阵: Rz * Ry * Rx + const QMatrix4x4 rotMatrix = + makeAxisRotationMatrix('z', rotZ->value() * kDegToRad) * + makeAxisRotationMatrix('y', rotY->value() * kDegToRad) * + makeAxisRotationMatrix('x', rotX->value() * kDegToRad); + + const size_t count = m_glWidget->rotateCloudsByZThreshold(rotMatrix, static_cast(zThreshold->value())); + if (count == 0) { + QMessageBox::information(this, "提示", "没有满足条件的点 (Z < 阈值)"); + return; + } + + statusBar()->showMessage( + QString("已旋转 %1 个点 (Z < %2, 绕X %3° Y %4° Z %5°)") + .arg(count) + .arg(zThreshold->value(), 0, 'f', 3) + .arg(rotX->value(), 0, 'f', 1) + .arg(rotY->value(), 0, 'f', 1) + .arg(rotZ->value(), 0, 'f', 1)); + LOG_INFO("[CloudView] Rotated %zu points with Z < %.3f by (%.1f, %.1f, %.1f) deg\n", + count, zThreshold->value(), rotX->value(), rotY->value(), rotZ->value()); +} + +void CloudViewMainWindow::onGeneratePointCloud() +{ + QDialog dialog(this); + dialog.setWindowTitle("生成长方体点云"); + dialog.setModal(true); + + QVBoxLayout* layout = new QVBoxLayout(&dialog); + + QLabel* tip = new QLabel("根据中心点和 XYZ 尺寸生成轴对齐长方体表面点云。", &dialog); + tip->setWordWrap(true); + tip->setStyleSheet("color: gray;"); + layout->addWidget(tip); + + auto makeSpinBox = [&](double minValue, double maxValue, double value) { + QDoubleSpinBox* spin = new QDoubleSpinBox(&dialog); + spin->setDecimals(3); + spin->setRange(minValue, maxValue); + spin->setValue(value); + spin->setSingleStep(1.0); + spin->setAlignment(Qt::AlignRight); + return spin; + }; + + QGroupBox* centerGroup = new QGroupBox("中心点", &dialog); + QFormLayout* centerLayout = new QFormLayout(centerGroup); + QDoubleSpinBox* centerX = makeSpinBox(-100000.0, 100000.0, 0.0); + QDoubleSpinBox* centerY = makeSpinBox(-100000.0, 100000.0, 0.0); + QDoubleSpinBox* centerZ = makeSpinBox(-100000.0, 100000.0, 0.0); + centerLayout->addRow("X:", centerX); + centerLayout->addRow("Y:", centerY); + centerLayout->addRow("Z:", centerZ); + layout->addWidget(centerGroup); + + QGroupBox* sizeGroup = new QGroupBox("三轴方向大小", &dialog); + QFormLayout* sizeLayout = new QFormLayout(sizeGroup); + QDoubleSpinBox* sizeX = makeSpinBox(0.001, 100000.0, 100.0); + QDoubleSpinBox* sizeY = makeSpinBox(0.001, 100000.0, 100.0); + QDoubleSpinBox* sizeZ = makeSpinBox(0.001, 100000.0, 100.0); + QDoubleSpinBox* spacing = makeSpinBox(0.001, 10000.0, 5.0); + sizeLayout->addRow("X 尺寸:", sizeX); + sizeLayout->addRow("Y 尺寸:", sizeY); + sizeLayout->addRow("Z 尺寸:", sizeZ); + sizeLayout->addRow("点间距:", spacing); + layout->addWidget(sizeGroup); + + QDialogButtonBox* buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, &dialog); + connect(buttonBox, &QDialogButtonBox::accepted, &dialog, &QDialog::accept); + connect(buttonBox, &QDialogButtonBox::rejected, &dialog, &QDialog::reject); + layout->addWidget(buttonBox); + + if (dialog.exec() != QDialog::Accepted) { + return; + } + + const QVector3D center(static_cast(centerX->value()), + static_cast(centerY->value()), + static_cast(centerZ->value())); + const QVector3D size(static_cast(sizeX->value()), + static_cast(sizeY->value()), + static_cast(sizeZ->value())); + const float pointSpacing = static_cast(spacing->value()); + + const int gridX = std::max(1, static_cast(std::ceil(size.x() / pointSpacing))) + 1; + const int gridY = std::max(1, static_cast(std::ceil(size.y() / pointSpacing))) + 1; + const int gridZ = std::max(1, static_cast(std::ceil(size.z() / pointSpacing))) + 1; + const double estimatedGridCount = static_cast(gridX) * gridY * gridZ; + if (estimatedGridCount > 3000000.0) { + QMessageBox::warning(this, "提示", "点数预计过大，请增大点间距或减小尺寸。"); + return; + } + + const PointCloudXYZ cloud = generateBoxPointCloud(center, size, pointSpacing); + if (cloud.empty()) { + QMessageBox::warning(this, "提示", "生成点云失败，结果为空。"); + return; + } + + addGeneratedCloud(cloud, QString("GeneratedBox_%1x%2x%3") + .arg(size.x(), 0, 'f', 1) + .arg(size.y(), 0, 'f', 1) + .arg(size.z(), 0, 'f', 1)); +} + +void CloudViewMainWindow::onConvertEulerMatrix() +{ + QDialog* dialog = new QDialog(this); + dialog->setAttribute(Qt::WA_DeleteOnClose); + dialog->setWindowTitle("欧拉角与方向向量矩阵互转"); + dialog->setWindowFlags(dialog->windowFlags() | Qt::Tool); + + QVBoxLayout* layout = new QVBoxLayout(dialog); + + QLabel* tip = new QLabel( + "输入欧拉角（单位: 度）按所选旋转顺序得到 3x3 方向向量矩阵；也可以从矩阵反算欧拉角。\n" + "方向向量矩阵采用列向量约定: R = [ X轴 | Y轴 | Z轴 ]，每列为该坐标轴在参考系中的单位方向向量。", + dialog); + tip->setWordWrap(true); + tip->setStyleSheet("color: gray;"); + layout->addWidget(tip); + + QGroupBox* eulerGroup = new QGroupBox("欧拉角 (deg)", dialog); + QGridLayout* eulerLayout = new QGridLayout(eulerGroup); + + auto makeAngleSpin = [](QWidget* parent) { + QDoubleSpinBox* spin = new QDoubleSpinBox(parent); + spin->setDecimals(6); + spin->setRange(-3600.0, 3600.0); + spin->setSingleStep(0.1); + spin->setValue(0.0); + spin->setAlignment(Qt::AlignRight); + spin->setButtonSymbols(QAbstractSpinBox::NoButtons); + return spin; + }; + + QDoubleSpinBox* eulerRx = makeAngleSpin(eulerGroup); + QDoubleSpinBox* eulerRy = makeAngleSpin(eulerGroup); + QDoubleSpinBox* eulerRz = makeAngleSpin(eulerGroup); + eulerLayout->addWidget(new QLabel("RX:", eulerGroup), 0, 0); + eulerLayout->addWidget(eulerRx, 0, 1); + eulerLayout->addWidget(new QLabel("RY:", eulerGroup), 1, 0); + eulerLayout->addWidget(eulerRy, 1, 1); + eulerLayout->addWidget(new QLabel("RZ:", eulerGroup), 2, 0); + eulerLayout->addWidget(eulerRz, 2, 1); + + QComboBox* comboOrder = new QComboBox(eulerGroup); + comboOrder->addItem("ZYX (Yaw-Pitch-Roll)", static_cast(EulerRotationOrder::ZYX)); + comboOrder->addItem("XYZ (Roll-Pitch-Yaw)", static_cast(EulerRotationOrder::XYZ)); + comboOrder->addItem("ZXY (Yaw-Roll-Pitch)", static_cast(EulerRotationOrder::ZXY)); + comboOrder->addItem("YXZ (Pitch-Roll-Yaw)", static_cast(EulerRotationOrder::YXZ)); + comboOrder->addItem("XZY (Roll-Yaw-Pitch)", static_cast(EulerRotationOrder::XZY)); + comboOrder->addItem("YZX (Pitch-Yaw-Roll)", static_cast(EulerRotationOrder::YZX)); + if (m_comboEulerOrder && m_comboEulerOrder->currentIndex() >= 0) { + comboOrder->setCurrentIndex(m_comboEulerOrder->currentIndex()); + } + eulerLayout->addWidget(new QLabel("旋转顺序:", eulerGroup), 3, 0); + eulerLayout->addWidget(comboOrder, 3, 1); + + QGroupBox* matrixGroup = new QGroupBox("方向向量矩阵 (3x3)", dialog); + QGridLayout* matLayout = new QGridLayout(matrixGroup); + + auto makeHeader = [matrixGroup](const QString& text) { + QLabel* label = new QLabel(text, matrixGroup); + label->setAlignment(Qt::AlignCenter); + label->setStyleSheet("font-weight: bold;"); + return label; + }; + + matLayout->addWidget(makeHeader("X轴"), 0, 1); + matLayout->addWidget(makeHeader("Y轴"), 0, 2); + matLayout->addWidget(makeHeader("Z轴"), 0, 3); + matLayout->addWidget(new QLabel("X分量:", matrixGroup), 1, 0); + matLayout->addWidget(new QLabel("Y分量:", matrixGroup), 2, 0); + matLayout->addWidget(new QLabel("Z分量:", matrixGroup), 3, 0); + + std::array, 3> matCells{}; + for (int c = 0; c < 3; ++c) { + for (int r = 0; r < 3; ++r) { + QDoubleSpinBox* spin = new QDoubleSpinBox(matrixGroup); + spin->setDecimals(6); + spin->setRange(-1.0, 1.0); + spin->setSingleStep(0.01); + spin->setAlignment(Qt::AlignRight); + spin->setButtonSymbols(QAbstractSpinBox::NoButtons); + spin->setValue(r == c ? 1.0 : 0.0); + matCells[r][c] = spin; + matLayout->addWidget(spin, r + 1, c + 1); + } + } + + layout->addWidget(eulerGroup); + layout->addWidget(matrixGroup); + + QHBoxLayout* actionLayout = new QHBoxLayout(); + QPushButton* eulerToMat = new QPushButton("欧拉角 -> 方向向量矩阵", dialog); + QPushButton* matToEuler = new QPushButton("方向向量矩阵 -> 欧拉角", dialog); + QPushButton* closeButton = new QPushButton("关闭", dialog); + actionLayout->addWidget(eulerToMat); + actionLayout->addWidget(matToEuler); + actionLayout->addWidget(closeButton); + layout->addLayout(actionLayout); + + connect(eulerToMat, &QPushButton::clicked, dialog, [=]() { + const EulerRotationOrder order = static_cast(comboOrder->currentData().toInt()); + const QMatrix4x4 rotation = buildEulerRotationMatrix(order, + eulerRx->value(), + eulerRy->value(), + eulerRz->value()); + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + matCells[r][c]->setValue(rotation(r, c)); + } + } + }); + + connect(matToEuler, &QPushButton::clicked, dialog, [=]() { + QMatrix4x4 rotation; + rotation.setToIdentity(); + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + rotation(r, c) = static_cast(matCells[r][c]->value()); + } + } + const EulerRotationOrder order = static_cast(comboOrder->currentData().toInt()); + const QVector3D euler = rotationMatrixToEuler(order, rotation); + eulerRx->setValue(euler.x()); + eulerRy->setValue(euler.y()); + eulerRz->setValue(euler.z()); + }); + + connect(closeButton, &QPushButton::clicked, dialog, &QDialog::close); + dialog->show(); +} + +void CloudViewMainWindow::onEulerOrderChanged(int index) +{ + if (!m_glWidget) { + return; + } + + EulerRotationOrder order = static_cast(m_comboEulerOrder->itemData(index).toInt()); + m_glWidget->setEulerRotationOrder(order); + + // 如果有姿态点，刷新显示 + m_glWidget->update(); + + QString orderName = m_comboEulerOrder->currentText(); + statusBar()->showMessage(QString("欧拉角旋转顺序已切换为: %1").arg(orderName)); + LOG_INFO("[CloudView] Euler rotation order changed to: %s\n", orderName.toStdString().c_str()); +} + +void CloudViewMainWindow::onViewAnglesChanged(float rotX, float rotY, float rotZ) +{ + // 更新显示的角度值 + m_editRotX->setText(QString::number(rotX, 'f', 1)); + m_editRotY->setText(QString::number(rotY, 'f', 1)); + m_editRotZ->setText(QString::number(rotZ, 'f', 1)); +} + +void CloudViewMainWindow::onPoint1CoordChanged() +{ + // 读取编辑框中的坐标 + bool ok = true; + float x = m_editPoint1X->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 X 值无效"); + return; + } + + float y = m_editPoint1Y->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 Y 值无效"); + return; + } + + float z = m_editPoint1Z->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 Z 值无效"); + return; + } + + // 直接设置选中点坐标（无论是否已有鼠标选点） + m_glWidget->setSelectedPointCoord(0, x, y, z); + updateSelectedPointsDisplay(); + + // 如果启用了测距且有两个点，重新计算距离 + auto updatedPoints = m_glWidget->getSelectedPoints(); + if (m_glWidget->isMeasureDistanceEnabled() && updatedPoints.size() >= 2 && updatedPoints[1].valid) { + float distance = m_glWidget->calculateDistance(updatedPoints[0], updatedPoints[1]); + m_lblDistance->setText(QString("%1 mm").arg(distance, 0, 'f', 3)); + statusBar()->showMessage(QString("点1坐标已更新，距离: %1 mm").arg(distance, 0, 'f', 3)); + } else { + statusBar()->showMessage(QString("点1坐标已更新为 (%1, %2, %3)").arg(x, 0, 'f', 3).arg(y, 0, 'f', 3).arg(z, 0, 'f', 3)); + } + + LOG_INFO("[CloudView] Point1 coord updated to (%.3f, %.3f, %.3f)\n", x, y, z); +} + +void CloudViewMainWindow::onPoint2CoordChanged() +{ + // 读取编辑框中的坐标 + bool ok = true; + float x = m_editPoint2X->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 X 值无效"); + return; + } + + float y = m_editPoint2Y->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 Y 值无效"); + return; + } + + float z = m_editPoint2Z->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 Z 值无效"); + return; + } + + // 直接设置选中点坐标（无论是否已有鼠标选点） + m_glWidget->setSelectedPointCoord(1, x, y, z); + updateSelectedPointsDisplay(); + + // 如果启用了测距，重新计算距离 + auto updatedPoints = m_glWidget->getSelectedPoints(); + if (m_glWidget->isMeasureDistanceEnabled() && updatedPoints.size() >= 2 && updatedPoints[0].valid) { + float distance = m_glWidget->calculateDistance(updatedPoints[0], updatedPoints[1]); + m_lblDistance->setText(QString("%1 mm").arg(distance, 0, 'f', 3)); + statusBar()->showMessage(QString("点2坐标已更新，距离: %1 mm").arg(distance, 0, 'f', 3)); + } else { + statusBar()->showMessage(QString("点2坐标已更新为 (%1, %2, %3)").arg(x, 0, 'f', 3).arg(y, 0, 'f', 3).arg(z, 0, 'f', 3)); + } + + LOG_INFO("[CloudView] Point2 coord updated to (%.3f, %.3f, %.3f)\n", x, y, z); +} + diff --git a/CloudView/Src/PointCloudGLWidget.cpp b/CloudView/Src/PointCloudGLWidget.cpp index 60b1d2d..a02d7e9 100644 --- a/CloudView/Src/PointCloudGLWidget.cpp +++ b/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>& 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 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(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(lines) * ptsPerLine; + } else { + // 无线信息，每个点云作为一条线 + std::vector line; + line.reserve(displayCount); + for (size_t i = 0; i < displayCount; ++i) { + SVzNL3DPosition pos; + memset(&pos, 0, sizeof(pos)); + pos.nPointIdx = static_cast(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()) {