3401 lines
122 KiB
C++
3401 lines
122 KiB
C++
#include "CloudViewMainWindow.h"
|
||
#include <QFileInfo>
|
||
#include <QDialog>
|
||
#include <QTextEdit>
|
||
#include <QTableWidget>
|
||
#include <QHeaderView>
|
||
#include <QVector3D>
|
||
#include <QFile>
|
||
#include <QTextStream>
|
||
#include <QRegExp>
|
||
#include <QFrame>
|
||
#include <QTabWidget>
|
||
#include <QGridLayout>
|
||
#include <QJsonDocument>
|
||
#include <QJsonObject>
|
||
#include <QJsonArray>
|
||
#include <QEvent>
|
||
#include <QResizeEvent>
|
||
#include <QMenuBar>
|
||
#include <QMenu>
|
||
#include <QAction>
|
||
#include <QFormLayout>
|
||
#include <QDialogButtonBox>
|
||
#include <QDoubleSpinBox>
|
||
#include <QGroupBox>
|
||
#include <QListWidget>
|
||
#include <QSettings>
|
||
#include <QProgressDialog>
|
||
#include <QCoreApplication>
|
||
#include <cmath>
|
||
#include <algorithm>
|
||
#include <array>
|
||
#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<QVector<float>> 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<float> 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<QPair<QString, QMatrix4x4>>& 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<float>(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<float>(std::cos(angleRad));
|
||
const float s = static_cast<float>(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<float>(rx / kDegToRad),
|
||
static_cast<float>(ry / kDegToRad),
|
||
static_cast<float>(rz / kDegToRad));
|
||
}
|
||
|
||
PointCloudXYZ generateBoxPointCloud(const QVector3D& center, const QVector3D& size, float spacing)
|
||
{
|
||
PointCloudXYZ cloud;
|
||
|
||
const int xSteps = std::max(1, static_cast<int>(std::ceil(size.x() / spacing)));
|
||
const int ySteps = std::max(1, static_cast<int>(std::ceil(size.y() / spacing)));
|
||
const int zSteps = std::max(1, static_cast<int>(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<size_t>((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<PointCloudConverter>())
|
||
, 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<QResizeEvent*>(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<int>(EulerRotationOrder::ZYX));
|
||
m_comboEulerOrder->addItem("XYZ (Roll-Pitch-Yaw)", static_cast<int>(EulerRotationOrder::XYZ));
|
||
m_comboEulerOrder->addItem("ZXY (Yaw-Roll-Pitch)", static_cast<int>(EulerRotationOrder::ZXY));
|
||
m_comboEulerOrder->addItem("YXZ (Pitch-Roll-Yaw)", static_cast<int>(EulerRotationOrder::YXZ));
|
||
m_comboEulerOrder->addItem("XZY (Roll-Yaw-Pitch)", static_cast<int>(EulerRotationOrder::XZY));
|
||
m_comboEulerOrder->addItem("YZX (Pitch-Yaw-Roll)", static_cast<int>(EulerRotationOrder::YZX));
|
||
m_comboEulerOrder->setCurrentIndex(0);
|
||
m_comboEulerOrder->setMaximumHeight(24);
|
||
connect(m_comboEulerOrder, QOverload<int>::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<int>(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<std::vector<SVzNLPointXYZRGBA>> 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<LineSegment> 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<uint8_t>((p1.nRGB >> 24) & 0xFF);
|
||
// A > 1 时作为线宽使用
|
||
float lineWidth = (a > 1) ? static_cast<float>(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<int>(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<PosePoint> 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<LineSegment> segments;
|
||
QVector<PosePoint> 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<float>(v["x"].toDouble());
|
||
vy[j] = static_cast<float>(v["y"].toDouble());
|
||
vz[j] = static_cast<float>(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<float>(pos["x"].toDouble());
|
||
float py = static_cast<float>(pos["y"].toDouble());
|
||
float pz = static_cast<float>(pos["z"].toDouble());
|
||
float roll = static_cast<float>(ori["roll"].toDouble());
|
||
float pitch = static_cast<float>(ori["pitch"].toDouble());
|
||
float yaw = static_cast<float>(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<float>(pos["x"].toDouble());
|
||
float py = static_cast<float>(pos["y"].toDouble());
|
||
float pz = static_cast<float>(pos["z"].toDouble());
|
||
float roll = static_cast<float>(ori["roll"].toDouble());
|
||
float pitch = static_cast<float>(ori["pitch"].toDouble());
|
||
float yaw = static_cast<float>(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<QVector3D> CloudViewMainWindow::getOriginalLinePoints(const SelectedLineInfo& lineInfo)
|
||
{
|
||
QVector<QVector3D> 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<int>(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<PosePoint> 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<PosePoint> 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<LineSegment> 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 矩阵作为“相机坐标系 -> 末端坐标系”的手眼标定矩阵。<br/>"
|
||
"输入当前机械臂位姿（mm / deg）后，可将所有点云转换到机械臂基坐标系。<br/>"
|
||
"角度组合方式：ZYX（Rz-Ry-Rx）。",
|
||
eyeGroup);
|
||
*/ lblEyeTip->setWordWrap(true);
|
||
/* lblEyeTip->setText(
|
||
"使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼标定矩阵。<br/>"
|
||
"输入当前机械臂位姿（mm / deg）后，可将所有点云转换到机械臂基坐标系。<br/>"
|
||
"角度组合方式：ZYX（Rz-Ry-Rx）。");
|
||
lblEyeTip->setText(
|
||
"使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼标定矩阵。<br/>"
|
||
"输入当前机械臂位姿（mm / deg）后，可将所有点云转换到机械臂基坐标系。<br/>"
|
||
"角度组合方式：ZYX（Rz-Ry-Rx）。");
|
||
*/ lblEyeTip->setTextFormat(Qt::RichText);
|
||
lblEyeTip->setText(
|
||
"使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼标定矩阵。<br/>"
|
||
"输入当前机械臂位姿（mm / deg）后，可将所有点云转换到机械臂基坐标系。<br/>"
|
||
"角度组合方式：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<QVector<float>> 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<float> 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<double>(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<QVector<float>> 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<float> 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 矩阵作为“相机坐标系 -> 末端坐标系”的手眼矩阵。<br/>"
|
||
"输入当前机械臂位姿（单位：mm / deg）后，可将所有点云转换到机械臂基坐标系。<br/>"
|
||
"角度组合方式：ZYX（Rz-Ry-Rx）。",
|
||
eyeGroup); */
|
||
lblEyeTip->setText("使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼矩阵。<br/>输入当前机械臂位姿（单位：mm / deg）后，可将所有点云转换到机械臂基坐标系。<br/>角度组合方式：ZYX（Rz-Ry-Rx）。");
|
||
lblEyeTip->setText("使用上方 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼矩阵。<br/>输入当前机械臂位姿（单位：mm / deg）后，可将所有点云转换到机械臂基坐标系。<br/>角度组合方式：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<QPair<QString, QMatrix4x4>> 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<std::vector<SVzNL3DPosition>> 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<int>(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<float>(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<float>(centerX->value()),
|
||
static_cast<float>(centerY->value()),
|
||
static_cast<float>(centerZ->value()));
|
||
const QVector3D size(static_cast<float>(sizeX->value()),
|
||
static_cast<float>(sizeY->value()),
|
||
static_cast<float>(sizeZ->value()));
|
||
const float pointSpacing = static_cast<float>(spacing->value());
|
||
|
||
const int gridX = std::max(1, static_cast<int>(std::ceil(size.x() / pointSpacing))) + 1;
|
||
const int gridY = std::max(1, static_cast<int>(std::ceil(size.y() / pointSpacing))) + 1;
|
||
const int gridZ = std::max(1, static_cast<int>(std::ceil(size.z() / pointSpacing))) + 1;
|
||
const double estimatedGridCount = static_cast<double>(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<int>(EulerRotationOrder::ZYX));
|
||
comboOrder->addItem("XYZ (Roll-Pitch-Yaw)", static_cast<int>(EulerRotationOrder::XYZ));
|
||
comboOrder->addItem("ZXY (Yaw-Roll-Pitch)", static_cast<int>(EulerRotationOrder::ZXY));
|
||
comboOrder->addItem("YXZ (Pitch-Roll-Yaw)", static_cast<int>(EulerRotationOrder::YXZ));
|
||
comboOrder->addItem("XZY (Roll-Yaw-Pitch)", static_cast<int>(EulerRotationOrder::XZY));
|
||
comboOrder->addItem("YZX (Pitch-Yaw-Roll)", static_cast<int>(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<std::array<QDoubleSpinBox*, 3>, 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<EulerRotationOrder>(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<float>(matCells[r][c]->value());
|
||
}
|
||
}
|
||
const EulerRotationOrder order = static_cast<EulerRotationOrder>(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<EulerRotationOrder>(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);
|
||
}
|
||
|