5356 lines
211 KiB
C++
5356 lines
211 KiB
C++
#include "CloudViewMainWindow.h"
|
||
#include "CloudViewSettings.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 <QColorDialog>
|
||
#include <QDoubleSpinBox>
|
||
#include <QGroupBox>
|
||
#include <QListWidget>
|
||
#include <QSettings>
|
||
#include <QStandardPaths>
|
||
#include <QProgressDialog>
|
||
#include <QCoreApplication>
|
||
#include <QDir>
|
||
#include <QDirIterator>
|
||
#include <QCloseEvent>
|
||
#include <cmath>
|
||
#include <cfloat>
|
||
#include <cctype>
|
||
#include <algorithm>
|
||
#include <array>
|
||
#include <utility>
|
||
#include "VrLog.h"
|
||
#include "LaserDataLoader.h"
|
||
|
||
namespace
|
||
{
|
||
constexpr double kPi = 3.14159265358979323846;
|
||
constexpr double kDegToRad = kPi / 180.0;
|
||
constexpr float kCircleR = 1.0f;
|
||
constexpr float kCircleG = 0.85f;
|
||
constexpr float kCircleB = 0.0f;
|
||
constexpr float kRectangleR = 1.0f;
|
||
constexpr float kRectangleG = 0.25f;
|
||
constexpr float kRectangleB = 0.9f;
|
||
constexpr float kSurfaceR = 0.0f;
|
||
constexpr float kSurfaceG = 0.8f;
|
||
constexpr float kSurfaceB = 1.0f;
|
||
constexpr float kSurfaceA = 0.5f;
|
||
constexpr float kShapeCenterR = 1.0f;
|
||
constexpr float kShapeCenterG = 0.1f;
|
||
constexpr float kShapeCenterB = 0.1f;
|
||
constexpr float kShapeCenterPointSize = 9.0f;
|
||
|
||
QString cloudViewExamplesDirectory()
|
||
{
|
||
QString dir = QStandardPaths::writableLocation(QStandardPaths::AppDataLocation);
|
||
if (dir.isEmpty()) {
|
||
dir = QDir::homePath() + "/CloudView";
|
||
}
|
||
dir = QDir(dir).filePath("Examples");
|
||
QDir().mkpath(dir);
|
||
return dir;
|
||
}
|
||
|
||
bool writeCloudViewExampleFile(const QString& fileName, const QString& content)
|
||
{
|
||
QFile file(QDir(cloudViewExamplesDirectory()).filePath(fileName));
|
||
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
|
||
return false;
|
||
}
|
||
QTextStream out(&file);
|
||
out << content;
|
||
return true;
|
||
}
|
||
|
||
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("矩阵格式无效，第 %1 行需要 4 个数值").arg(rows.size() + 1);
|
||
}
|
||
return false;
|
||
}
|
||
|
||
rows.append(row);
|
||
}
|
||
|
||
if (rows.size() != 4) {
|
||
if (errorMessage) {
|
||
*errorMessage = QString("矩阵需要 4 行数值，当前为 %1 行").arg(rows.size());
|
||
}
|
||
return false;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
void getShapeFrame(const PointCloudXYZ& cloud, QVector3D& center, QVector3D& size)
|
||
{
|
||
center = QVector3D(0.0f, 0.0f, 0.0f);
|
||
size = QVector3D(100.0f, 100.0f, 100.0f);
|
||
|
||
if (cloud.empty()) {
|
||
return;
|
||
}
|
||
|
||
QVector3D minBound(FLT_MAX, FLT_MAX, FLT_MAX);
|
||
QVector3D maxBound(-FLT_MAX, -FLT_MAX, -FLT_MAX);
|
||
for (const auto& point : cloud.points) {
|
||
minBound.setX(std::min(minBound.x(), point.x));
|
||
minBound.setY(std::min(minBound.y(), point.y));
|
||
minBound.setZ(std::min(minBound.z(), point.z));
|
||
maxBound.setX(std::max(maxBound.x(), point.x));
|
||
maxBound.setY(std::max(maxBound.y(), point.y));
|
||
maxBound.setZ(std::max(maxBound.z(), point.z));
|
||
}
|
||
|
||
center = (minBound + maxBound) * 0.5f;
|
||
size = maxBound - minBound;
|
||
}
|
||
|
||
QDoubleSpinBox* createShapeSpinBox(QWidget* parent, double value, double minValue = -1000000000.0, double maxValue = 1000000000.0)
|
||
{
|
||
QDoubleSpinBox* spin = new QDoubleSpinBox(parent);
|
||
spin->setDecimals(6);
|
||
spin->setRange(minValue, maxValue);
|
||
spin->setValue(value);
|
||
spin->setSingleStep(1.0);
|
||
spin->setAlignment(Qt::AlignRight);
|
||
spin->setButtonSymbols(QAbstractSpinBox::NoButtons);
|
||
return spin;
|
||
}
|
||
|
||
EulerRotationOrder currentShapeEulerOrder(const QComboBox* combo)
|
||
{
|
||
if (combo && combo->currentIndex() >= 0) {
|
||
return static_cast<EulerRotationOrder>(combo->currentData().toInt());
|
||
}
|
||
return EulerRotationOrder::ZYX;
|
||
}
|
||
|
||
QVector3D transformShapePoint(const QVector3D& center, const QMatrix4x4& rotation, const QVector3D& localPoint)
|
||
{
|
||
return center + rotation.map(localPoint);
|
||
}
|
||
|
||
QVector<LineSegment> makeClosedShapeSegments(const QVector<QVector3D>& points, float r, float g, float b)
|
||
{
|
||
QVector<LineSegment> segments;
|
||
if (points.size() < 2) {
|
||
return segments;
|
||
}
|
||
|
||
segments.reserve(points.size());
|
||
for (int i = 0; i < points.size(); ++i) {
|
||
const QVector3D& p1 = points[i];
|
||
const QVector3D& p2 = points[(i + 1) % points.size()];
|
||
segments.append(LineSegment(p1.x(), p1.y(), p1.z(),
|
||
p2.x(), p2.y(), p2.z(),
|
||
r, g, b, 3.0f));
|
||
}
|
||
return segments;
|
||
}
|
||
|
||
QVector<BasicShapePoint> makeShapeCenterPoint(const QVector3D& center)
|
||
{
|
||
QVector<BasicShapePoint> points;
|
||
points.append(BasicShapePoint(center.x(),
|
||
center.y(),
|
||
center.z(),
|
||
kShapeCenterR,
|
||
kShapeCenterG,
|
||
kShapeCenterB,
|
||
kShapeCenterPointSize));
|
||
return points;
|
||
}
|
||
|
||
QVector<LineSegment> makeCircleSegments(const QVector3D& center,
|
||
double rxDeg,
|
||
double ryDeg,
|
||
double rzDeg,
|
||
float radius,
|
||
EulerRotationOrder order)
|
||
{
|
||
QVector<LineSegment> segments;
|
||
if (radius <= 0.0f) {
|
||
return segments;
|
||
}
|
||
|
||
const QMatrix4x4 rotation = buildEulerRotationMatrix(order, rxDeg, ryDeg, rzDeg);
|
||
const int segmentCount = 96;
|
||
segments.reserve(segmentCount);
|
||
for (int i = 0; i < segmentCount; ++i) {
|
||
const float a1 = static_cast<float>(2.0 * kPi * i / segmentCount);
|
||
const float a2 = static_cast<float>(2.0 * kPi * (i + 1) / segmentCount);
|
||
const QVector3D p1 = transformShapePoint(
|
||
center, rotation, QVector3D(radius * std::cos(a1), radius * std::sin(a1), 0.0f));
|
||
const QVector3D p2 = transformShapePoint(
|
||
center, rotation, QVector3D(radius * std::cos(a2), radius * std::sin(a2), 0.0f));
|
||
segments.append(LineSegment(
|
||
p1.x(), p1.y(), p1.z(),
|
||
p2.x(), p2.y(), p2.z(),
|
||
kCircleR, kCircleG, kCircleB, 3.0f));
|
||
}
|
||
return segments;
|
||
}
|
||
|
||
QVector<LineSegment> makeRectangleSegments(const QVector3D& center,
|
||
double rxDeg,
|
||
double ryDeg,
|
||
double rzDeg,
|
||
float width,
|
||
float height,
|
||
EulerRotationOrder order,
|
||
float r = kRectangleR,
|
||
float g = kRectangleG,
|
||
float b = kRectangleB,
|
||
float lineWidth = 3.0f)
|
||
{
|
||
if (width <= 0.0f || height <= 0.0f) {
|
||
return {};
|
||
}
|
||
|
||
QVector<QVector3D> points;
|
||
points.reserve(4);
|
||
const QMatrix4x4 rotation = buildEulerRotationMatrix(order, rxDeg, ryDeg, rzDeg);
|
||
const float halfWidth = width * 0.5f;
|
||
const float halfHeight = height * 0.5f;
|
||
|
||
points.append(transformShapePoint(center, rotation, QVector3D(-halfWidth, -halfHeight, 0.0f)));
|
||
points.append(transformShapePoint(center, rotation, QVector3D( halfWidth, -halfHeight, 0.0f)));
|
||
points.append(transformShapePoint(center, rotation, QVector3D( halfWidth, halfHeight, 0.0f)));
|
||
points.append(transformShapePoint(center, rotation, QVector3D(-halfWidth, halfHeight, 0.0f)));
|
||
QVector<LineSegment> segments = makeClosedShapeSegments(points, r, g, b);
|
||
if (lineWidth > 0.0f) {
|
||
for (LineSegment& segment : segments) {
|
||
segment.lineWidth = lineWidth;
|
||
}
|
||
}
|
||
return segments;
|
||
}
|
||
|
||
QVector<QVector3D> makeSurfacePoints(const QVector3D& center,
|
||
double rxDeg,
|
||
double ryDeg,
|
||
double rzDeg,
|
||
float length,
|
||
float width,
|
||
EulerRotationOrder order)
|
||
{
|
||
QVector<QVector3D> points;
|
||
if (length <= 0.0f || width <= 0.0f) {
|
||
return points;
|
||
}
|
||
|
||
const QMatrix4x4 rotation = buildEulerRotationMatrix(order, rxDeg, ryDeg, rzDeg);
|
||
const float halfLength = length * 0.5f;
|
||
const float halfWidth = width * 0.5f;
|
||
|
||
points.reserve(4);
|
||
points.append(transformShapePoint(center, rotation, QVector3D(-halfLength, -halfWidth, 0.0f)));
|
||
points.append(transformShapePoint(center, rotation, QVector3D( halfLength, -halfWidth, 0.0f)));
|
||
points.append(transformShapePoint(center, rotation, QVector3D( halfLength, halfWidth, 0.0f)));
|
||
points.append(transformShapePoint(center, rotation, QVector3D(-halfLength, halfWidth, 0.0f)));
|
||
return points;
|
||
}
|
||
|
||
QVector<SurfaceQuad> makeSurfaceQuads(const QVector3D& center,
|
||
double rxDeg,
|
||
double ryDeg,
|
||
double rzDeg,
|
||
float length,
|
||
float width,
|
||
EulerRotationOrder order)
|
||
{
|
||
QVector<SurfaceQuad> surfaces;
|
||
const QVector<QVector3D> points = makeSurfacePoints(center, rxDeg, ryDeg, rzDeg, length, width, order);
|
||
if (points.size() != 4) {
|
||
return surfaces;
|
||
}
|
||
|
||
surfaces.append(SurfaceQuad(points[0], points[1], points[2], points[3], kSurfaceR, kSurfaceG, kSurfaceB, kSurfaceA));
|
||
return surfaces;
|
||
}
|
||
|
||
QVector<LineSegment> makeSurfaceOutlineSegments(const QVector3D& center,
|
||
double rxDeg,
|
||
double ryDeg,
|
||
double rzDeg,
|
||
float length,
|
||
float width,
|
||
EulerRotationOrder order)
|
||
{
|
||
const QVector<QVector3D> points = makeSurfacePoints(center, rxDeg, ryDeg, rzDeg, length, width, order);
|
||
return makeClosedShapeSegments(points, kSurfaceR, kSurfaceG, kSurfaceB);
|
||
}
|
||
|
||
void appendLineSegments(QVector<LineSegment>& target, const QVector<LineSegment>& source)
|
||
{
|
||
for (const LineSegment& segment : source) {
|
||
target.append(segment);
|
||
}
|
||
}
|
||
|
||
void appendBasicShapePoints(QVector<BasicShapePoint>& target, const QVector<BasicShapePoint>& source)
|
||
{
|
||
for (const BasicShapePoint& point : source) {
|
||
target.append(point);
|
||
}
|
||
}
|
||
|
||
void appendSurfaceQuads(QVector<SurfaceQuad>& target, const QVector<SurfaceQuad>& source)
|
||
{
|
||
for (const SurfaceQuad& surface : source) {
|
||
target.append(surface);
|
||
}
|
||
}
|
||
|
||
struct NumericRow
|
||
{
|
||
int lineNumber;
|
||
QVector<double> values;
|
||
};
|
||
|
||
QVector<double> parseNumericValuesFromLine(QString line)
|
||
{
|
||
const int hashIndex = line.indexOf('#');
|
||
if (hashIndex >= 0) {
|
||
line = line.left(hashIndex);
|
||
}
|
||
const int slashIndex = line.indexOf("//");
|
||
if (slashIndex >= 0) {
|
||
line = line.left(slashIndex);
|
||
}
|
||
|
||
line.replace(QRegExp("[,;:\\{\\}\\[\\]\\(\\)\"]"), " ");
|
||
|
||
QVector<double> values;
|
||
const QStringList parts = line.split(QRegExp("\\s+"), QString::SkipEmptyParts);
|
||
for (const QString& part : parts) {
|
||
bool ok = false;
|
||
const double value = part.toDouble(&ok);
|
||
if (ok) {
|
||
values.append(value);
|
||
}
|
||
}
|
||
return values;
|
||
}
|
||
|
||
bool loadNumericRowsFromFile(const QString& fileName, QVector<NumericRow>& rows, QString& errorMessage)
|
||
{
|
||
rows.clear();
|
||
|
||
QFile file(fileName);
|
||
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
|
||
errorMessage = QString("无法打开文件: %1").arg(file.errorString());
|
||
return false;
|
||
}
|
||
|
||
QTextStream in(&file);
|
||
int lineNumber = 0;
|
||
while (!in.atEnd()) {
|
||
++lineNumber;
|
||
const QString rawLine = in.readLine();
|
||
const QString trimmedLine = rawLine.trimmed();
|
||
// 参数化图形文件允许使用与图形块一致的数据头，例如
|
||
// Circle_0_1、Rectangle_0_1、Surface_0_1、Pose_0_1。
|
||
if (trimmedLine.contains(QRegExp("^[A-Za-z]+_[0-9]+_[0-9]+$"))) {
|
||
continue;
|
||
}
|
||
const QVector<double> values = parseNumericValuesFromLine(rawLine);
|
||
if (values.isEmpty()) {
|
||
continue;
|
||
}
|
||
rows.append(NumericRow{lineNumber, values});
|
||
}
|
||
|
||
if (rows.isEmpty()) {
|
||
errorMessage = "文件中没有可读取的数值";
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool makeCircleSegmentsFromRows(const QVector<NumericRow>& rows,
|
||
EulerRotationOrder order,
|
||
QVector<LineSegment>& segments,
|
||
QVector<BasicShapePoint>& centerPoints,
|
||
int& shapeCount,
|
||
QString& errorMessage)
|
||
{
|
||
constexpr int kRecordSize = 7;
|
||
segments.clear();
|
||
centerPoints.clear();
|
||
shapeCount = 0;
|
||
|
||
if (rows.isEmpty()) {
|
||
errorMessage = "圆数据格式应为每行一个 {x,y,z}-{r,p,y}-{r}";
|
||
return false;
|
||
}
|
||
|
||
for (const NumericRow& row : rows) {
|
||
const QVector<double>& values = row.values;
|
||
if (values.size() != kRecordSize) {
|
||
errorMessage = QString("第 %1 行圆数据格式应为 {x,y,z}-{r,p,y}-{r}，共 7 个数值").arg(row.lineNumber);
|
||
return false;
|
||
}
|
||
if (values[6] <= 0.0) {
|
||
errorMessage = QString("第 %1 行圆半径必须大于 0").arg(row.lineNumber);
|
||
return false;
|
||
}
|
||
|
||
const QVector3D center(static_cast<float>(values[0]),
|
||
static_cast<float>(values[1]),
|
||
static_cast<float>(values[2]));
|
||
appendLineSegments(segments, makeCircleSegments(center,
|
||
values[3],
|
||
values[4],
|
||
values[5],
|
||
static_cast<float>(values[6]),
|
||
order));
|
||
appendBasicShapePoints(centerPoints, makeShapeCenterPoint(center));
|
||
++shapeCount;
|
||
}
|
||
|
||
return shapeCount > 0;
|
||
}
|
||
|
||
bool makeRectangleSegmentsFromRows(const QVector<NumericRow>& rows,
|
||
EulerRotationOrder order,
|
||
QVector<LineSegment>& segments,
|
||
QVector<BasicShapePoint>& centerPoints,
|
||
int& shapeCount,
|
||
QString& errorMessage)
|
||
{
|
||
constexpr int kBaseRecordSize = 8;
|
||
constexpr int kColoredRecordSize = 12;
|
||
segments.clear();
|
||
centerPoints.clear();
|
||
shapeCount = 0;
|
||
|
||
if (rows.isEmpty()) {
|
||
errorMessage = "矩形数据格式应为每行一个 {x,y,z}-{r,p,y}-{w,h} 或 {x,y,z}-{r,p,y}-{w,h}-{R,G,B,A}";
|
||
return false;
|
||
}
|
||
|
||
for (const NumericRow& row : rows) {
|
||
const QVector<double>& values = row.values;
|
||
if (values.size() != kBaseRecordSize && values.size() != kColoredRecordSize) {
|
||
errorMessage = QString("第 %1 行矩形数据格式应为 {x,y,z}-{r,p,y}-{w,h} 或附加 {R,G,B,A}，共 8 或 12 个数值").arg(row.lineNumber);
|
||
return false;
|
||
}
|
||
if (values[6] <= 0.0 || values[7] <= 0.0) {
|
||
errorMessage = QString("第 %1 行矩形宽高必须大于 0").arg(row.lineNumber);
|
||
return false;
|
||
}
|
||
|
||
const QVector3D center(static_cast<float>(values[0]),
|
||
static_cast<float>(values[1]),
|
||
static_cast<float>(values[2]));
|
||
float r = kRectangleR;
|
||
float g = kRectangleG;
|
||
float b = kRectangleB;
|
||
float lineWidth = 3.0f;
|
||
if (values.size() == kColoredRecordSize) {
|
||
r = static_cast<float>(values[8] / 255.0);
|
||
g = static_cast<float>(values[9] / 255.0);
|
||
b = static_cast<float>(values[10] / 255.0);
|
||
// LineSegment stores RGB and line width; project shape files use A as line width.
|
||
lineWidth = static_cast<float>(values[11]);
|
||
if (lineWidth <= 0.0f) {
|
||
lineWidth = 3.0f;
|
||
}
|
||
}
|
||
appendLineSegments(segments, makeRectangleSegments(center,
|
||
values[3],
|
||
values[4],
|
||
values[5],
|
||
static_cast<float>(values[6]),
|
||
static_cast<float>(values[7]),
|
||
order,
|
||
r, g, b, lineWidth));
|
||
appendBasicShapePoints(centerPoints, makeShapeCenterPoint(center));
|
||
++shapeCount;
|
||
}
|
||
|
||
return shapeCount > 0;
|
||
}
|
||
|
||
bool makeSurfaceDataFromRows(const QVector<NumericRow>& rows,
|
||
EulerRotationOrder order,
|
||
QVector<SurfaceQuad>& surfaces,
|
||
QVector<LineSegment>& outlines,
|
||
int& shapeCount,
|
||
QString& errorMessage)
|
||
{
|
||
surfaces.clear();
|
||
outlines.clear();
|
||
shapeCount = 0;
|
||
|
||
if (rows.isEmpty()) {
|
||
errorMessage = "面数据格式应为每行一个 {x,y,z}-{l,w} 或 {x,y,z}-{r,p,y}-{l,w}";
|
||
return false;
|
||
}
|
||
|
||
for (const NumericRow& row : rows) {
|
||
const QVector<double>& values = row.values;
|
||
if (values.size() != 5 && values.size() != 8) {
|
||
errorMessage = QString("第 %1 行面数据格式应为 {x,y,z}-{l,w} 或 {x,y,z}-{r,p,y}-{l,w}").arg(row.lineNumber);
|
||
return false;
|
||
}
|
||
|
||
const bool hasPose = values.size() == 8;
|
||
const int sizeIndex = hasPose ? 6 : 3;
|
||
if (values[sizeIndex] <= 0.0 || values[sizeIndex + 1] <= 0.0) {
|
||
errorMessage = QString("第 %1 行面长宽必须大于 0").arg(row.lineNumber);
|
||
return false;
|
||
}
|
||
|
||
const QVector3D center(static_cast<float>(values[0]),
|
||
static_cast<float>(values[1]),
|
||
static_cast<float>(values[2]));
|
||
const double rx = hasPose ? values[3] : 0.0;
|
||
const double ry = hasPose ? values[4] : 0.0;
|
||
const double rz = hasPose ? values[5] : 0.0;
|
||
const float length = static_cast<float>(values[sizeIndex]);
|
||
const float width = static_cast<float>(values[sizeIndex + 1]);
|
||
|
||
appendSurfaceQuads(surfaces, makeSurfaceQuads(center, rx, ry, rz, length, width, order));
|
||
appendLineSegments(outlines, makeSurfaceOutlineSegments(center, rx, ry, rz, length, width, order));
|
||
++shapeCount;
|
||
}
|
||
|
||
return shapeCount > 0;
|
||
}
|
||
|
||
bool makeTriangleSegmentsFromRows(const QVector<NumericRow>& rows,
|
||
QVector<LineSegment>& segments,
|
||
int& shapeCount,
|
||
QString& errorMessage)
|
||
{
|
||
constexpr int kRecordSize = 9;
|
||
segments.clear();
|
||
shapeCount = 0;
|
||
|
||
if (rows.isEmpty()) {
|
||
errorMessage = "三角形数据格式应为每行一个 {x,y,z}-{x,y,z}-{x,y,z}";
|
||
return false;
|
||
}
|
||
|
||
for (const NumericRow& row : rows) {
|
||
const QVector<double>& values = row.values;
|
||
if (values.size() != kRecordSize) {
|
||
errorMessage = QString("第 %1 行三角形数据格式应为 {x,y,z}-{x,y,z}-{x,y,z}，共 9 个数值").arg(row.lineNumber);
|
||
return false;
|
||
}
|
||
|
||
QVector<QVector3D> points;
|
||
points.reserve(3);
|
||
for (int i = 0; i < 3; ++i) {
|
||
points.append(QVector3D(static_cast<float>(values[i * 3]),
|
||
static_cast<float>(values[i * 3 + 1]),
|
||
static_cast<float>(values[i * 3 + 2])));
|
||
}
|
||
appendLineSegments(segments, makeClosedShapeSegments(points, 0.2f, 1.0f, 0.2f));
|
||
++shapeCount;
|
||
}
|
||
|
||
return shapeCount > 0;
|
||
}
|
||
|
||
int maxPointIndexInLines(const PointCloudXYZ& cloud)
|
||
{
|
||
int maxPointCount = 0;
|
||
int previousLineIndex = 0;
|
||
int fallbackPointIndex = 0;
|
||
bool hasPreviousLine = false;
|
||
|
||
for (size_t i = 0; i < cloud.points.size(); ++i) {
|
||
const int lineIndex = (i < cloud.lineIndices.size())
|
||
? cloud.lineIndices[i] : -1;
|
||
if (!hasPreviousLine || lineIndex != previousLineIndex) {
|
||
fallbackPointIndex = 0;
|
||
previousLineIndex = lineIndex;
|
||
hasPreviousLine = true;
|
||
}
|
||
|
||
int pointIndex = (i < cloud.pointInLineIndices.size())
|
||
? cloud.pointInLineIndices[i] : -1;
|
||
if (pointIndex < 0) {
|
||
pointIndex = fallbackPointIndex;
|
||
}
|
||
maxPointCount = qMax(maxPointCount, pointIndex + 1);
|
||
++fallbackPointIndex;
|
||
}
|
||
|
||
return maxPointCount;
|
||
}
|
||
} // namespace
|
||
|
||
CloudViewMainWindow::CloudViewMainWindow(QWidget* parent)
|
||
: QMainWindow(parent)
|
||
, m_glWidget(nullptr)
|
||
, m_converter(std::make_unique<PointCloudConverter>())
|
||
, m_btnPointCloudRange(nullptr)
|
||
, m_lblDeltaXYZ(nullptr)
|
||
, m_cloudCount(0)
|
||
, m_currentLineNum(0)
|
||
, m_currentLinePtNum(0)
|
||
, m_linePointsDialog(nullptr)
|
||
, m_linePointsTable(nullptr)
|
||
, m_startupSequenceIndex(-1)
|
||
, m_startupSequentialMode(false)
|
||
{
|
||
setupUI();
|
||
const CloudViewSettingsData settings = CloudViewSettings::load();
|
||
setPointCloudRange(settings.pointCloudRange);
|
||
m_editPoseScale->setText(QString::number(settings.poseAxisLength, 'g', 8));
|
||
m_glWidget->setPseudoColorSettings(settings.pseudoColorEnabled,
|
||
settings.pseudoColorPeriod,
|
||
settings.pseudoColorStartColor,
|
||
settings.pseudoColorEndColor,
|
||
settings.pseudoColorDefaultPalette);
|
||
LOG_INFO("CloudViewMainWindow initialized\n");
|
||
}
|
||
|
||
CloudViewMainWindow::~CloudViewMainWindow()
|
||
{
|
||
CloudViewSettingsData settings;
|
||
bool poseScaleOk = false;
|
||
const float poseScale = m_editPoseScale->text().toFloat(&poseScaleOk);
|
||
if (poseScaleOk && std::isfinite(poseScale) && poseScale > 0.0f) {
|
||
settings.poseAxisLength = poseScale;
|
||
}
|
||
settings.pseudoColorEnabled = m_glWidget->isPseudoColorEnabled();
|
||
settings.pseudoColorPeriod = m_glWidget->pseudoColorPeriod();
|
||
settings.pseudoColorStartColor = m_glWidget->pseudoColorStartColor();
|
||
settings.pseudoColorEndColor = m_glWidget->pseudoColorEndColor();
|
||
settings.pseudoColorDefaultPalette = m_glWidget->isPseudoColorDefaultPalette();
|
||
settings.pointCloudRange = m_pointCloudRange;
|
||
CloudViewSettings::save(settings);
|
||
}
|
||
|
||
void CloudViewMainWindow::buildStartupSequence(const StartupFiles& files)
|
||
{
|
||
m_startupSequence.clear();
|
||
m_startupSequenceIndex = -1;
|
||
m_startupSequentialMode = false;
|
||
|
||
auto appendFiles = [this](StartupFileType type, const QStringList& fileNames) {
|
||
for (const QString& fileName : fileNames) {
|
||
m_startupSequence.append(StartupItem{type, fileName});
|
||
}
|
||
};
|
||
|
||
appendFiles(StartupFileType::Display, files.displayFiles);
|
||
appendFiles(StartupFileType::BBox, files.bboxFiles);
|
||
appendFiles(StartupFileType::Circle, files.circleFiles);
|
||
appendFiles(StartupFileType::Rectangle, files.rectangleFiles);
|
||
appendFiles(StartupFileType::Surface, files.surfaceFiles);
|
||
appendFiles(StartupFileType::Triangle, files.triangleFiles);
|
||
}
|
||
|
||
bool CloudViewMainWindow::loadStartupItem(const StartupItem& item)
|
||
{
|
||
switch (item.type) {
|
||
case StartupFileType::Display:
|
||
return loadDisplayFile(item.fileName);
|
||
case StartupFileType::BBox:
|
||
return loadBoundingBoxFile(item.fileName);
|
||
case StartupFileType::Circle:
|
||
return loadCircleShapeFile(item.fileName);
|
||
case StartupFileType::Rectangle:
|
||
return loadRectangleShapeFile(item.fileName);
|
||
case StartupFileType::Surface:
|
||
return loadSurfaceShapeFile(item.fileName);
|
||
case StartupFileType::Triangle:
|
||
return loadTriangleShapeFile(item.fileName);
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
bool CloudViewMainWindow::loadCurrentStartupSequenceItem()
|
||
{
|
||
if (m_startupSequenceIndex < 0 || m_startupSequenceIndex >= m_startupSequence.size()) {
|
||
return false;
|
||
}
|
||
|
||
const StartupItem& item = m_startupSequence[m_startupSequenceIndex];
|
||
const bool ok = loadStartupItem(item);
|
||
if (ok) {
|
||
fitViewToContent();
|
||
} else {
|
||
LOG_WARN("[CloudView] Startup sequence load failed: %s\n", item.fileName.toStdString().c_str());
|
||
}
|
||
|
||
statusBar()->showMessage(QString("序列显示 %1/%2：%3%4")
|
||
.arg(m_startupSequenceIndex + 1)
|
||
.arg(m_startupSequence.size())
|
||
.arg(QFileInfo(item.fileName).fileName())
|
||
.arg(ok ? "" : " 加载失败"));
|
||
return ok;
|
||
}
|
||
|
||
bool CloudViewMainWindow::loadStartupFiles(const StartupFiles& files)
|
||
{
|
||
if (files.sequential) {
|
||
buildStartupSequence(files);
|
||
if (m_startupSequence.isEmpty()) {
|
||
return true;
|
||
}
|
||
|
||
m_startupSequentialMode = true;
|
||
m_startupSequenceIndex = 0;
|
||
return loadCurrentStartupSequenceItem();
|
||
}
|
||
|
||
const int requestedCount =
|
||
files.displayFiles.size() +
|
||
files.bboxFiles.size() +
|
||
files.circleFiles.size() +
|
||
files.rectangleFiles.size() +
|
||
files.surfaceFiles.size() +
|
||
files.triangleFiles.size();
|
||
|
||
if (requestedCount == 0) {
|
||
return true;
|
||
}
|
||
|
||
int loadedCount = 0;
|
||
QStringList failedFiles;
|
||
|
||
auto recordLoadResult = [&](const QString& fileName, bool ok) {
|
||
if (ok) {
|
||
++loadedCount;
|
||
} else {
|
||
failedFiles.append(fileName);
|
||
}
|
||
};
|
||
|
||
for (const QString& fileName : files.displayFiles) {
|
||
recordLoadResult(fileName, loadDisplayFile(fileName));
|
||
}
|
||
for (const QString& fileName : files.bboxFiles) {
|
||
recordLoadResult(fileName, loadBoundingBoxFile(fileName));
|
||
}
|
||
for (const QString& fileName : files.circleFiles) {
|
||
recordLoadResult(fileName, loadCircleShapeFile(fileName));
|
||
}
|
||
for (const QString& fileName : files.rectangleFiles) {
|
||
recordLoadResult(fileName, loadRectangleShapeFile(fileName));
|
||
}
|
||
for (const QString& fileName : files.surfaceFiles) {
|
||
recordLoadResult(fileName, loadSurfaceShapeFile(fileName));
|
||
}
|
||
for (const QString& fileName : files.triangleFiles) {
|
||
recordLoadResult(fileName, loadTriangleShapeFile(fileName));
|
||
}
|
||
|
||
if (loadedCount > 0) {
|
||
fitViewToContent();
|
||
}
|
||
|
||
if (!failedFiles.isEmpty()) {
|
||
for (const QString& fileName : failedFiles) {
|
||
LOG_WARN("[CloudView] Startup load failed: %s\n", fileName.toStdString().c_str());
|
||
}
|
||
statusBar()->showMessage(QString("启动参数加载完成：%1/%2 成功，%3 失败")
|
||
.arg(loadedCount)
|
||
.arg(requestedCount)
|
||
.arg(failedFiles.size()));
|
||
return false;
|
||
}
|
||
|
||
statusBar()->showMessage(QString("启动参数加载完成：%1 个文件").arg(loadedCount));
|
||
return true;
|
||
}
|
||
|
||
bool CloudViewMainWindow::loadDisplayFile(const QString& fileName)
|
||
{
|
||
// 同一文件中可能同时包含点云和线段，两者都尝试加载。
|
||
const bool cloudOk = loadPointCloudFile(fileName);
|
||
const QString cloudStatusMessage = statusBar()->currentMessage();
|
||
const bool segmentOk = loadSegmentFile(fileName);
|
||
|
||
if (cloudOk && !segmentOk)
|
||
{
|
||
statusBar()->showMessage(cloudStatusMessage);
|
||
}
|
||
|
||
if (!cloudOk && !segmentOk) {
|
||
statusBar()->showMessage(QString("加载失败：%1").arg(QFileInfo(fileName).fileName()));
|
||
LOG_WARN("[CloudView] Failed to load display file: %s\n", fileName.toStdString().c_str());
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool CloudViewMainWindow::loadCircleShapeFile(const QString& fileName)
|
||
{
|
||
QVector<NumericRow> rows;
|
||
QString errorMessage;
|
||
if (!loadNumericRowsFromFile(fileName, rows, errorMessage)) {
|
||
statusBar()->showMessage(QString("加载圆数据失败：%1").arg(errorMessage));
|
||
LOG_WARN("[CloudView] Load circle file failed: %s, %s\n",
|
||
fileName.toStdString().c_str(), errorMessage.toStdString().c_str());
|
||
return false;
|
||
}
|
||
|
||
QVector<LineSegment> segments;
|
||
QVector<BasicShapePoint> centerPoints;
|
||
int shapeCount = 0;
|
||
if (!makeCircleSegmentsFromRows(rows, currentShapeEulerOrder(m_comboEulerOrder), segments, centerPoints, shapeCount, errorMessage)) {
|
||
statusBar()->showMessage(QString("加载圆数据失败：%1").arg(errorMessage));
|
||
LOG_WARN("[CloudView] Parse circle file failed: %s, %s\n",
|
||
fileName.toStdString().c_str(), errorMessage.toStdString().c_str());
|
||
return false;
|
||
}
|
||
|
||
showBasicShape(QString("%1 个圆").arg(shapeCount), segments, centerPoints);
|
||
m_cloudList->addItem(QString("Circle (%1) - %2 个").arg(QFileInfo(fileName).fileName()).arg(shapeCount));
|
||
return true;
|
||
}
|
||
|
||
bool CloudViewMainWindow::loadRectangleShapeFile(const QString& fileName)
|
||
{
|
||
QVector<NumericRow> rows;
|
||
QString errorMessage;
|
||
if (!loadNumericRowsFromFile(fileName, rows, errorMessage)) {
|
||
statusBar()->showMessage(QString("加载矩形数据失败：%1").arg(errorMessage));
|
||
LOG_WARN("[CloudView] Load rectangle file failed: %s, %s\n",
|
||
fileName.toStdString().c_str(), errorMessage.toStdString().c_str());
|
||
return false;
|
||
}
|
||
|
||
QVector<LineSegment> segments;
|
||
QVector<BasicShapePoint> centerPoints;
|
||
int shapeCount = 0;
|
||
if (!makeRectangleSegmentsFromRows(rows, currentShapeEulerOrder(m_comboEulerOrder), segments, centerPoints, shapeCount, errorMessage)) {
|
||
statusBar()->showMessage(QString("加载矩形数据失败：%1").arg(errorMessage));
|
||
LOG_WARN("[CloudView] Parse rectangle file failed: %s, %s\n",
|
||
fileName.toStdString().c_str(), errorMessage.toStdString().c_str());
|
||
return false;
|
||
}
|
||
|
||
showBasicShape(QString("%1 个矩形").arg(shapeCount), segments, centerPoints);
|
||
m_cloudList->addItem(QString("Rectangle (%1) - %2 个").arg(QFileInfo(fileName).fileName()).arg(shapeCount));
|
||
return true;
|
||
}
|
||
|
||
bool CloudViewMainWindow::loadSurfaceShapeFile(const QString& fileName)
|
||
{
|
||
QVector<NumericRow> rows;
|
||
QString errorMessage;
|
||
if (!loadNumericRowsFromFile(fileName, rows, errorMessage)) {
|
||
statusBar()->showMessage(QString("加载面数据失败：%1").arg(errorMessage));
|
||
LOG_WARN("[CloudView] Load surface file failed: %s, %s\n",
|
||
fileName.toStdString().c_str(), errorMessage.toStdString().c_str());
|
||
return false;
|
||
}
|
||
|
||
QVector<SurfaceQuad> surfaces;
|
||
QVector<LineSegment> outlines;
|
||
int shapeCount = 0;
|
||
if (!makeSurfaceDataFromRows(rows, currentShapeEulerOrder(m_comboEulerOrder), surfaces, outlines, shapeCount, errorMessage)) {
|
||
statusBar()->showMessage(QString("加载面数据失败：%1").arg(errorMessage));
|
||
LOG_WARN("[CloudView] Parse surface file failed: %s, %s\n",
|
||
fileName.toStdString().c_str(), errorMessage.toStdString().c_str());
|
||
return false;
|
||
}
|
||
|
||
showBasicSurface(QString("%1 个面").arg(shapeCount), surfaces, outlines);
|
||
m_cloudList->addItem(QString("Surface (%1) - %2 个").arg(QFileInfo(fileName).fileName()).arg(shapeCount));
|
||
return true;
|
||
}
|
||
|
||
bool CloudViewMainWindow::loadTriangleShapeFile(const QString& fileName)
|
||
{
|
||
QVector<NumericRow> rows;
|
||
QString errorMessage;
|
||
if (!loadNumericRowsFromFile(fileName, rows, errorMessage)) {
|
||
statusBar()->showMessage(QString("加载三角形数据失败：%1").arg(errorMessage));
|
||
LOG_WARN("[CloudView] Load triangle file failed: %s, %s\n",
|
||
fileName.toStdString().c_str(), errorMessage.toStdString().c_str());
|
||
return false;
|
||
}
|
||
|
||
QVector<LineSegment> segments;
|
||
int shapeCount = 0;
|
||
if (!makeTriangleSegmentsFromRows(rows, segments, shapeCount, errorMessage)) {
|
||
statusBar()->showMessage(QString("加载三角形数据失败：%1").arg(errorMessage));
|
||
LOG_WARN("[CloudView] Parse triangle file failed: %s, %s\n",
|
||
fileName.toStdString().c_str(), errorMessage.toStdString().c_str());
|
||
return false;
|
||
}
|
||
|
||
showBasicShape(QString("%1 个三角形").arg(shapeCount), segments);
|
||
m_cloudList->addItem(QString("Triangle (%1) - %2 个").arg(QFileInfo(fileName).fileName()).arg(shapeCount));
|
||
return true;
|
||
}
|
||
|
||
void CloudViewMainWindow::fitViewToContent()
|
||
{
|
||
if (m_glWidget) {
|
||
m_glWidget->fitToContent();
|
||
}
|
||
}
|
||
|
||
void CloudViewMainWindow::setPointCloudRange(const PointCloudXYZRange& range)
|
||
{
|
||
if (!range.isValid()) {
|
||
m_pointCloudRange = PointCloudXYZRange();
|
||
} else {
|
||
m_pointCloudRange = range;
|
||
}
|
||
if (m_converter) {
|
||
m_converter->setPointCloudRange(m_pointCloudRange);
|
||
}
|
||
if (m_btnPointCloudRange) {
|
||
m_btnPointCloudRange->setText(m_pointCloudRange.enabled
|
||
? "XYZ 范围过滤（已开启）..."
|
||
: "XYZ 范围过滤...");
|
||
}
|
||
}
|
||
|
||
bool CloudViewMainWindow::eventFilter(QObject* obj, QEvent* event)
|
||
{
|
||
if (obj == m_glWidget && event->type() == QEvent::Resize &&
|
||
m_btnZoomIn && m_btnZoomOut) {
|
||
const auto* resizeEvent = static_cast<QResizeEvent*>(event);
|
||
const int width = resizeEvent->size().width();
|
||
const int height = resizeEvent->size().height();
|
||
const int buttonWidth = m_btnZoomIn->width();
|
||
const int buttonHeight = m_btnZoomIn->height();
|
||
const int margin = 8;
|
||
const int gap = 4;
|
||
const int x = width - buttonWidth - margin;
|
||
const int y = qMax(0, (height - buttonHeight * 2 - gap) / 2);
|
||
m_btnZoomIn->move(x, y);
|
||
m_btnZoomOut->move(x, y + buttonHeight + gap);
|
||
}
|
||
return QMainWindow::eventFilter(obj, event);
|
||
}
|
||
|
||
void CloudViewMainWindow::closeEvent(QCloseEvent* event)
|
||
{
|
||
if (m_startupSequentialMode && m_startupSequenceIndex + 1 < m_startupSequence.size()) {
|
||
event->ignore();
|
||
++m_startupSequenceIndex;
|
||
onClearAll();
|
||
loadCurrentStartupSequenceItem();
|
||
return;
|
||
}
|
||
|
||
m_startupSequentialMode = false;
|
||
QMainWindow::closeEvent(event);
|
||
}
|
||
|
||
void CloudViewMainWindow::setupUI()
|
||
{
|
||
// ── 文件菜单 ──
|
||
QMenu* fileMenu = menuBar()->addMenu("文件");
|
||
QAction* actOpenFile = fileMenu->addAction("打开...\tCtrl+O");
|
||
actOpenFile->setShortcut(QKeySequence("Ctrl+O"));
|
||
connect(actOpenFile, &QAction::triggered, this, &CloudViewMainWindow::onOpenFile);
|
||
fileMenu->addSeparator();
|
||
QAction* actSaveCloud = fileMenu->addAction("保存txt点云...\tCtrl+S");
|
||
actSaveCloud->setShortcut(QKeySequence("Ctrl+S"));
|
||
connect(actSaveCloud, &QAction::triggered, this, &CloudViewMainWindow::onSavePointCloud);
|
||
QAction* actSavePlyPcd = fileMenu->addAction("保存ply/pcd点云...\tCtrl+Shift+S");
|
||
actSavePlyPcd->setShortcut(QKeySequence("Ctrl+Shift+S"));
|
||
connect(actSavePlyPcd, &QAction::triggered, this, &CloudViewMainWindow::onSavePlyPcdCloud);
|
||
fileMenu->addSeparator();
|
||
QAction* actBatchConvert = fileMenu->addAction("批量转换txt->ply...");
|
||
connect(actBatchConvert, &QAction::triggered, this, &CloudViewMainWindow::onBatchConvertTxtToPly);
|
||
|
||
// ── 点云菜单 ──
|
||
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* actPseudoColor = cloudMenu->addAction("伪彩色设置...");
|
||
connect(actPseudoColor, &QAction::triggered, this, &CloudViewMainWindow::onPseudoColorSettings);
|
||
QAction* actPointCloudRange = cloudMenu->addAction("XYZ 范围过滤...");
|
||
connect(actPointCloudRange, &QAction::triggered,
|
||
this, &CloudViewMainWindow::onPointCloudRangeSettings);
|
||
|
||
QMenu* toolMenu = menuBar()->addMenu("姿态工具");
|
||
QAction* actEulerPose = toolMenu->addAction("欧拉角与方向向量矩阵互转...");
|
||
connect(actEulerPose, &QAction::triggered, this, &CloudViewMainWindow::onConvertEulerMatrix);
|
||
QAction* actPoseComp = toolMenu->addAction("姿态补偿计算...");
|
||
connect(actPoseComp, &QAction::triggered, this, &CloudViewMainWindow::onPoseCompensation);
|
||
|
||
QMenu* imageToolMenu = menuBar()->addMenu("图形工具");
|
||
QAction* actOpenPosePoint = imageToolMenu->addAction("点(带姿态)");
|
||
connect(actOpenPosePoint, &QAction::triggered, this, &CloudViewMainWindow::onOpenPoseFile);
|
||
imageToolMenu->addSeparator();
|
||
QAction* actCircleShape = imageToolMenu->addAction("圆");
|
||
connect(actCircleShape, &QAction::triggered, this, &CloudViewMainWindow::onShowCircleShape);
|
||
QAction* actRectangleShape = imageToolMenu->addAction("矩形");
|
||
connect(actRectangleShape, &QAction::triggered, this, &CloudViewMainWindow::onShowRectangleShape);
|
||
QAction* actSurfaceShape = imageToolMenu->addAction("面");
|
||
connect(actSurfaceShape, &QAction::triggered, this, &CloudViewMainWindow::onShowSurfaceShape);
|
||
|
||
QMenu* transformMenu = menuBar()->addMenu("矩阵变换");
|
||
QAction* actEyeToHand = transformMenu->addAction("眼在手外...");
|
||
connect(actEyeToHand, &QAction::triggered, this, &CloudViewMainWindow::onEyeToHandTransform);
|
||
QAction* actEyeInHand = transformMenu->addAction("眼在手上...");
|
||
connect(actEyeInHand, &QAction::triggered, this, &CloudViewMainWindow::onEyeInHandTransform);
|
||
|
||
// Keep Help as the last menu on the menu bar.
|
||
QMenu* helpMenu = menuBar()->addMenu("帮助");
|
||
QAction* actFormatHelp = helpMenu->addAction("数据格式示例...");
|
||
connect(actFormatHelp, &QAction::triggered, this, &CloudViewMainWindow::onShowDataFormatHelp);
|
||
// 创建中央控件
|
||
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);
|
||
|
||
// 左侧：CloudView 自己的视图工具栏 + 点云显示区域
|
||
QWidget* viewerContainer = new QWidget(splitter);
|
||
QHBoxLayout* viewerLayout = new QHBoxLayout(viewerContainer);
|
||
viewerLayout->setContentsMargins(0, 0, 0, 0);
|
||
viewerLayout->setSpacing(2);
|
||
|
||
// 视图方向工具栏（保持 CloudView 原有布局）
|
||
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, false);
|
||
m_glWidget->setMinimumSize(400, 300);
|
||
m_glWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
|
||
layout->addWidget(m_glWidget);
|
||
|
||
// CloudView 自己的悬浮缩放按钮
|
||
const int zoomBtnSize = 28;
|
||
const 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);
|
||
|
||
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::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;
|
||
};
|
||
const 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}
|
||
};
|
||
|
||
const int buttonSize = 32;
|
||
for (const ViewPreset& preset : presets) {
|
||
QToolButton* button = new QToolButton(widget);
|
||
button->setIcon(QIcon(preset.iconPath));
|
||
button->setIconSize(QSize(24, 24));
|
||
button->setFixedSize(buttonSize, buttonSize);
|
||
button->setToolTip(preset.tooltip);
|
||
button->setAutoRaise(true);
|
||
const float rx = preset.rotX;
|
||
const float ry = preset.rotY;
|
||
const float rz = preset.rotZ;
|
||
connect(button, &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(button);
|
||
}
|
||
layout->addStretch();
|
||
|
||
m_editRotX = new QLineEdit("180.0", widget);
|
||
m_editRotY = new QLineEdit("0.0", widget);
|
||
m_editRotZ = new QLineEdit("0.0", widget);
|
||
auto addAngleRow = [widget, layout, buttonSize](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* caption = new QLabel(label, row);
|
||
caption->setStyleSheet("font-size: 10px; color: gray;");
|
||
rowLayout->addWidget(caption);
|
||
edit->setFixedWidth(buttonSize);
|
||
edit->setMaximumHeight(20);
|
||
edit->setStyleSheet("font-size: 10px;");
|
||
rowLayout->addWidget(edit);
|
||
layout->addWidget(row);
|
||
};
|
||
addAngleRow("RX", m_editRotX);
|
||
addAngleRow("RY", m_editRotY);
|
||
addAngleRow("RZ", m_editRotZ);
|
||
|
||
QPushButton* applyButton = new QPushButton("GO", widget);
|
||
applyButton->setFixedSize(buttonSize, 20);
|
||
applyButton->setStyleSheet("font-size: 10px;");
|
||
auto applyAngles = [this]() {
|
||
bool okX = false, okY = false, okZ = false;
|
||
const float rx = m_editRotX->text().toFloat(&okX);
|
||
const float ry = m_editRotY->text().toFloat(&okY);
|
||
const float rz = m_editRotZ->text().toFloat(&okZ);
|
||
if (okX && okY && okZ) {
|
||
m_glWidget->setViewAngles(rx, ry, rz);
|
||
}
|
||
};
|
||
connect(applyButton, &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(applyButton);
|
||
widget->setFixedWidth(buttonSize + 8);
|
||
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(), "选线");
|
||
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_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_btnPointCloudRange = new QPushButton("XYZ 范围过滤...", group);
|
||
m_btnPointCloudRange->setMinimumHeight(24);
|
||
m_btnPointCloudRange->setMaximumHeight(24);
|
||
connect(m_btnPointCloudRange, &QPushButton::clicked,
|
||
this, &CloudViewMainWindow::onPointCloudRangeSettings);
|
||
layout->addWidget(m_btnPointCloudRange);
|
||
|
||
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::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: --");
|
||
clearMeasureResultDisplay();
|
||
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: 20px;");
|
||
distLayout->addWidget(lblDistTitle);
|
||
distLayout->addWidget(m_lblDistance, 1);
|
||
layout->addLayout(distLayout);
|
||
|
||
QHBoxLayout* deltaLayout = new QHBoxLayout();
|
||
QLabel* lblDeltaTitle = new QLabel("差值:", group);
|
||
lblDeltaTitle->setStyleSheet("font-size: 10px;");
|
||
m_lblDeltaXYZ = new QLabel("--", group);
|
||
m_lblDeltaXYZ->setStyleSheet("font-weight: bold; color: #1f6feb; font-size: 13px;");
|
||
deltaLayout->addWidget(lblDeltaTitle);
|
||
deltaLayout->addWidget(m_lblDeltaXYZ, 1);
|
||
layout->addLayout(deltaLayout);
|
||
|
||
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 *.dat);;PCD 文件 (*.pcd);;PLY 文件 (*.ply);;TXT 文件 (*.txt);;DAT 文件 (*.dat);;所有文件 (*.*)"
|
||
);
|
||
|
||
if (fileName.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
if (!loadDisplayFile(fileName)) {
|
||
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());
|
||
|
||
// 文本/DAT 激光扫描文件支持按扫描线回调，边读边显示；PCD/PLY 继续使用原有解析器。
|
||
if (ext == "txt" || ext == "dat") {
|
||
PointCloudXYZ loadedOriginalCloud;
|
||
bool cloudAdded = false;
|
||
bool hadColor = false;
|
||
int callbackLineCount = 0;
|
||
int callbackMaxSourcePointCount = 0;
|
||
size_t callbackPointCount = 0;
|
||
int callbackResult = -1;
|
||
|
||
const int result = m_converter->loadFromFileByCallback(
|
||
fileName.toStdString(),
|
||
[&](const PointCloudXYZRGB& chunk, bool chunkHasColor,
|
||
int sourceLineIndex, int sourcePointCount)
|
||
{
|
||
hadColor = hadColor || chunkHasColor;
|
||
|
||
if (chunkHasColor) {
|
||
for (size_t i = 0; i < chunk.points.size(); ++i) {
|
||
const Point3DRGB& point = chunk.points[i];
|
||
const int lineIndex = (i < chunk.lineIndices.size()) ? chunk.lineIndices[i] : 0;
|
||
const int pointIndex = (i < chunk.pointInLineIndices.size())
|
||
? chunk.pointInLineIndices[i] : static_cast<int>(i);
|
||
loadedOriginalCloud.push_back(Point3D(point.x, point.y, point.z),
|
||
lineIndex, pointIndex);
|
||
}
|
||
if (!chunk.empty()) {
|
||
if (!cloudAdded) {
|
||
m_glWidget->addPointCloud(chunk, cloudName, false);
|
||
cloudAdded = true;
|
||
} else {
|
||
m_glWidget->appendPointCloud(chunk, cloudName);
|
||
}
|
||
}
|
||
} else {
|
||
PointCloudXYZ xyzChunk;
|
||
xyzChunk.reserve(chunk.size());
|
||
for (size_t i = 0; i < chunk.points.size(); ++i) {
|
||
const Point3DRGB& point = chunk.points[i];
|
||
const int lineIndex = (i < chunk.lineIndices.size()) ? chunk.lineIndices[i] : 0;
|
||
const Point3D xyz(point.x, point.y, point.z);
|
||
const int pointIndex = (i < chunk.pointInLineIndices.size())
|
||
? chunk.pointInLineIndices[i] : static_cast<int>(i);
|
||
xyzChunk.push_back(xyz, lineIndex, pointIndex);
|
||
loadedOriginalCloud.push_back(xyz, lineIndex, pointIndex);
|
||
}
|
||
if (!xyzChunk.empty()) {
|
||
if (!cloudAdded) {
|
||
m_glWidget->addPointCloud(xyzChunk, cloudName, false);
|
||
cloudAdded = true;
|
||
} else {
|
||
m_glWidget->appendPointCloud(xyzChunk, cloudName);
|
||
}
|
||
}
|
||
}
|
||
|
||
callbackLineCount = qMax(callbackLineCount, sourceLineIndex + 1);
|
||
callbackMaxSourcePointCount =
|
||
qMax(callbackMaxSourcePointCount, sourcePointCount);
|
||
callbackPointCount += chunk.size();
|
||
if ((callbackLineCount % 4) == 0) {
|
||
statusBar()->showMessage(QString("正在加载点云：已加载 %1 条扫描线，%2 个点...")
|
||
.arg(callbackLineCount)
|
||
.arg(static_cast<qulonglong>(callbackPointCount)));
|
||
QCoreApplication::processEvents();
|
||
}
|
||
},
|
||
[&](int finishedResult, int finishedLineCount, size_t finishedPointCount, bool finishedHasColor) {
|
||
callbackResult = finishedResult;
|
||
callbackLineCount = qMax(callbackLineCount, finishedLineCount);
|
||
callbackPointCount = qMax(callbackPointCount, finishedPointCount);
|
||
hadColor = hadColor || finishedHasColor;
|
||
});
|
||
|
||
QCoreApplication::processEvents();
|
||
|
||
if (result != 0 || callbackResult != 0) {
|
||
if (cloudAdded) {
|
||
m_glWidget->removeNewestPointCloud();
|
||
}
|
||
--m_cloudCount;
|
||
LOG_INFO("[CloudView] Callback load failed: %s\n", m_converter->getLastError().c_str());
|
||
return false;
|
||
}
|
||
|
||
if (m_converter->getLoadedPointCount() == 0) {
|
||
--m_cloudCount;
|
||
statusBar()->showMessage("点云读取成功，但当前 XYZ 范围内没有点");
|
||
return true;
|
||
}
|
||
|
||
m_originalCloud = std::move(loadedOriginalCloud);
|
||
m_currentLineNum = m_converter->getLoadedLineCount();
|
||
m_currentLinePtNum = (m_currentLineNum > 0)
|
||
? qMax(callbackMaxSourcePointCount, maxPointIndexInLines(m_originalCloud)) : 0;
|
||
m_glWidget->setNewestPointCloudLineLayout(m_currentLineNum, m_currentLinePtNum);
|
||
|
||
QString itemText = QString("%1 - %2 点, %3 线")
|
||
.arg(cloudName)
|
||
.arg(static_cast<qulonglong>(m_converter->getLoadedPointCount()))
|
||
.arg(m_converter->getLoadedLineCount());
|
||
if (hadColor) {
|
||
itemText += " [彩色]";
|
||
}
|
||
m_cloudList->addItem(itemText);
|
||
statusBar()->showMessage(QString("已加载 %1 个点%2")
|
||
.arg(static_cast<qulonglong>(m_converter->getLoadedPointCount()))
|
||
.arg(hadColor ? " (彩色)" : ""));
|
||
// 过滤器可能合法地排除全部点；读取成功不应被报告为文件错误。
|
||
return true;
|
||
}
|
||
|
||
// 统一使用 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;
|
||
}
|
||
|
||
if (rgbCloud.empty()) {
|
||
--m_cloudCount;
|
||
statusBar()->showMessage("点云读取成功，但当前 XYZ 范围内没有点");
|
||
return true;
|
||
}
|
||
|
||
// 保存原始完整点云 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);
|
||
if (i < rgbCloud.lineIndices.size())
|
||
{
|
||
const int lineIdx = rgbCloud.lineIndices[i];
|
||
const int pointIdx = (i < rgbCloud.pointInLineIndices.size())
|
||
? rgbCloud.pointInLineIndices[i] : static_cast<int>(i);
|
||
m_originalCloud.push_back(xyzPt, lineIdx, pointIdx);
|
||
}
|
||
else
|
||
{
|
||
// PCD/PLY 通常没有扫描线，不能用 0 伪造一条线。
|
||
m_originalCloud.push_back(xyzPt);
|
||
}
|
||
}
|
||
|
||
// 根据是否有颜色选择显示方式
|
||
const bool hadColor = m_converter->lastLoadHadColor();
|
||
|
||
if (ext == "pcd" || ext == "ply")
|
||
{
|
||
if (hadColor)
|
||
{
|
||
// 带 RGB 字段的文件保留源颜色；无 RGB 文件走纯 XYZ 路径以启用伪彩。
|
||
m_glWidget->addPointCloud(rgbCloud, cloudName);
|
||
LOG_INFO("[CloudView] PCD/PLY loaded with original RGB colors, points: %zu\n",
|
||
rgbCloud.size());
|
||
}
|
||
else
|
||
{
|
||
m_glWidget->addPointCloud(m_originalCloud, cloudName);
|
||
LOG_INFO("[CloudView] PCD/PLY loaded without RGB; using Z pseudo-color, points: %zu\n",
|
||
m_originalCloud.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 (Z pseudo-color), points: %zu\n", m_originalCloud.size());
|
||
}
|
||
|
||
// 保存线信息（用于旋转功能）
|
||
int lineCount = m_converter->getLoadedLineCount();
|
||
if (lineCount > 0) {
|
||
m_currentLineNum = lineCount;
|
||
m_currentLinePtNum = maxPointIndexInLines(m_originalCloud);
|
||
} 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>> shapeBlocks;
|
||
int result = loader.LoadShapeBlocks(fileName.toStdString(), shapeBlocks);
|
||
|
||
if (result != 0) {
|
||
LOG_INFO("[CloudView] Load segments failed: %s\n", loader.GetLastError().c_str());
|
||
return false;
|
||
}
|
||
|
||
if (shapeBlocks.empty()) {
|
||
return false;
|
||
}
|
||
|
||
// 将图形块按几何点数转换为线段/三角形/四边形面；保留所有原始轮廓线。
|
||
QVector<LineSegment> segments;
|
||
QVector<SurfaceQuad> surfaces;
|
||
int triangleCount = 0;
|
||
int quadCount = 0;
|
||
int closedOutlineCount = 0;
|
||
for (const auto& polyLine : shapeBlocks) {
|
||
if (polyLine.size() < 2) {
|
||
continue;
|
||
}
|
||
|
||
const auto& first = polyLine.front();
|
||
const float r = ((first.nRGB >> 0) & 0xFF) / 255.0f;
|
||
const float g = ((first.nRGB >> 8) & 0xFF) / 255.0f;
|
||
const float b = ((first.nRGB >> 16) & 0xFF) / 255.0f;
|
||
const uint8_t a = static_cast<uint8_t>((first.nRGB >> 24) & 0xFF);
|
||
const float lineWidth = (a > 1) ? static_cast<float>(a) : 0.0f;
|
||
|
||
for (size_t i = 0; i + 1 < polyLine.size(); ++i) {
|
||
const auto& p1 = polyLine[i];
|
||
const auto& p2 = polyLine[i + 1];
|
||
segments.append(LineSegment(
|
||
p1.x, p1.y, p1.z,
|
||
p2.x, p2.y, p2.z,
|
||
r, g, b, lineWidth));
|
||
}
|
||
|
||
const bool explicitlyClosed =
|
||
std::fabs(polyLine.front().x - polyLine.back().x) < 1e-5f &&
|
||
std::fabs(polyLine.front().y - polyLine.back().y) < 1e-5f &&
|
||
std::fabs(polyLine.front().z - polyLine.back().z) < 1e-5f;
|
||
const bool isTriangle = polyLine.size() == 3 ||
|
||
(polyLine.size() == 4 && explicitlyClosed);
|
||
const bool isQuad = (polyLine.size() == 4 && !explicitlyClosed) ||
|
||
(polyLine.size() == 5 && explicitlyClosed);
|
||
if (isTriangle || isQuad) {
|
||
// 三点（或首尾闭合的四点）是三角形；四点（或闭合五点）是四边形/矩形面。
|
||
if (isTriangle) {
|
||
const auto& p1 = polyLine[0];
|
||
const auto& p2 = polyLine[1];
|
||
const auto& p3 = polyLine[2];
|
||
if (!explicitlyClosed) {
|
||
segments.append(LineSegment(p3.x, p3.y, p3.z,
|
||
p1.x, p1.y, p1.z,
|
||
r, g, b, lineWidth));
|
||
}
|
||
++triangleCount;
|
||
} else {
|
||
const auto& p1 = polyLine[0];
|
||
const auto& p2 = polyLine[1];
|
||
const auto& p3 = polyLine[2];
|
||
const auto& p4 = polyLine[3];
|
||
surfaces.append(SurfaceQuad(
|
||
QVector3D(p1.x, p1.y, p1.z),
|
||
QVector3D(p2.x, p2.y, p2.z),
|
||
QVector3D(p3.x, p3.y, p3.z),
|
||
QVector3D(p4.x, p4.y, p4.z),
|
||
r, g, b, 0.45f));
|
||
if (!explicitlyClosed) {
|
||
segments.append(LineSegment(p4.x, p4.y, p4.z,
|
||
p1.x, p1.y, p1.z,
|
||
r, g, b, lineWidth));
|
||
}
|
||
++quadCount;
|
||
}
|
||
} else if (polyLine.size() > 3) {
|
||
// 多点闭合轮廓按圆/闭合轮廓处理；若文件未重复首点，则补最后一条边。
|
||
if (!explicitlyClosed) {
|
||
const auto& last = polyLine.back();
|
||
segments.append(LineSegment(last.x, last.y, last.z,
|
||
first.x, first.y, first.z,
|
||
r, g, b, lineWidth));
|
||
}
|
||
++closedOutlineCount;
|
||
}
|
||
}
|
||
|
||
if (segments.isEmpty() && surfaces.isEmpty()) {
|
||
return false;
|
||
}
|
||
|
||
if (!segments.isEmpty()) {
|
||
m_glWidget->addLineSegments(segments);
|
||
}
|
||
if (!surfaces.isEmpty()) {
|
||
m_glWidget->appendBasicShapeSurfaces(surfaces);
|
||
}
|
||
m_glWidget->fitToContent();
|
||
|
||
// 添加到列表
|
||
QFileInfo fileInfo(fileName);
|
||
int totalShapeCount = static_cast<int>(shapeBlocks.size());
|
||
QString itemName = QString("Segments (%1)").arg(fileInfo.fileName());
|
||
m_cloudList->addItem(QString("%1 - %2 个图形块, %3 条线段（%4 三角形，%5 四边形，%6 闭合轮廓）")
|
||
.arg(itemName).arg(totalShapeCount).arg(segments.size())
|
||
.arg(triangleCount).arg(quadCount).arg(closedOutlineCount));
|
||
|
||
statusBar()->showMessage(QString("已加载 %1 个图形块、%2 条线段、%3 个面")
|
||
.arg(totalShapeCount).arg(segments.size()).arg(surfaces.size()));
|
||
LOG_INFO("[CloudView] Loaded %d shape blocks, %d segments, %d surfaces from %s\n",
|
||
totalShapeCount, segments.size(), surfaces.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;
|
||
|
||
// 使用界面上配置的坐标轴长度
|
||
bool scaleOk = true;
|
||
float scale = m_editPoseScale->text().toFloat(&scaleOk);
|
||
if (!scaleOk || scale <= 0) 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]);
|
||
|
||
// 使用界面上配置的坐标轴长度
|
||
bool poseScaleOk = true;
|
||
float poseScale = m_editPoseScale->text().toFloat(&poseScaleOk);
|
||
if (!poseScaleOk || poseScale <= 0) poseScale = 10.0f;
|
||
|
||
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, poseScale));
|
||
}
|
||
|
||
// 解析左抓取点
|
||
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, poseScale));
|
||
}
|
||
}
|
||
|
||
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->clearBasicShapeSegments();
|
||
m_glWidget->clearPosePoints();
|
||
|
||
m_lblPoint1->setText("点1: --");
|
||
m_lblPoint2->setText("点2: --");
|
||
clearMeasureResultDisplay();
|
||
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::showBasicShape(const QString& shapeName,
|
||
const QVector<LineSegment>& segments,
|
||
const QVector<BasicShapePoint>& points)
|
||
{
|
||
if (segments.isEmpty() && points.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
if (!segments.isEmpty()) {
|
||
m_glWidget->appendBasicShapeSegments(segments);
|
||
}
|
||
if (!points.isEmpty()) {
|
||
m_glWidget->appendBasicShapePoints(points);
|
||
}
|
||
m_glWidget->fitToContent();
|
||
statusBar()->showMessage(QString("已添加 %1").arg(shapeName));
|
||
LOG_INFO("[CloudView] Add basic shape: %s, segments=%d, points=%d\n",
|
||
shapeName.toStdString().c_str(), segments.size(), points.size());
|
||
}
|
||
|
||
void CloudViewMainWindow::showBasicSurface(const QString& shapeName, const QVector<SurfaceQuad>& surfaces, const QVector<LineSegment>& outlines)
|
||
{
|
||
if (surfaces.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
m_glWidget->appendBasicShapeSurfaces(surfaces);
|
||
m_glWidget->appendBasicShapeSegments(outlines);
|
||
m_glWidget->fitToContent();
|
||
statusBar()->showMessage(QString("已添加 %1").arg(shapeName));
|
||
LOG_INFO("[CloudView] Add basic surface: %s, surfaces=%d, outlines=%d\n",
|
||
shapeName.toStdString().c_str(), surfaces.size(), outlines.size());
|
||
}
|
||
|
||
void CloudViewMainWindow::onShowCircleShape()
|
||
{
|
||
QVector3D center;
|
||
QVector3D size;
|
||
getShapeFrame(m_originalCloud, center, size);
|
||
const double defaultRadius = std::max(1.0f, std::max(size.x(), size.y()) * 0.25f);
|
||
|
||
QDialog dialog(this);
|
||
dialog.setWindowTitle("圆");
|
||
dialog.setModal(true);
|
||
|
||
QVBoxLayout* layout = new QVBoxLayout(&dialog);
|
||
|
||
QLabel* tip = new QLabel("输入圆心、姿态和半径后点击添加；文件每行一个：{x,y,z}-{r,p,y}-{r}。", &dialog);
|
||
tip->setWordWrap(true);
|
||
tip->setStyleSheet("color: gray;");
|
||
layout->addWidget(tip);
|
||
|
||
QGroupBox* centerGroup = new QGroupBox("圆心", &dialog);
|
||
QFormLayout* centerLayout = new QFormLayout(centerGroup);
|
||
QDoubleSpinBox* centerX = createShapeSpinBox(centerGroup, center.x());
|
||
QDoubleSpinBox* centerY = createShapeSpinBox(centerGroup, center.y());
|
||
QDoubleSpinBox* centerZ = createShapeSpinBox(centerGroup, center.z());
|
||
centerLayout->addRow("X:", centerX);
|
||
centerLayout->addRow("Y:", centerY);
|
||
centerLayout->addRow("Z:", centerZ);
|
||
layout->addWidget(centerGroup);
|
||
|
||
QGroupBox* poseGroup = new QGroupBox("姿态", &dialog);
|
||
QFormLayout* poseLayout = new QFormLayout(poseGroup);
|
||
QDoubleSpinBox* poseR = createShapeSpinBox(poseGroup, 0.0);
|
||
QDoubleSpinBox* poseP = createShapeSpinBox(poseGroup, 0.0);
|
||
QDoubleSpinBox* poseY = createShapeSpinBox(poseGroup, 0.0);
|
||
poseLayout->addRow("R/RX:", poseR);
|
||
poseLayout->addRow("P/RY:", poseP);
|
||
poseLayout->addRow("Y/RZ:", poseY);
|
||
layout->addWidget(poseGroup);
|
||
|
||
QGroupBox* radiusGroup = new QGroupBox("尺寸", &dialog);
|
||
QFormLayout* radiusLayout = new QFormLayout(radiusGroup);
|
||
QDoubleSpinBox* radius = createShapeSpinBox(radiusGroup, defaultRadius, 0.000001);
|
||
radiusLayout->addRow("半径:", radius);
|
||
layout->addWidget(radiusGroup);
|
||
|
||
auto addCurrentCircle = [this, centerX, centerY, centerZ, poseR, poseP, poseY, radius]() {
|
||
const QVector3D inputCenter(static_cast<float>(centerX->value()),
|
||
static_cast<float>(centerY->value()),
|
||
static_cast<float>(centerZ->value()));
|
||
showBasicShape("圆",
|
||
makeCircleSegments(inputCenter,
|
||
poseR->value(),
|
||
poseP->value(),
|
||
poseY->value(),
|
||
static_cast<float>(radius->value()),
|
||
currentShapeEulerOrder(m_comboEulerOrder)),
|
||
makeShapeCenterPoint(inputCenter));
|
||
};
|
||
|
||
QPushButton* loadButton = new QPushButton("加载文件", &dialog);
|
||
connect(loadButton, &QPushButton::clicked, this, [this, centerX, centerY, centerZ, poseR, poseP, poseY, radius]() {
|
||
const QString fileName = QFileDialog::getOpenFileName(
|
||
this, "加载圆数据", QString(),
|
||
"数据文件 (*.txt *.json *.dat);;所有文件 (*.*)");
|
||
if (fileName.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
QVector<NumericRow> rows;
|
||
QString errorMessage;
|
||
if (!loadNumericRowsFromFile(fileName, rows, errorMessage)) {
|
||
QMessageBox::warning(this, "错误", errorMessage);
|
||
return;
|
||
}
|
||
|
||
QVector<LineSegment> segments;
|
||
QVector<BasicShapePoint> centerPoints;
|
||
int shapeCount = 0;
|
||
if (!makeCircleSegmentsFromRows(rows, currentShapeEulerOrder(m_comboEulerOrder), segments, centerPoints, shapeCount, errorMessage)) {
|
||
QMessageBox::warning(this, "错误", errorMessage);
|
||
return;
|
||
}
|
||
|
||
if (shapeCount == 1) {
|
||
const QVector<double>& values = rows.first().values;
|
||
centerX->setValue(values[0]);
|
||
centerY->setValue(values[1]);
|
||
centerZ->setValue(values[2]);
|
||
poseR->setValue(values[3]);
|
||
poseP->setValue(values[4]);
|
||
poseY->setValue(values[5]);
|
||
radius->setValue(values[6]);
|
||
}
|
||
|
||
showBasicShape(QString("%1 个圆").arg(shapeCount), segments, centerPoints);
|
||
});
|
||
|
||
QPushButton* addButton = new QPushButton("添加", &dialog);
|
||
connect(addButton, &QPushButton::clicked, this, addCurrentCircle);
|
||
|
||
QHBoxLayout* actionLayout = new QHBoxLayout();
|
||
actionLayout->addWidget(loadButton);
|
||
actionLayout->addWidget(addButton);
|
||
actionLayout->addStretch();
|
||
layout->addLayout(actionLayout);
|
||
|
||
QDialogButtonBox* buttonBox = new QDialogButtonBox(QDialogButtonBox::Close, &dialog);
|
||
connect(buttonBox, &QDialogButtonBox::rejected, &dialog, &QDialog::reject);
|
||
layout->addWidget(buttonBox);
|
||
|
||
dialog.exec();
|
||
}
|
||
|
||
void CloudViewMainWindow::onShowRectangleShape()
|
||
{
|
||
QVector3D center;
|
||
QVector3D size;
|
||
getShapeFrame(m_originalCloud, center, size);
|
||
const double defaultWidth = std::max(1.0f, size.x() * 0.5f);
|
||
const double defaultHeight = std::max(1.0f, size.y() * 0.5f);
|
||
|
||
QDialog dialog(this);
|
||
dialog.setWindowTitle("矩形");
|
||
dialog.setModal(true);
|
||
|
||
QVBoxLayout* layout = new QVBoxLayout(&dialog);
|
||
|
||
QLabel* tip = new QLabel("输入矩形中心、姿态和宽高后点击添加；文件每行一个：{x,y,z}-{r,p,y}-{w,h}。", &dialog);
|
||
tip->setWordWrap(true);
|
||
tip->setStyleSheet("color: gray;");
|
||
layout->addWidget(tip);
|
||
|
||
QGroupBox* centerGroup = new QGroupBox("中心", &dialog);
|
||
QFormLayout* centerLayout = new QFormLayout(centerGroup);
|
||
QDoubleSpinBox* centerX = createShapeSpinBox(centerGroup, center.x());
|
||
QDoubleSpinBox* centerY = createShapeSpinBox(centerGroup, center.y());
|
||
QDoubleSpinBox* centerZ = createShapeSpinBox(centerGroup, center.z());
|
||
centerLayout->addRow("X:", centerX);
|
||
centerLayout->addRow("Y:", centerY);
|
||
centerLayout->addRow("Z:", centerZ);
|
||
layout->addWidget(centerGroup);
|
||
|
||
QGroupBox* poseGroup = new QGroupBox("姿态", &dialog);
|
||
QFormLayout* poseLayout = new QFormLayout(poseGroup);
|
||
QDoubleSpinBox* poseR = createShapeSpinBox(poseGroup, 0.0);
|
||
QDoubleSpinBox* poseP = createShapeSpinBox(poseGroup, 0.0);
|
||
QDoubleSpinBox* poseY = createShapeSpinBox(poseGroup, 0.0);
|
||
poseLayout->addRow("R/RX:", poseR);
|
||
poseLayout->addRow("P/RY:", poseP);
|
||
poseLayout->addRow("Y/RZ:", poseY);
|
||
layout->addWidget(poseGroup);
|
||
|
||
QGroupBox* sizeGroup = new QGroupBox("尺寸", &dialog);
|
||
QFormLayout* sizeLayout = new QFormLayout(sizeGroup);
|
||
QDoubleSpinBox* width = createShapeSpinBox(sizeGroup, defaultWidth, 0.000001);
|
||
QDoubleSpinBox* height = createShapeSpinBox(sizeGroup, defaultHeight, 0.000001);
|
||
sizeLayout->addRow("宽:", width);
|
||
sizeLayout->addRow("高:", height);
|
||
layout->addWidget(sizeGroup);
|
||
|
||
auto addCurrentRectangle = [this, centerX, centerY, centerZ, poseR, poseP, poseY, width, height]() {
|
||
const QVector3D inputCenter(static_cast<float>(centerX->value()),
|
||
static_cast<float>(centerY->value()),
|
||
static_cast<float>(centerZ->value()));
|
||
showBasicShape("矩形",
|
||
makeRectangleSegments(inputCenter,
|
||
poseR->value(),
|
||
poseP->value(),
|
||
poseY->value(),
|
||
static_cast<float>(width->value()),
|
||
static_cast<float>(height->value()),
|
||
currentShapeEulerOrder(m_comboEulerOrder)),
|
||
makeShapeCenterPoint(inputCenter));
|
||
};
|
||
|
||
QPushButton* loadButton = new QPushButton("加载文件", &dialog);
|
||
connect(loadButton, &QPushButton::clicked, this, [this, centerX, centerY, centerZ, poseR, poseP, poseY, width, height]() {
|
||
const QString fileName = QFileDialog::getOpenFileName(
|
||
this, "加载矩形数据", QString(),
|
||
"数据文件 (*.txt *.json *.dat);;所有文件 (*.*)");
|
||
if (fileName.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
QVector<NumericRow> rows;
|
||
QString errorMessage;
|
||
if (!loadNumericRowsFromFile(fileName, rows, errorMessage)) {
|
||
QMessageBox::warning(this, "错误", errorMessage);
|
||
return;
|
||
}
|
||
|
||
QVector<LineSegment> segments;
|
||
QVector<BasicShapePoint> centerPoints;
|
||
int shapeCount = 0;
|
||
if (!makeRectangleSegmentsFromRows(rows, currentShapeEulerOrder(m_comboEulerOrder), segments, centerPoints, shapeCount, errorMessage)) {
|
||
QMessageBox::warning(this, "错误", errorMessage);
|
||
return;
|
||
}
|
||
|
||
if (shapeCount == 1) {
|
||
const QVector<double>& values = rows.first().values;
|
||
centerX->setValue(values[0]);
|
||
centerY->setValue(values[1]);
|
||
centerZ->setValue(values[2]);
|
||
poseR->setValue(values[3]);
|
||
poseP->setValue(values[4]);
|
||
poseY->setValue(values[5]);
|
||
width->setValue(values[6]);
|
||
height->setValue(values[7]);
|
||
}
|
||
|
||
showBasicShape(QString("%1 个矩形").arg(shapeCount), segments, centerPoints);
|
||
});
|
||
|
||
QPushButton* addButton = new QPushButton("添加", &dialog);
|
||
connect(addButton, &QPushButton::clicked, this, addCurrentRectangle);
|
||
|
||
QHBoxLayout* actionLayout = new QHBoxLayout();
|
||
actionLayout->addWidget(loadButton);
|
||
actionLayout->addWidget(addButton);
|
||
actionLayout->addStretch();
|
||
layout->addLayout(actionLayout);
|
||
|
||
QDialogButtonBox* buttonBox = new QDialogButtonBox(QDialogButtonBox::Close, &dialog);
|
||
connect(buttonBox, &QDialogButtonBox::rejected, &dialog, &QDialog::reject);
|
||
layout->addWidget(buttonBox);
|
||
|
||
dialog.exec();
|
||
}
|
||
|
||
void CloudViewMainWindow::onShowSurfaceShape()
|
||
{
|
||
QVector3D center;
|
||
QVector3D size;
|
||
getShapeFrame(m_originalCloud, center, size);
|
||
const double defaultLength = std::max(1.0f, size.x() * 0.5f);
|
||
const double defaultWidth = std::max(1.0f, size.y() * 0.5f);
|
||
|
||
QDialog dialog(this);
|
||
dialog.setWindowTitle("面");
|
||
dialog.setModal(true);
|
||
|
||
QVBoxLayout* layout = new QVBoxLayout(&dialog);
|
||
|
||
QLabel* tip = new QLabel("输入中心、姿态和长宽后点击添加；文件每行一个：{x,y,z}-{l,w} 或 {x,y,z}-{r,p,y}-{l,w}。", &dialog);
|
||
tip->setWordWrap(true);
|
||
tip->setStyleSheet("color: gray;");
|
||
layout->addWidget(tip);
|
||
|
||
QGroupBox* centerGroup = new QGroupBox("中心", &dialog);
|
||
QFormLayout* centerLayout = new QFormLayout(centerGroup);
|
||
QDoubleSpinBox* centerX = createShapeSpinBox(centerGroup, center.x());
|
||
QDoubleSpinBox* centerY = createShapeSpinBox(centerGroup, center.y());
|
||
QDoubleSpinBox* centerZ = createShapeSpinBox(centerGroup, center.z());
|
||
centerLayout->addRow("X:", centerX);
|
||
centerLayout->addRow("Y:", centerY);
|
||
centerLayout->addRow("Z:", centerZ);
|
||
layout->addWidget(centerGroup);
|
||
|
||
QGroupBox* poseGroup = new QGroupBox("姿态", &dialog);
|
||
QFormLayout* poseLayout = new QFormLayout(poseGroup);
|
||
QDoubleSpinBox* poseR = createShapeSpinBox(poseGroup, 0.0);
|
||
QDoubleSpinBox* poseP = createShapeSpinBox(poseGroup, 0.0);
|
||
QDoubleSpinBox* poseY = createShapeSpinBox(poseGroup, 0.0);
|
||
poseLayout->addRow("R/RX:", poseR);
|
||
poseLayout->addRow("P/RY:", poseP);
|
||
poseLayout->addRow("Y/RZ:", poseY);
|
||
layout->addWidget(poseGroup);
|
||
|
||
QGroupBox* sizeGroup = new QGroupBox("尺寸", &dialog);
|
||
QFormLayout* sizeLayout = new QFormLayout(sizeGroup);
|
||
QDoubleSpinBox* length = createShapeSpinBox(sizeGroup, defaultLength, 0.000001);
|
||
QDoubleSpinBox* width = createShapeSpinBox(sizeGroup, defaultWidth, 0.000001);
|
||
sizeLayout->addRow("长:", length);
|
||
sizeLayout->addRow("宽:", width);
|
||
layout->addWidget(sizeGroup);
|
||
|
||
auto addCurrentSurface = [this, centerX, centerY, centerZ, poseR, poseP, poseY, length, width]() {
|
||
const QVector3D inputCenter(static_cast<float>(centerX->value()),
|
||
static_cast<float>(centerY->value()),
|
||
static_cast<float>(centerZ->value()));
|
||
const EulerRotationOrder order = currentShapeEulerOrder(m_comboEulerOrder);
|
||
const QVector<SurfaceQuad> surfaces = makeSurfaceQuads(inputCenter,
|
||
poseR->value(),
|
||
poseP->value(),
|
||
poseY->value(),
|
||
static_cast<float>(length->value()),
|
||
static_cast<float>(width->value()),
|
||
order);
|
||
const QVector<LineSegment> outlines = makeSurfaceOutlineSegments(inputCenter,
|
||
poseR->value(),
|
||
poseP->value(),
|
||
poseY->value(),
|
||
static_cast<float>(length->value()),
|
||
static_cast<float>(width->value()),
|
||
order);
|
||
showBasicSurface("面", surfaces, outlines);
|
||
};
|
||
|
||
QPushButton* loadButton = new QPushButton("加载文件", &dialog);
|
||
connect(loadButton, &QPushButton::clicked, this, [this, centerX, centerY, centerZ, poseR, poseP, poseY, length, width]() {
|
||
const QString fileName = QFileDialog::getOpenFileName(
|
||
this, "加载面数据", QString(),
|
||
"数据文件 (*.txt *.json *.dat);;所有文件 (*.*)");
|
||
if (fileName.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
QVector<NumericRow> rows;
|
||
QString errorMessage;
|
||
if (!loadNumericRowsFromFile(fileName, rows, errorMessage)) {
|
||
QMessageBox::warning(this, "错误", errorMessage);
|
||
return;
|
||
}
|
||
|
||
QVector<SurfaceQuad> surfaces;
|
||
QVector<LineSegment> outlines;
|
||
int shapeCount = 0;
|
||
if (!makeSurfaceDataFromRows(rows, currentShapeEulerOrder(m_comboEulerOrder), surfaces, outlines, shapeCount, errorMessage)) {
|
||
QMessageBox::warning(this, "错误", errorMessage);
|
||
return;
|
||
}
|
||
|
||
if (shapeCount == 1) {
|
||
const QVector<double>& values = rows.first().values;
|
||
const bool hasPose = values.size() == 8;
|
||
centerX->setValue(values[0]);
|
||
centerY->setValue(values[1]);
|
||
centerZ->setValue(values[2]);
|
||
poseR->setValue(hasPose ? values[3] : 0.0);
|
||
poseP->setValue(hasPose ? values[4] : 0.0);
|
||
poseY->setValue(hasPose ? values[5] : 0.0);
|
||
length->setValue(hasPose ? values[6] : values[3]);
|
||
width->setValue(hasPose ? values[7] : values[4]);
|
||
}
|
||
|
||
showBasicSurface(QString("%1 个面").arg(shapeCount), surfaces, outlines);
|
||
});
|
||
|
||
QPushButton* addButton = new QPushButton("添加", &dialog);
|
||
connect(addButton, &QPushButton::clicked, this, addCurrentSurface);
|
||
|
||
QHBoxLayout* actionLayout = new QHBoxLayout();
|
||
actionLayout->addWidget(loadButton);
|
||
actionLayout->addWidget(addButton);
|
||
actionLayout->addStretch();
|
||
layout->addLayout(actionLayout);
|
||
|
||
QDialogButtonBox* buttonBox = new QDialogButtonBox(QDialogButtonBox::Close, &dialog);
|
||
connect(buttonBox, &QDialogButtonBox::rejected, &dialog, &QDialog::reject);
|
||
layout->addWidget(buttonBox);
|
||
|
||
dialog.exec();
|
||
}
|
||
|
||
void CloudViewMainWindow::onShowTriangleShape()
|
||
{
|
||
QVector3D center;
|
||
QVector3D size;
|
||
getShapeFrame(m_originalCloud, center, size);
|
||
const float radius = std::max(1.0f, std::max(size.x(), size.y()) * 0.3f);
|
||
|
||
const QVector3D defaultP1(center.x(), center.y() - radius, center.z());
|
||
const QVector3D defaultP2(center.x() + radius * 0.8660254f, center.y() + radius * 0.5f, center.z());
|
||
const QVector3D defaultP3(center.x() - radius * 0.8660254f, center.y() + radius * 0.5f, center.z());
|
||
|
||
QDialog dialog(this);
|
||
dialog.setWindowTitle("三角形");
|
||
dialog.setModal(true);
|
||
|
||
QVBoxLayout* layout = new QVBoxLayout(&dialog);
|
||
|
||
QLabel* tip = new QLabel("输入三个顶点后点击添加；文件每行一个：{x,y,z}-{x,y,z}-{x,y,z}。", &dialog);
|
||
tip->setWordWrap(true);
|
||
tip->setStyleSheet("color: gray;");
|
||
layout->addWidget(tip);
|
||
|
||
auto addPointGroup = [&](const QString& title, const QVector3D& value, QDoubleSpinBox*& x, QDoubleSpinBox*& y, QDoubleSpinBox*& z) {
|
||
QGroupBox* group = new QGroupBox(title, &dialog);
|
||
QFormLayout* form = new QFormLayout(group);
|
||
x = createShapeSpinBox(group, value.x());
|
||
y = createShapeSpinBox(group, value.y());
|
||
z = createShapeSpinBox(group, value.z());
|
||
form->addRow("X:", x);
|
||
form->addRow("Y:", y);
|
||
form->addRow("Z:", z);
|
||
layout->addWidget(group);
|
||
};
|
||
|
||
QDoubleSpinBox* x1 = nullptr;
|
||
QDoubleSpinBox* y1 = nullptr;
|
||
QDoubleSpinBox* z1 = nullptr;
|
||
QDoubleSpinBox* x2 = nullptr;
|
||
QDoubleSpinBox* y2 = nullptr;
|
||
QDoubleSpinBox* z2 = nullptr;
|
||
QDoubleSpinBox* x3 = nullptr;
|
||
QDoubleSpinBox* y3 = nullptr;
|
||
QDoubleSpinBox* z3 = nullptr;
|
||
addPointGroup("点1", defaultP1, x1, y1, z1);
|
||
addPointGroup("点2", defaultP2, x2, y2, z2);
|
||
addPointGroup("点3", defaultP3, x3, y3, z3);
|
||
|
||
auto addCurrentTriangle = [this, x1, y1, z1, x2, y2, z2, x3, y3, z3]() {
|
||
QVector<QVector3D> points;
|
||
points.reserve(3);
|
||
points.append(QVector3D(static_cast<float>(x1->value()), static_cast<float>(y1->value()), static_cast<float>(z1->value())));
|
||
points.append(QVector3D(static_cast<float>(x2->value()), static_cast<float>(y2->value()), static_cast<float>(z2->value())));
|
||
points.append(QVector3D(static_cast<float>(x3->value()), static_cast<float>(y3->value()), static_cast<float>(z3->value())));
|
||
showBasicShape("三角形", makeClosedShapeSegments(points, 0.2f, 1.0f, 0.2f));
|
||
};
|
||
|
||
QPushButton* loadButton = new QPushButton("加载文件", &dialog);
|
||
connect(loadButton, &QPushButton::clicked, this, [this, x1, y1, z1, x2, y2, z2, x3, y3, z3]() {
|
||
const QString fileName = QFileDialog::getOpenFileName(
|
||
this, "加载三角形数据", QString(),
|
||
"数据文件 (*.txt *.json *.dat);;所有文件 (*.*)");
|
||
if (fileName.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
QVector<NumericRow> rows;
|
||
QString errorMessage;
|
||
if (!loadNumericRowsFromFile(fileName, rows, errorMessage)) {
|
||
QMessageBox::warning(this, "错误", errorMessage);
|
||
return;
|
||
}
|
||
|
||
QVector<LineSegment> segments;
|
||
int shapeCount = 0;
|
||
if (!makeTriangleSegmentsFromRows(rows, segments, shapeCount, errorMessage)) {
|
||
QMessageBox::warning(this, "错误", errorMessage);
|
||
return;
|
||
}
|
||
|
||
if (shapeCount == 1) {
|
||
const QVector<double>& values = rows.first().values;
|
||
x1->setValue(values[0]);
|
||
y1->setValue(values[1]);
|
||
z1->setValue(values[2]);
|
||
x2->setValue(values[3]);
|
||
y2->setValue(values[4]);
|
||
z2->setValue(values[5]);
|
||
x3->setValue(values[6]);
|
||
y3->setValue(values[7]);
|
||
z3->setValue(values[8]);
|
||
}
|
||
|
||
showBasicShape(QString("%1 个三角形").arg(shapeCount), segments);
|
||
});
|
||
|
||
QPushButton* addButton = new QPushButton("添加", &dialog);
|
||
connect(addButton, &QPushButton::clicked, this, addCurrentTriangle);
|
||
|
||
QHBoxLayout* actionLayout = new QHBoxLayout();
|
||
actionLayout->addWidget(loadButton);
|
||
actionLayout->addWidget(addButton);
|
||
actionLayout->addStretch();
|
||
layout->addLayout(actionLayout);
|
||
|
||
QDialogButtonBox* buttonBox = new QDialogButtonBox(QDialogButtonBox::Close, &dialog);
|
||
connect(buttonBox, &QDialogButtonBox::rejected, &dialog, &QDialog::reject);
|
||
layout->addWidget(buttonBox);
|
||
|
||
dialog.exec();
|
||
}
|
||
|
||
void CloudViewMainWindow::onClearAll()
|
||
{
|
||
m_glWidget->clearPointClouds();
|
||
m_glWidget->clearLineSegments();
|
||
m_glWidget->clearBasicShapeSegments();
|
||
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: --");
|
||
clearMeasureResultDisplay();
|
||
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::onViewAnglesChanged(float rotX, float rotY, float rotZ)
|
||
{
|
||
if (m_editRotX) {
|
||
m_editRotX->setText(QString::number(rotX, 'f', 1));
|
||
}
|
||
if (m_editRotY) {
|
||
m_editRotY->setText(QString::number(rotY, 'f', 1));
|
||
}
|
||
if (m_editRotZ) {
|
||
m_editRotZ->setText(QString::number(rotZ, 'f', 1));
|
||
}
|
||
}
|
||
|
||
void CloudViewMainWindow::onClearSelectedPoints()
|
||
{
|
||
m_glWidget->clearSelectedPoints();
|
||
m_glWidget->clearPosePoints(); // 清除选点时也清除姿态
|
||
m_lblPoint1->setText("点1: --");
|
||
m_lblPoint2->setText("点2: --");
|
||
clearMeasureResultDisplay();
|
||
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();
|
||
updateMeasureResultDisplay(p1, p2, distance, "测量距离");
|
||
}
|
||
|
||
void CloudViewMainWindow::clearMeasureResultDisplay()
|
||
{
|
||
m_lblDistance->setText("--");
|
||
if (m_lblDeltaXYZ) {
|
||
m_lblDeltaXYZ->setText("--");
|
||
}
|
||
}
|
||
|
||
void CloudViewMainWindow::updateMeasureResultDisplay(const SelectedPointInfo& p1,
|
||
const SelectedPointInfo& p2,
|
||
float distance,
|
||
const QString& statusPrefix)
|
||
{
|
||
const float dx = p2.x - p1.x;
|
||
const float dy = p2.y - p1.y;
|
||
const float dz = p2.z - p1.z;
|
||
|
||
m_lblDistance->setText(QString("%1 mm").arg(distance, 0, 'f', 3));
|
||
m_lblDeltaXYZ->setText(QString("X:%1 mm Y:%2 mm Z:%3 mm")
|
||
.arg(dx, 0, 'f', 3)
|
||
.arg(dy, 0, 'f', 3)
|
||
.arg(dz, 0, 'f', 3));
|
||
|
||
statusBar()->showMessage(QString("%1: %2 mm | X:%3 Y:%4 Z:%5")
|
||
.arg(statusPrefix)
|
||
.arg(distance, 0, 'f', 3)
|
||
.arg(dx, 0, 'f', 3)
|
||
.arg(dy, 0, 'f', 3)
|
||
.arg(dz, 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("--");
|
||
}
|
||
|
||
if (selectedPoints.size() < 2 || !selectedPoints[0].valid || !selectedPoints[1].valid) {
|
||
clearMeasureResultDisplay();
|
||
}
|
||
}
|
||
|
||
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) {
|
||
int previousLineIndex = 0;
|
||
int fallbackPointIndex = 0;
|
||
bool hasPreviousLine = false;
|
||
for (size_t i = 0; i < m_originalCloud.points.size(); ++i) {
|
||
const int lineIndex = (i < m_originalCloud.lineIndices.size())
|
||
? m_originalCloud.lineIndices[i] : -1;
|
||
if (!hasPreviousLine || lineIndex != previousLineIndex) {
|
||
fallbackPointIndex = 0;
|
||
previousLineIndex = lineIndex;
|
||
hasPreviousLine = true;
|
||
}
|
||
int pointIndex = (i < m_originalCloud.pointInLineIndices.size())
|
||
? m_originalCloud.pointInLineIndices[i] : -1;
|
||
if (pointIndex < 0) {
|
||
pointIndex = fallbackPointIndex;
|
||
}
|
||
if (pointIndex == lineInfo.pointIndex) {
|
||
const auto& pt = m_originalCloud.points[i];
|
||
points.append(QVector3D(pt.x, pt.y, pt.z));
|
||
}
|
||
++fallbackPointIndex;
|
||
}
|
||
}
|
||
}
|
||
|
||
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(QString("已清除输入的线段"));
|
||
}
|
||
|
||
void CloudViewMainWindow::onEyeToHandTransform()
|
||
{
|
||
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("输入或从文件加载 4x4 变换矩阵（相机坐标系 -> 基坐标系），直接应用到所有点云。", &dialog);
|
||
tip->setWordWrap(true);
|
||
tip->setStyleSheet("color: gray; font-size: 10px;");
|
||
layout->addWidget(tip);
|
||
|
||
QLabel* lblMatrix = new QLabel("变换矩阵 (4x4):", &dialog);
|
||
lblMatrix->setStyleSheet("font-weight: bold;");
|
||
layout->addWidget(lblMatrix);
|
||
|
||
QTextEdit* matrixEdit = new QTextEdit(&dialog);
|
||
matrixEdit->setFont(QFont("Consolas", 10));
|
||
matrixEdit->setMinimumHeight(100);
|
||
matrixEdit->setMaximumHeight(120);
|
||
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");
|
||
layout->addWidget(matrixEdit);
|
||
|
||
QPushButton* btnLoad = new QPushButton("从文件加载矩阵", &dialog);
|
||
connect(btnLoad, &QPushButton::clicked, [this, matrixEdit]() {
|
||
const QString fileName = QFileDialog::getOpenFileName(
|
||
this, "打开矩阵文件", QString(),
|
||
"标定矩阵文件 (*.ini *.txt);;INI文件 (*.ini);;文本文件 (*.txt);;所有文件 (*.*)");
|
||
if (fileName.isEmpty()) return;
|
||
|
||
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) {
|
||
matrixEdit->setPlainText(formatMatrixText(matrixList[0].second));
|
||
return;
|
||
}
|
||
QDialog dlg(this);
|
||
dlg.setWindowTitle("选择标定矩阵");
|
||
QVBoxLayout* l = new QVBoxLayout(&dlg);
|
||
l->addWidget(new QLabel(QString("文件中包含 %1 个标定矩阵，请选择：").arg(matrixList.size()), &dlg));
|
||
QListWidget* lw = new QListWidget(&dlg);
|
||
for (auto& m : matrixList) lw->addItem(m.first);
|
||
lw->setCurrentRow(0);
|
||
l->addWidget(lw);
|
||
QDialogButtonBox* bb = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, &dlg);
|
||
l->addWidget(bb);
|
||
connect(bb, &QDialogButtonBox::accepted, &dlg, &QDialog::accept);
|
||
connect(bb, &QDialogButtonBox::rejected, &dlg, &QDialog::reject);
|
||
if (dlg.exec() == QDialog::Accepted && lw->currentRow() >= 0 && lw->currentRow() < matrixList.size())
|
||
matrixEdit->setPlainText(formatMatrixText(matrixList[lw->currentRow()].second));
|
||
return;
|
||
}
|
||
|
||
QFile file(fileName);
|
||
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
|
||
QMessageBox::critical(this, "错误", QString("无法打开文件: %1").arg(fileName));
|
||
return;
|
||
}
|
||
QTextStream in(&file);
|
||
QString text = in.readAll();
|
||
file.close();
|
||
QMatrix4x4 matrix;
|
||
QString errorMessage;
|
||
if (!parseMatrixText(text, matrix, &errorMessage)) {
|
||
QMessageBox::warning(this, "格式错误", errorMessage);
|
||
return;
|
||
}
|
||
matrixEdit->setPlainText(formatMatrixText(matrix));
|
||
});
|
||
layout->addWidget(btnLoad);
|
||
|
||
QLabel* lblFormat = new QLabel("矩阵文件格式：4 行，每行 4 个数值，分隔符：空格 / Tab / 逗号", &dialog);
|
||
lblFormat->setWordWrap(true);
|
||
lblFormat->setStyleSheet("color: gray; font-size: 9px;");
|
||
layout->addWidget(lblFormat);
|
||
|
||
QDialogButtonBox* btnBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, &dialog);
|
||
btnBox->button(QDialogButtonBox::Ok)->setText("应用变换");
|
||
btnBox->button(QDialogButtonBox::Ok)->setStyleSheet("QPushButton { background-color: #4CAF50; color: white; font-weight: bold; }"
|
||
"QPushButton:hover { background-color: #45a049; }");
|
||
layout->addWidget(btnBox);
|
||
connect(btnBox, &QDialogButtonBox::accepted, &dialog, &QDialog::accept);
|
||
connect(btnBox, &QDialogButtonBox::rejected, &dialog, &QDialog::reject);
|
||
|
||
if (dialog.exec() != QDialog::Accepted) return;
|
||
|
||
QMatrix4x4 matrix;
|
||
QString errorMessage;
|
||
if (!parseMatrixText(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 eye-to-hand matrix transform to all point clouds\n");
|
||
}
|
||
|
||
void CloudViewMainWindow::onEyeInHandTransform()
|
||
{
|
||
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(
|
||
"使用 4x4 矩阵作为“相机坐标系 -> 末端坐标系”的手眼矩阵。<br/>"
|
||
"输入当前机械臂位姿（单位：mm / deg）后，将所有点云转换到机械臂基坐标系。<br/>"
|
||
"角度组合方式：ZYX（Rz-Ry-Rx）。", &dialog);
|
||
tip->setWordWrap(true);
|
||
tip->setTextFormat(Qt::RichText);
|
||
tip->setStyleSheet("color: gray; font-size: 10px;");
|
||
layout->addWidget(tip);
|
||
|
||
QLabel* lblMatrix = new QLabel("手眼矩阵 (4x4):", &dialog);
|
||
lblMatrix->setStyleSheet("font-weight: bold;");
|
||
layout->addWidget(lblMatrix);
|
||
|
||
QTextEdit* matrixEdit = new QTextEdit(&dialog);
|
||
matrixEdit->setFont(QFont("Consolas", 10));
|
||
matrixEdit->setMinimumHeight(100);
|
||
matrixEdit->setMaximumHeight(120);
|
||
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");
|
||
layout->addWidget(matrixEdit);
|
||
|
||
QPushButton* btnLoad = new QPushButton("从文件加载矩阵", &dialog);
|
||
connect(btnLoad, &QPushButton::clicked, [this, matrixEdit]() {
|
||
const QString fileName = QFileDialog::getOpenFileName(
|
||
this, "打开矩阵文件", QString(),
|
||
"标定矩阵文件 (*.ini *.txt);;INI文件 (*.ini);;文本文件 (*.txt);;所有文件 (*.*)");
|
||
if (fileName.isEmpty()) return;
|
||
|
||
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) {
|
||
matrixEdit->setPlainText(formatMatrixText(matrixList[0].second));
|
||
return;
|
||
}
|
||
QDialog dlg(this);
|
||
dlg.setWindowTitle("选择标定矩阵");
|
||
QVBoxLayout* l = new QVBoxLayout(&dlg);
|
||
l->addWidget(new QLabel(QString("文件中包含 %1 个标定矩阵，请选择：").arg(matrixList.size()), &dlg));
|
||
QListWidget* lw = new QListWidget(&dlg);
|
||
for (auto& m : matrixList) lw->addItem(m.first);
|
||
lw->setCurrentRow(0);
|
||
l->addWidget(lw);
|
||
QDialogButtonBox* bb = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, &dlg);
|
||
l->addWidget(bb);
|
||
connect(bb, &QDialogButtonBox::accepted, &dlg, &QDialog::accept);
|
||
connect(bb, &QDialogButtonBox::rejected, &dlg, &QDialog::reject);
|
||
if (dlg.exec() == QDialog::Accepted && lw->currentRow() >= 0 && lw->currentRow() < matrixList.size())
|
||
matrixEdit->setPlainText(formatMatrixText(matrixList[lw->currentRow()].second));
|
||
return;
|
||
}
|
||
|
||
QFile file(fileName);
|
||
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
|
||
QMessageBox::critical(this, "错误", QString("无法打开文件: %1").arg(fileName));
|
||
return;
|
||
}
|
||
QTextStream in(&file);
|
||
QString text = in.readAll();
|
||
file.close();
|
||
QMatrix4x4 matrix;
|
||
QString errorMessage;
|
||
if (!parseMatrixText(text, matrix, &errorMessage)) {
|
||
QMessageBox::warning(this, "格式错误", errorMessage);
|
||
return;
|
||
}
|
||
matrixEdit->setPlainText(formatMatrixText(matrix));
|
||
});
|
||
layout->addWidget(btnLoad);
|
||
|
||
// 机械臂位姿输入
|
||
QGroupBox* poseGroup = new QGroupBox("机械臂位姿", &dialog);
|
||
QGridLayout* poseLayout = new QGridLayout(poseGroup);
|
||
poseLayout->setHorizontalSpacing(8);
|
||
poseLayout->setVerticalSpacing(6);
|
||
|
||
auto addPoseEdit = [&](const QString& label, int row, int col) -> QLineEdit* {
|
||
QLabel* lbl = new QLabel(label, poseGroup);
|
||
QLineEdit* edit = new QLineEdit("0.0", poseGroup);
|
||
edit->setAlignment(Qt::AlignRight);
|
||
edit->setFont(QFont("Consolas", 10));
|
||
poseLayout->addWidget(lbl, row, col * 2);
|
||
poseLayout->addWidget(edit, row, col * 2 + 1);
|
||
return edit;
|
||
};
|
||
|
||
QLineEdit* editX = addPoseEdit("X:", 0, 0);
|
||
QLineEdit* editY = addPoseEdit("Y:", 0, 1);
|
||
QLineEdit* editZ = addPoseEdit("Z:", 0, 2);
|
||
QLineEdit* editRx = addPoseEdit("RX:", 1, 0);
|
||
QLineEdit* editRy = addPoseEdit("RY:", 1, 1);
|
||
QLineEdit* editRz = addPoseEdit("RZ:", 1, 2);
|
||
layout->addWidget(poseGroup);
|
||
|
||
QDialogButtonBox* btnBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, &dialog);
|
||
btnBox->button(QDialogButtonBox::Ok)->setText("应用转换");
|
||
btnBox->button(QDialogButtonBox::Ok)->setStyleSheet("QPushButton { background-color: #1976D2; color: white; font-weight: bold; }"
|
||
"QPushButton:hover { background-color: #1565C0; }");
|
||
layout->addWidget(btnBox);
|
||
connect(btnBox, &QDialogButtonBox::accepted, &dialog, &QDialog::accept);
|
||
connect(btnBox, &QDialogButtonBox::rejected, &dialog, &QDialog::reject);
|
||
|
||
if (dialog.exec() != QDialog::Accepted) return;
|
||
|
||
QMatrix4x4 handEyeMatrix;
|
||
QString errorMessage;
|
||
if (!parseMatrixText(matrixEdit->toPlainText().trimmed(), handEyeMatrix, &errorMessage)) {
|
||
QMessageBox::warning(this, "格式错误", QString("手眼矩阵无效：%1").arg(errorMessage));
|
||
return;
|
||
}
|
||
|
||
bool ok = false;
|
||
float vals[6];
|
||
const char* labels[] = {"X", "Y", "Z", "RX", "RY", "RZ"};
|
||
QLineEdit* edits[] = {editX, editY, editZ, editRx, editRy, editRz};
|
||
for (int i = 0; i < 6; ++i) {
|
||
vals[i] = edits[i]->text().trimmed().toFloat(&ok);
|
||
if (!ok) {
|
||
QMessageBox::warning(this, "格式错误", QString("机械臂 %1 位姿无效").arg(labels[i]));
|
||
return;
|
||
}
|
||
}
|
||
|
||
const QMatrix4x4 totalMatrix = buildRobotPoseMatrix(vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]) * handEyeMatrix;
|
||
if (totalMatrix.isIdentity()) {
|
||
QMessageBox::information(this, "提示", "当前位姿和手眼矩阵组合为单位矩阵，无需变换");
|
||
return;
|
||
}
|
||
|
||
applyTransformToAllClouds(totalMatrix);
|
||
statusBar()->showMessage(
|
||
QString("已应用眼在手上转换，机械臂位姿: [%1, %2, %3, %4, %5, %6]")
|
||
.arg(vals[0], 0, 'f', 3).arg(vals[1], 0, 'f', 3).arg(vals[2], 0, 'f', 3)
|
||
.arg(vals[3], 0, 'f', 3).arg(vals[4], 0, 'f', 3).arg(vals[5], 0, 'f', 3));
|
||
LOG_INFO("[CloudView] Applied eye-in-hand transform, robot pose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n",
|
||
vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]);
|
||
}
|
||
|
||
void CloudViewMainWindow::onSavePointCloud()
|
||
{
|
||
if (m_glWidget->getCloudCount() == 0) {
|
||
QMessageBox::warning(this, "提示", "请先加载点云");
|
||
return;
|
||
}
|
||
|
||
const QString fileName = QFileDialog::getSaveFileName(
|
||
this, "保存 TXT 点云", 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::onSavePlyPcdCloud()
|
||
{
|
||
if (m_glWidget->getCloudCount() == 0) {
|
||
QMessageBox::warning(this, "提示", "请先加载点云");
|
||
return;
|
||
}
|
||
|
||
const QString fileName = QFileDialog::getSaveFileName(
|
||
this, "保存 PLY/PCD 点云", QString(),
|
||
"PLY 文件 (*.ply);;PCD 文件 (*.pcd);;所有文件 (*.*)");
|
||
if (fileName.isEmpty()) return;
|
||
|
||
// 获取所有显示的点云，展平为 PointCloudXYZ（跳过零点）
|
||
std::vector<std::vector<SVzNL3DPosition>> scanLines;
|
||
const size_t totalPoints = m_glWidget->getAllCloudsByLines(scanLines);
|
||
if (scanLines.empty() || totalPoints == 0) {
|
||
QMessageBox::warning(this, "提示", "当前无可保存的点云数据");
|
||
return;
|
||
}
|
||
|
||
PointCloudXYZ cloud;
|
||
const float EPS = 1e-6f;
|
||
for (const auto& line : scanLines) {
|
||
for (const auto& pt : line) {
|
||
// 跳过零点（网格填充的空位）
|
||
if (std::fabs(pt.pt3D.x) < EPS &&
|
||
std::fabs(pt.pt3D.y) < EPS &&
|
||
std::fabs(pt.pt3D.z) < EPS)
|
||
continue;
|
||
cloud.points.emplace_back(pt.pt3D.x, pt.pt3D.y, pt.pt3D.z);
|
||
}
|
||
}
|
||
|
||
if (cloud.empty()) {
|
||
QMessageBox::warning(this, "提示", "当前无可保存的有效点云数据");
|
||
return;
|
||
}
|
||
|
||
// 根据扩展名选择保存格式
|
||
const std::string path = fileName.toStdString();
|
||
std::string ext;
|
||
const auto dotPos = path.rfind('.');
|
||
if (dotPos != std::string::npos)
|
||
ext = path.substr(dotPos + 1);
|
||
for (auto& c : ext) c = static_cast<char>(std::tolower(c));
|
||
|
||
int ret = -1;
|
||
if (ext == "ply") {
|
||
ret = m_converter->saveToPly(path, cloud);
|
||
} else if (ext == "pcd") {
|
||
ret = m_converter->saveToPcd(path, cloud);
|
||
} else {
|
||
// 用户手动输入了其他扩展名，默认用 PLY
|
||
ret = m_converter->saveToPly(path, cloud);
|
||
}
|
||
|
||
if (ret != 0) {
|
||
QMessageBox::critical(this, "保存失败",
|
||
QString("保存点云失败: %1")
|
||
.arg(QString::fromStdString(m_converter->getLastError())));
|
||
return;
|
||
}
|
||
|
||
statusBar()->showMessage(
|
||
QString("已保存点云: %1 个点 -> %2")
|
||
.arg(cloud.points.size()).arg(QFileInfo(fileName).fileName()));
|
||
LOG_INFO("[CloudView] Saved %zu points to %s\n",
|
||
cloud.points.size(), fileName.toStdString().c_str());
|
||
}
|
||
|
||
void CloudViewMainWindow::onBatchConvertTxtToPly()
|
||
{
|
||
// 选择源文件夹
|
||
const QString srcDir = QFileDialog::getExistingDirectory(
|
||
this, "选择源文件夹（包含 txt 点云文件）", QString(),
|
||
QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
|
||
if (srcDir.isEmpty()) return;
|
||
|
||
// 选择目标文件夹
|
||
const QString dstDir = QFileDialog::getExistingDirectory(
|
||
this, "选择目标文件夹（PLY 文件输出位置）", srcDir,
|
||
QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
|
||
if (dstDir.isEmpty()) return;
|
||
|
||
// 递归收集所有 .txt 文件及其相对路径
|
||
struct FileEntry
|
||
{
|
||
QString absolutePath;
|
||
QString relativePath; // 相对于源文件夹的路径，用于构建目标路径
|
||
};
|
||
QVector<FileEntry> fileEntries;
|
||
|
||
{
|
||
QDirIterator it(srcDir, QStringList("*.txt"), QDir::Files,
|
||
QDirIterator::Subdirectories | QDirIterator::FollowSymlinks);
|
||
while (it.hasNext()) {
|
||
it.next();
|
||
FileEntry entry;
|
||
entry.absolutePath = it.filePath();
|
||
// 计算相对路径（文件路径去掉源目录前缀）
|
||
entry.relativePath = QDir(srcDir).relativeFilePath(it.filePath());
|
||
fileEntries.append(entry);
|
||
}
|
||
}
|
||
|
||
if (fileEntries.isEmpty()) {
|
||
QMessageBox::information(this, "提示",
|
||
QString("源文件夹及其子目录中没有找到 .txt 文件:\n%1").arg(srcDir));
|
||
return;
|
||
}
|
||
|
||
// 按相对路径排序
|
||
std::sort(fileEntries.begin(), fileEntries.end(),
|
||
[](const FileEntry& a, const FileEntry& b) {
|
||
return a.relativePath < b.relativePath;
|
||
});
|
||
|
||
// 进度对话框
|
||
QProgressDialog progress("正在批量转换 txt -> ply...", "取消", 0, fileEntries.size(), this);
|
||
progress.setWindowTitle("批量转换");
|
||
progress.setWindowModality(Qt::WindowModal);
|
||
progress.setMinimumDuration(0);
|
||
progress.setAutoClose(true);
|
||
|
||
// 使用独立的转换器，避免干扰主窗口状态
|
||
PointCloudConverter converter;
|
||
converter.setPointCloudRange(m_pointCloudRange);
|
||
int successCount = 0;
|
||
int failCount = 0;
|
||
int skipCount = 0;
|
||
QStringList failedFiles;
|
||
|
||
const float kZeroEps = 1e-6f;
|
||
|
||
for (int i = 0; i < fileEntries.size(); ++i) {
|
||
if (progress.wasCanceled()) {
|
||
statusBar()->showMessage("批量转换已取消");
|
||
break;
|
||
}
|
||
|
||
const FileEntry& entry = fileEntries[i];
|
||
const QString& srcPath = entry.absolutePath;
|
||
const QString& relPath = entry.relativePath;
|
||
|
||
// 构建目标路径：目标文件夹 + 相对路径，扩展名改为 .ply
|
||
const QString baseName = QFileInfo(relPath).completeBaseName();
|
||
const QString relDir = QFileInfo(relPath).path(); // 相对路径中的目录部分
|
||
const QString dstRelPath = (relDir == "." ? baseName + ".ply"
|
||
: relDir + "/" + baseName + ".ply");
|
||
const QString dstPath = dstDir + "/" + dstRelPath;
|
||
|
||
// 确保目标子目录存在
|
||
const QString dstParentDir = QFileInfo(dstPath).absolutePath();
|
||
if (!QDir().mkpath(dstParentDir)) {
|
||
failCount++;
|
||
failedFiles.append(QString("%1 (无法创建目标目录)").arg(relPath));
|
||
LOG_WARN("[CloudView] Batch convert: failed to create dir %s\n",
|
||
dstParentDir.toStdString().c_str());
|
||
continue;
|
||
}
|
||
|
||
// 显示简短的相对路径便于阅读
|
||
const QString displayPath = relPath.length() > 60
|
||
? ("..." + relPath.right(57)) : relPath;
|
||
|
||
progress.setLabelText(QString("正在转换: %1 (%2/%3)")
|
||
.arg(displayPath).arg(i + 1).arg(fileEntries.size()));
|
||
progress.setValue(i);
|
||
QCoreApplication::processEvents();
|
||
|
||
// 加载 txt 点云
|
||
PointCloudXYZ cloud;
|
||
int loadRet = converter.loadFromTxt(srcPath.toStdString(), cloud);
|
||
if (loadRet != 0) {
|
||
failCount++;
|
||
failedFiles.append(QString("%1 (加载失败: %2)")
|
||
.arg(relPath)
|
||
.arg(QString::fromStdString(converter.getLastError())));
|
||
LOG_WARN("[CloudView] Batch convert: failed to load %s\n",
|
||
srcPath.toStdString().c_str());
|
||
continue;
|
||
}
|
||
|
||
// 过滤掉零点，构建用于保存的点云
|
||
PointCloudXYZ filteredCloud;
|
||
filteredCloud.reserve(cloud.size());
|
||
for (const auto& pt : cloud.points) {
|
||
if (std::fabs(pt.x) < kZeroEps &&
|
||
std::fabs(pt.y) < kZeroEps &&
|
||
std::fabs(pt.z) < kZeroEps)
|
||
continue;
|
||
filteredCloud.push_back(pt);
|
||
}
|
||
|
||
if (filteredCloud.empty()) {
|
||
failCount++;
|
||
failedFiles.append(QString("%1 (无有效点)").arg(relPath));
|
||
LOG_WARN("[CloudView] Batch convert: no valid points in %s\n",
|
||
srcPath.toStdString().c_str());
|
||
continue;
|
||
}
|
||
|
||
// 保存为 PLY
|
||
int saveRet = converter.saveToPly(dstPath.toStdString(), filteredCloud);
|
||
if (saveRet != 0) {
|
||
failCount++;
|
||
failedFiles.append(QString("%1 (保存失败: %2)")
|
||
.arg(relPath)
|
||
.arg(QString::fromStdString(converter.getLastError())));
|
||
LOG_WARN("[CloudView] Batch convert: failed to save %s\n",
|
||
dstPath.toStdString().c_str());
|
||
continue;
|
||
}
|
||
|
||
successCount++;
|
||
LOG_INFO("[CloudView] Batch convert: %s -> %s (%zu points)\n",
|
||
srcPath.toStdString().c_str(),
|
||
dstPath.toStdString().c_str(),
|
||
filteredCloud.size());
|
||
}
|
||
|
||
progress.setValue(fileEntries.size());
|
||
|
||
// 汇总结果
|
||
const int totalCount = fileEntries.size();
|
||
QString summary = QString("批量转换完成!\n\n"
|
||
"总文件数: %1\n"
|
||
"成功: %2\n"
|
||
"失败: %3")
|
||
.arg(totalCount)
|
||
.arg(successCount)
|
||
.arg(failCount);
|
||
if (skipCount > 0) {
|
||
summary += QString("\n跳过: %1").arg(skipCount);
|
||
}
|
||
|
||
if (!failedFiles.isEmpty()) {
|
||
summary += "\n\n失败文件:\n";
|
||
// 最多显示前 10 个失败文件
|
||
const int maxShow = std::min(failedFiles.size(), 10);
|
||
for (int i = 0; i < maxShow; ++i) {
|
||
summary += QString(" • %1\n").arg(failedFiles[i]);
|
||
}
|
||
if (failedFiles.size() > maxShow) {
|
||
summary += QString(" ... 还有 %1 个\n")
|
||
.arg(failedFiles.size() - maxShow);
|
||
}
|
||
}
|
||
|
||
QMessageBox::information(this, "转换结果", summary);
|
||
statusBar()->showMessage(
|
||
QString("批量转换完成: %1 成功, %2 失败").arg(successCount).arg(failCount));
|
||
}
|
||
|
||
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::onPseudoColorSettings()
|
||
{
|
||
QDialog dialog(this);
|
||
dialog.setWindowTitle("点云伪彩色设置");
|
||
dialog.setModal(true);
|
||
dialog.setMinimumWidth(420);
|
||
|
||
QVBoxLayout* layout = new QVBoxLayout(&dialog);
|
||
QLabel* tip = new QLabel(
|
||
"仅对不带 RGB 的纯点云生效。按固定 Z 周期循环着色，同一高度颜色相同。"
|
||
"默认方案使用颜色选择器色相条（红→品红→蓝→青→绿→黄→红），"
|
||
"自定义方案可在起止色之间做线性渐变。",
|
||
&dialog);
|
||
tip->setWordWrap(true);
|
||
tip->setStyleSheet("color: gray;");
|
||
layout->addWidget(tip);
|
||
|
||
QCheckBox* enabled = new QCheckBox("启用纯点云伪彩色", &dialog);
|
||
enabled->setChecked(m_glWidget->isPseudoColorEnabled());
|
||
layout->addWidget(enabled);
|
||
|
||
QFormLayout* form = new QFormLayout();
|
||
QDoubleSpinBox* period = new QDoubleSpinBox(&dialog);
|
||
period->setRange(1.0, 1.0e9);
|
||
period->setDecimals(0);
|
||
period->setSingleStep(1.0);
|
||
period->setValue(m_glWidget->pseudoColorPeriod());
|
||
period->setSuffix(QStringLiteral(" mm"));
|
||
form->addRow("颜色周期:", period);
|
||
|
||
QComboBox* palette = new QComboBox(&dialog);
|
||
palette->addItem("默认 (颜色选择器色相条)", true);
|
||
palette->addItem("自定义起止色", false);
|
||
palette->setCurrentIndex(m_glWidget->isPseudoColorDefaultPalette() ? 0 : 1);
|
||
form->addRow("颜色方案:", palette);
|
||
|
||
QPushButton* startColorButton = new QPushButton(&dialog);
|
||
QPushButton* endColorButton = new QPushButton(&dialog);
|
||
QColor startColor = QColor::fromRgbF(m_glWidget->pseudoColorStartColor().x(),
|
||
m_glWidget->pseudoColorStartColor().y(),
|
||
m_glWidget->pseudoColorStartColor().z());
|
||
QColor endColor = QColor::fromRgbF(m_glWidget->pseudoColorEndColor().x(),
|
||
m_glWidget->pseudoColorEndColor().y(),
|
||
m_glWidget->pseudoColorEndColor().z());
|
||
auto updateColorButton = [](QPushButton* button, const QColor& color) {
|
||
button->setText(color.name().toUpper());
|
||
button->setStyleSheet(QString("QPushButton { background-color: %1; color: %2; }")
|
||
.arg(color.name())
|
||
.arg(color.lightness() > 160 ? "black" : "white"));
|
||
};
|
||
updateColorButton(startColorButton, startColor);
|
||
updateColorButton(endColorButton, endColor);
|
||
form->addRow("起始颜色 (低 Z):", startColorButton);
|
||
form->addRow("结束颜色 (高 Z):", endColorButton);
|
||
connect(startColorButton, &QPushButton::clicked, &dialog,
|
||
[&dialog, startColorButton, &startColor, updateColorButton, this]() {
|
||
const QColor selected = QColorDialog::getColor(startColor, this, "选择起始颜色");
|
||
if (selected.isValid()) {
|
||
startColor = selected;
|
||
updateColorButton(startColorButton, startColor);
|
||
}
|
||
});
|
||
connect(endColorButton, &QPushButton::clicked, &dialog,
|
||
[&dialog, endColorButton, &endColor, updateColorButton, this]() {
|
||
const QColor selected = QColorDialog::getColor(endColor, this, "选择结束颜色");
|
||
if (selected.isValid()) {
|
||
endColor = selected;
|
||
updateColorButton(endColorButton, endColor);
|
||
}
|
||
});
|
||
auto updatePaletteControls = [palette, startColorButton, endColorButton]() {
|
||
const bool customPalette = !palette->currentData().toBool();
|
||
startColorButton->setEnabled(customPalette);
|
||
endColorButton->setEnabled(customPalette);
|
||
};
|
||
connect(palette, QOverload<int>::of(&QComboBox::currentIndexChanged),
|
||
&dialog, [updatePaletteControls](int) { updatePaletteControls(); });
|
||
updatePaletteControls();
|
||
|
||
layout->addLayout(form);
|
||
|
||
QDialogButtonBox* buttons = new QDialogButtonBox(
|
||
QDialogButtonBox::Reset | QDialogButtonBox::Ok | QDialogButtonBox::Cancel,
|
||
&dialog);
|
||
buttons->button(QDialogButtonBox::Reset)->setText("重置默认");
|
||
buttons->button(QDialogButtonBox::Ok)->setText("应用");
|
||
buttons->button(QDialogButtonBox::Cancel)->setText("取消");
|
||
connect(buttons->button(QDialogButtonBox::Reset), &QPushButton::clicked, &dialog,
|
||
[enabled, period, palette, startColorButton, endColorButton,
|
||
&startColor, &endColor, updateColorButton, updatePaletteControls]() {
|
||
enabled->setChecked(true);
|
||
period->setValue(PointCloudGLWidget::defaultPseudoColorPeriod());
|
||
palette->setCurrentIndex(0);
|
||
const QVector3D defaultStart = PointCloudGLWidget::defaultPseudoColorStartColor();
|
||
const QVector3D defaultEnd = PointCloudGLWidget::defaultPseudoColorEndColor();
|
||
startColor = QColor::fromRgbF(defaultStart.x(), defaultStart.y(), defaultStart.z());
|
||
endColor = QColor::fromRgbF(defaultEnd.x(), defaultEnd.y(), defaultEnd.z());
|
||
updateColorButton(startColorButton, startColor);
|
||
updateColorButton(endColorButton, endColor);
|
||
updatePaletteControls();
|
||
});
|
||
connect(buttons, &QDialogButtonBox::accepted, &dialog, &QDialog::accept);
|
||
connect(buttons, &QDialogButtonBox::rejected, &dialog, &QDialog::reject);
|
||
layout->addWidget(buttons);
|
||
|
||
if (dialog.exec() != QDialog::Accepted) {
|
||
return;
|
||
}
|
||
|
||
m_glWidget->setPseudoColorSettings(
|
||
enabled->isChecked(),
|
||
static_cast<float>(period->value()),
|
||
QVector3D(startColor.redF(), startColor.greenF(), startColor.blueF()),
|
||
QVector3D(endColor.redF(), endColor.greenF(), endColor.blueF()),
|
||
palette->currentData().toBool());
|
||
statusBar()->showMessage("伪彩色参数已应用");
|
||
}
|
||
|
||
void CloudViewMainWindow::onPointCloudRangeSettings()
|
||
{
|
||
QDialog dialog(this);
|
||
dialog.setWindowTitle("XYZ 范围过滤");
|
||
dialog.setModal(true);
|
||
dialog.setMinimumWidth(420);
|
||
|
||
QVBoxLayout* layout = new QVBoxLayout(&dialog);
|
||
QCheckBox* enabled = new QCheckBox("启用 XYZ 范围过滤", &dialog);
|
||
enabled->setChecked(m_pointCloudRange.enabled);
|
||
layout->addWidget(enabled);
|
||
|
||
QHBoxLayout* modeLayout = new QHBoxLayout();
|
||
modeLayout->addWidget(new QLabel("超出范围的点", &dialog));
|
||
QComboBox* filterMode = new QComboBox(&dialog);
|
||
filterMode->addItem("替换为全 0",
|
||
static_cast<int>(PointCloudXYZRangeFilterMode::ReplaceWithZero));
|
||
filterMode->addItem("直接删除",
|
||
static_cast<int>(PointCloudXYZRangeFilterMode::Remove));
|
||
const int currentMode = static_cast<int>(m_pointCloudRange.filterMode);
|
||
const int currentModeIndex = filterMode->findData(currentMode);
|
||
filterMode->setCurrentIndex(currentModeIndex >= 0 ? currentModeIndex : 0);
|
||
modeLayout->addWidget(filterMode);
|
||
modeLayout->addStretch();
|
||
layout->addLayout(modeLayout);
|
||
|
||
QGridLayout* grid = new QGridLayout();
|
||
grid->addWidget(new QLabel("坐标轴", &dialog), 0, 0);
|
||
grid->addWidget(new QLabel("最小值", &dialog), 0, 1);
|
||
grid->addWidget(new QLabel("最大值", &dialog), 0, 2);
|
||
|
||
QDoubleSpinBox* minX = nullptr;
|
||
QDoubleSpinBox* maxX = nullptr;
|
||
QDoubleSpinBox* minY = nullptr;
|
||
QDoubleSpinBox* maxY = nullptr;
|
||
QDoubleSpinBox* minZ = nullptr;
|
||
QDoubleSpinBox* maxZ = nullptr;
|
||
auto makeSpinBox = [&dialog](double value, bool isMinimum) {
|
||
QDoubleSpinBox* spin = new QDoubleSpinBox(&dialog);
|
||
spin->setRange(-1.0e15, 1.0e15);
|
||
spin->setDecimals(6);
|
||
spin->setSingleStep(1.0);
|
||
// QDoubleSpinBox cannot represent infinity; use a wide practical bound
|
||
// so enabling a previously disabled filter does not collapse to [0, 0].
|
||
const double displayValue = std::isfinite(value)
|
||
? value : (isMinimum ? -1.0e15 : 1.0e15);
|
||
spin->setValue(displayValue);
|
||
return spin;
|
||
};
|
||
auto addAxis = [&grid, &dialog, &makeSpinBox](int row, const QString& axis,
|
||
double minValue, double maxValue,
|
||
QDoubleSpinBox*& minSpin,
|
||
QDoubleSpinBox*& maxSpin) {
|
||
minSpin = makeSpinBox(minValue, true);
|
||
maxSpin = makeSpinBox(maxValue, false);
|
||
grid->addWidget(new QLabel(axis, &dialog), row, 0);
|
||
grid->addWidget(minSpin, row, 1);
|
||
grid->addWidget(maxSpin, row, 2);
|
||
};
|
||
addAxis(1, "X", m_pointCloudRange.minX, m_pointCloudRange.maxX, minX, maxX);
|
||
addAxis(2, "Y", m_pointCloudRange.minY, m_pointCloudRange.maxY, minY, maxY);
|
||
addAxis(3, "Z", m_pointCloudRange.minZ, m_pointCloudRange.maxZ, minZ, maxZ);
|
||
layout->addLayout(grid);
|
||
|
||
auto updateEnabled = [enabled, minX, maxX, minY, maxY, minZ, maxZ]() {
|
||
const bool active = enabled->isChecked();
|
||
minX->setEnabled(active);
|
||
maxX->setEnabled(active);
|
||
minY->setEnabled(active);
|
||
maxY->setEnabled(active);
|
||
minZ->setEnabled(active);
|
||
maxZ->setEnabled(active);
|
||
};
|
||
connect(enabled, &QCheckBox::toggled, &dialog, updateEnabled);
|
||
updateEnabled();
|
||
|
||
QDialogButtonBox* buttons =
|
||
new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, &dialog);
|
||
buttons->button(QDialogButtonBox::Ok)->setText("应用");
|
||
buttons->button(QDialogButtonBox::Cancel)->setText("取消");
|
||
connect(buttons, &QDialogButtonBox::accepted, &dialog, &QDialog::accept);
|
||
connect(buttons, &QDialogButtonBox::rejected, &dialog, &QDialog::reject);
|
||
layout->addWidget(buttons);
|
||
|
||
if (dialog.exec() != QDialog::Accepted) {
|
||
return;
|
||
}
|
||
|
||
PointCloudXYZRange range;
|
||
range.enabled = enabled->isChecked();
|
||
range.minX = minX->value();
|
||
range.maxX = maxX->value();
|
||
range.minY = minY->value();
|
||
range.maxY = maxY->value();
|
||
range.minZ = minZ->value();
|
||
range.maxZ = maxZ->value();
|
||
const int selectedMode = filterMode->currentData().toInt();
|
||
range.filterMode = selectedMode == static_cast<int>(PointCloudXYZRangeFilterMode::Remove)
|
||
? PointCloudXYZRangeFilterMode::Remove
|
||
: PointCloudXYZRangeFilterMode::ReplaceWithZero;
|
||
if (!range.isValid()) {
|
||
QMessageBox::warning(this, "范围无效", "每个坐标轴的最小值不能大于最大值。");
|
||
return;
|
||
}
|
||
|
||
setPointCloudRange(range);
|
||
statusBar()->showMessage(range.enabled
|
||
? "XYZ 范围过滤已启用，将应用于后续加载"
|
||
: "XYZ 范围过滤已关闭");
|
||
}
|
||
|
||
void CloudViewMainWindow::onShowDataFormatHelp()
|
||
{
|
||
const QString circleExample =
|
||
"Circle_0_1\n"
|
||
"{0,0,10}-{0,0,0}-{50}\n";
|
||
const QString poseExample =
|
||
"Pose_0_1\n"
|
||
"{0,0,10}-{0,0,0}\n";
|
||
const QString rectangleExample =
|
||
"Rectangle_0_1\n"
|
||
"{0,0,10}-{0,0,30}-{100,60}-{255,0,255,8}\n";
|
||
const QString surfaceExample =
|
||
"Surface_0_1\n"
|
||
"{180,0,10}-{0,0,0}-{80,40}\n";
|
||
const QString segmentExample =
|
||
"# Segment 图形块，顶点也使用完整激光点格式\n"
|
||
"Segment_1_2\n"
|
||
"{0,0,10}-{0,0}-{0,0}-{0,180,255,5}\n"
|
||
"{80,0,10}-{0,0}-{0,0}-{0,180,255,5}\n";
|
||
const QString polyExample =
|
||
"# Poly 顶点完全兼容激光点格式；A 按现有约定作为线宽\n"
|
||
"Poly_13_2\n"
|
||
"{0,-1,10}-{0,0}-{0,0}-{250,0,0,8}\n"
|
||
"{80,0,10}-{0,0}-{0,0}-{250,0,0,8}\n";
|
||
|
||
const QString examplesDir = cloudViewExamplesDirectory();
|
||
const QString circleFile = QDir(examplesDir).filePath("circle_example.txt");
|
||
const QString poseFile = QDir(examplesDir).filePath("pose_example.txt");
|
||
const QString rectangleFile = QDir(examplesDir).filePath("rectangle_example.txt");
|
||
const QString surfaceFile = QDir(examplesDir).filePath("surface_example.txt");
|
||
const QString segmentFile = QDir(examplesDir).filePath("segment_example.txt");
|
||
const QString polyFile = QDir(examplesDir).filePath("poly_example.txt");
|
||
writeCloudViewExampleFile("circle_example.txt", circleExample);
|
||
writeCloudViewExampleFile("pose_example.txt", poseExample);
|
||
writeCloudViewExampleFile("rectangle_example.txt", rectangleExample);
|
||
writeCloudViewExampleFile("surface_example.txt", surfaceExample);
|
||
writeCloudViewExampleFile("segment_example.txt", segmentExample);
|
||
writeCloudViewExampleFile("poly_example.txt", polyExample);
|
||
|
||
QDialog dialog(this);
|
||
dialog.setWindowTitle("CloudView 数据格式示例");
|
||
dialog.resize(820, 620);
|
||
|
||
QVBoxLayout* layout = new QVBoxLayout(&dialog);
|
||
QLabel* tip = new QLabel(
|
||
QString("示例文件已保存到用户配置目录：%1。点击下方按钮可将圆、矩形、面、姿态点、Segment 和 Poly 一键加入当前视图。")
|
||
.arg(examplesDir),
|
||
&dialog);
|
||
tip->setWordWrap(true);
|
||
tip->setStyleSheet("color: gray;");
|
||
layout->addWidget(tip);
|
||
|
||
QTextEdit* example = new QTextEdit(&dialog);
|
||
example->setReadOnly(true);
|
||
example->setFont(QFont("Consolas", 10));
|
||
example->setPlainText(
|
||
"# 1. 圆：Circle_编号_记录数\n" + circleExample + "\n"
|
||
"# 2. 矩形：Rectangle_编号_记录数（颜色为 RGBA 0~255）\n" + rectangleExample + "\n"
|
||
"# 3. 面：Surface_编号_记录数\n" + surfaceExample + "\n"
|
||
"# 4. 姿态点：Pose_编号_记录数\n" + poseExample + "\n"
|
||
"# 5. Segment：Segment_编号_点数（每个顶点与激光点格式完全一致）\n" + segmentExample + "\n"
|
||
"# 6. Poly：Poly_编号_点数（每个顶点与激光点格式完全一致）\n" + polyExample);
|
||
layout->addWidget(example);
|
||
|
||
QPushButton* addButton = new QPushButton("一键加入示例到视图", &dialog);
|
||
connect(addButton, &QPushButton::clicked, &dialog, [this, &dialog, circleFile, rectangleFile, surfaceFile, segmentFile, polyFile, poseFile]() {
|
||
const bool circleOk = loadCircleShapeFile(circleFile);
|
||
const bool rectangleOk = loadRectangleShapeFile(rectangleFile);
|
||
const bool surfaceOk = loadSurfaceShapeFile(surfaceFile);
|
||
const bool segmentOk = loadSegmentFile(segmentFile);
|
||
const bool polyOk = loadSegmentFile(polyFile);
|
||
bool poseOk = QFileInfo::exists(poseFile);
|
||
if (poseOk) {
|
||
bool scaleOk = false;
|
||
float scale = m_editPoseScale->text().toFloat(&scaleOk);
|
||
if (!scaleOk || scale <= 0.0f) {
|
||
scale = 10.0f;
|
||
}
|
||
QVector<PosePoint> poses;
|
||
poses.append(PosePoint(0.0f, 0.0f, 10.0f, 0.0f, 0.0f, 0.0f, scale));
|
||
m_glWidget->addPosePoints(poses);
|
||
}
|
||
if (circleOk || rectangleOk || surfaceOk || segmentOk || polyOk || poseOk) {
|
||
statusBar()->showMessage("示例图形已加入视图");
|
||
dialog.accept();
|
||
} else {
|
||
QMessageBox::warning(this, "加载失败", "示例文件未能加入视图，请检查用户配置目录中的示例文件。");
|
||
}
|
||
});
|
||
layout->addWidget(addButton);
|
||
|
||
QDialogButtonBox* buttons = new QDialogButtonBox(QDialogButtonBox::Close, &dialog);
|
||
connect(buttons, &QDialogButtonBox::rejected, &dialog, &QDialog::reject);
|
||
layout->addWidget(buttons);
|
||
dialog.exec();
|
||
}
|
||
|
||
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::onPoseCompensation()
|
||
{
|
||
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(
|
||
"输入当前欧拉角和期望欧拉角（单位: 度），计算出补偿角。\n"
|
||
"分量相减：补偿 = 期望 − 当前（仅小角度差/单轴旋转时近似有效，结果与顺序无关）\n"
|
||
"矩阵补偿：补偿 = 提取(R_cur^T × R_des)\n"
|
||
" · 输入顺序：当前/期望欧拉角的旋转顺序（取决于机器人/算法侧约定）\n"
|
||
" · 补偿输出顺序：反解 R_comp 用的旋转顺序，要与下游施加补偿的约定一致\n"
|
||
" （DetectPresenter::ApplyAxesRotation 硬编码 R = Rx·Ry·Rz，即 XYZ 内旋）",
|
||
dialog);
|
||
tip->setWordWrap(true);
|
||
tip->setStyleSheet("color: gray;");
|
||
layout->addWidget(tip);
|
||
|
||
auto fillEulerOrderItems = [](QComboBox* combo) {
|
||
combo->addItem("ZYX (Yaw-Pitch-Roll)", static_cast<int>(EulerRotationOrder::ZYX));
|
||
combo->addItem("XYZ (Roll-Pitch-Yaw)", static_cast<int>(EulerRotationOrder::XYZ));
|
||
combo->addItem("ZXY (Yaw-Roll-Pitch)", static_cast<int>(EulerRotationOrder::ZXY));
|
||
combo->addItem("YXZ (Pitch-Roll-Yaw)", static_cast<int>(EulerRotationOrder::YXZ));
|
||
combo->addItem("XZY (Roll-Yaw-Pitch)", static_cast<int>(EulerRotationOrder::XZY));
|
||
combo->addItem("YZX (Pitch-Yaw-Roll)", static_cast<int>(EulerRotationOrder::YZX));
|
||
};
|
||
|
||
QComboBox* comboInputOrder = new QComboBox(dialog);
|
||
fillEulerOrderItems(comboInputOrder);
|
||
if (m_comboEulerOrder && m_comboEulerOrder->currentIndex() >= 0) {
|
||
comboInputOrder->setCurrentIndex(m_comboEulerOrder->currentIndex());
|
||
}
|
||
|
||
QComboBox* comboOutputOrder = new QComboBox(dialog);
|
||
fillEulerOrderItems(comboOutputOrder);
|
||
// 默认 XYZ，匹配 DetectPresenter::ApplyAxesRotation 中 R = Rx·Ry·Rz 的硬编码
|
||
const int outputXyzIndex = comboOutputOrder->findData(static_cast<int>(EulerRotationOrder::XYZ));
|
||
if (outputXyzIndex >= 0) {
|
||
comboOutputOrder->setCurrentIndex(outputXyzIndex);
|
||
}
|
||
|
||
QHBoxLayout* orderLayout = new QHBoxLayout();
|
||
orderLayout->addWidget(new QLabel("输入顺序:", dialog));
|
||
orderLayout->addWidget(comboInputOrder);
|
||
orderLayout->addSpacing(20);
|
||
orderLayout->addWidget(new QLabel("补偿输出顺序:", dialog));
|
||
orderLayout->addWidget(comboOutputOrder);
|
||
orderLayout->addStretch();
|
||
layout->addLayout(orderLayout);
|
||
|
||
QComboBox* comboMethod = new QComboBox(dialog);
|
||
comboMethod->addItem("分量相减（期望 − 当前）");
|
||
comboMethod->addItem("矩阵补偿（R_cur^T × R_des）");
|
||
|
||
QHBoxLayout* methodLayout = new QHBoxLayout();
|
||
methodLayout->addWidget(new QLabel("计算方法:", dialog));
|
||
methodLayout->addWidget(comboMethod);
|
||
methodLayout->addStretch();
|
||
layout->addLayout(methodLayout);
|
||
|
||
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;
|
||
};
|
||
|
||
QGroupBox* currentGroup = new QGroupBox("当前欧拉角 (deg)", dialog);
|
||
QGridLayout* currentLayout = new QGridLayout(currentGroup);
|
||
QDoubleSpinBox* curRx = makeAngleSpin(currentGroup);
|
||
QDoubleSpinBox* curRy = makeAngleSpin(currentGroup);
|
||
QDoubleSpinBox* curRz = makeAngleSpin(currentGroup);
|
||
currentLayout->addWidget(new QLabel("RX:", currentGroup), 0, 0);
|
||
currentLayout->addWidget(curRx, 0, 1);
|
||
currentLayout->addWidget(new QLabel("RY:", currentGroup), 1, 0);
|
||
currentLayout->addWidget(curRy, 1, 1);
|
||
currentLayout->addWidget(new QLabel("RZ:", currentGroup), 2, 0);
|
||
currentLayout->addWidget(curRz, 2, 1);
|
||
|
||
QGroupBox* desiredGroup = new QGroupBox("期望欧拉角 (deg)", dialog);
|
||
QGridLayout* desiredLayout = new QGridLayout(desiredGroup);
|
||
QDoubleSpinBox* desRx = makeAngleSpin(desiredGroup);
|
||
QDoubleSpinBox* desRy = makeAngleSpin(desiredGroup);
|
||
QDoubleSpinBox* desRz = makeAngleSpin(desiredGroup);
|
||
desiredLayout->addWidget(new QLabel("RX:", desiredGroup), 0, 0);
|
||
desiredLayout->addWidget(desRx, 0, 1);
|
||
desiredLayout->addWidget(new QLabel("RY:", desiredGroup), 1, 0);
|
||
desiredLayout->addWidget(desRy, 1, 1);
|
||
desiredLayout->addWidget(new QLabel("RZ:", desiredGroup), 2, 0);
|
||
desiredLayout->addWidget(desRz, 2, 1);
|
||
|
||
QGroupBox* resultGroup = new QGroupBox("补偿欧拉角 (deg)", dialog);
|
||
QGridLayout* resultLayout = new QGridLayout(resultGroup);
|
||
QLineEdit* resultRx = new QLineEdit(resultGroup);
|
||
QLineEdit* resultRy = new QLineEdit(resultGroup);
|
||
QLineEdit* resultRz = new QLineEdit(resultGroup);
|
||
resultRx->setReadOnly(true);
|
||
resultRy->setReadOnly(true);
|
||
resultRz->setReadOnly(true);
|
||
resultRx->setAlignment(Qt::AlignRight);
|
||
resultRy->setAlignment(Qt::AlignRight);
|
||
resultRz->setAlignment(Qt::AlignRight);
|
||
resultRx->setStyleSheet("background-color: #f0f0f0; font-weight: bold;");
|
||
resultRy->setStyleSheet("background-color: #f0f0f0; font-weight: bold;");
|
||
resultRz->setStyleSheet("background-color: #f0f0f0; font-weight: bold;");
|
||
resultLayout->addWidget(new QLabel("RX:", resultGroup), 0, 0);
|
||
resultLayout->addWidget(resultRx, 0, 1);
|
||
resultLayout->addWidget(new QLabel("RY:", resultGroup), 1, 0);
|
||
resultLayout->addWidget(resultRy, 1, 1);
|
||
resultLayout->addWidget(new QLabel("RZ:", resultGroup), 2, 0);
|
||
resultLayout->addWidget(resultRz, 2, 1);
|
||
|
||
layout->addWidget(currentGroup);
|
||
layout->addWidget(desiredGroup);
|
||
layout->addWidget(resultGroup);
|
||
|
||
QHBoxLayout* actionLayout = new QHBoxLayout();
|
||
QPushButton* calcButton = new QPushButton("计算补偿", dialog);
|
||
QPushButton* closeButton = new QPushButton("关闭", dialog);
|
||
actionLayout->addStretch();
|
||
actionLayout->addWidget(calcButton);
|
||
actionLayout->addWidget(closeButton);
|
||
layout->addLayout(actionLayout);
|
||
|
||
connect(calcButton, &QPushButton::clicked, dialog, [=]() {
|
||
if (comboMethod->currentIndex() == 0) {
|
||
// 分量相减
|
||
resultRx->setText(QString::number(desRx->value() - curRx->value(), 'f', 6));
|
||
resultRy->setText(QString::number(desRy->value() - curRy->value(), 'f', 6));
|
||
resultRz->setText(QString::number(desRz->value() - curRz->value(), 'f', 6));
|
||
} else {
|
||
// 矩阵补偿
|
||
const EulerRotationOrder inputOrder = static_cast<EulerRotationOrder>(comboInputOrder->currentData().toInt());
|
||
const EulerRotationOrder outputOrder = static_cast<EulerRotationOrder>(comboOutputOrder->currentData().toInt());
|
||
|
||
// 输入欧拉角按 inputOrder 还原成旋转矩阵
|
||
const QMatrix4x4 curMat = buildEulerRotationMatrix(inputOrder, curRx->value(), curRy->value(), curRz->value());
|
||
const QMatrix4x4 desMat = buildEulerRotationMatrix(inputOrder, desRx->value(), desRy->value(), desRz->value());
|
||
|
||
// R_des = R_cur * R_comp（工具坐标系补偿，R_comp = R_cur^T * R_des）
|
||
const QMatrix4x4 compMat = curMat.transposed() * desMat;
|
||
|
||
// 补偿矩阵按 outputOrder 反解（要与下游施加补偿的旋转顺序一致）
|
||
const QVector3D euler = rotationMatrixToEuler(outputOrder, compMat);
|
||
resultRx->setText(QString::number(euler.x(), 'f', 6));
|
||
resultRy->setText(QString::number(euler.y(), 'f', 6));
|
||
resultRz->setText(QString::number(euler.z(), 'f', 6));
|
||
}
|
||
});
|
||
|
||
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::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) {
|
||
const float distance = m_glWidget->calculateDistance(updatedPoints[0], updatedPoints[1]);
|
||
updateMeasureResultDisplay(updatedPoints[0], updatedPoints[1], distance, "点1坐标已更新");
|
||
} 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) {
|
||
const float distance = m_glWidget->calculateDistance(updatedPoints[0], updatedPoints[1]);
|
||
updateMeasureResultDisplay(updatedPoints[0], updatedPoints[1], distance, "点2坐标已更新");
|
||
} 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);
|
||
}
|
||
|