3137 lines
108 KiB
C++
3137 lines
108 KiB
C++
#include "PointCloudGLWidget.h"
|
||
#include <QColor>
|
||
#include <QDebug>
|
||
#include <QtMath>
|
||
#include <QDialog>
|
||
#include <QDialogButtonBox>
|
||
#include <QDoubleSpinBox>
|
||
#include <QFormLayout>
|
||
#include <QGestureEvent>
|
||
#include <QHBoxLayout>
|
||
#include <QOpenGLContext>
|
||
#include <QPainter>
|
||
#include <QPinchGesture>
|
||
#include <QPushButton>
|
||
#include <QResizeEvent>
|
||
#include <QToolButton>
|
||
#include <QVBoxLayout>
|
||
#include <algorithm>
|
||
#include <array>
|
||
#include <cfloat>
|
||
#include <cmath>
|
||
#include <map>
|
||
#include "VrLog.h"
|
||
|
||
// OpenGL/GLU 头文件
|
||
#ifdef _WIN32
|
||
#include <windows.h>
|
||
#include <GL/gl.h>
|
||
#include <GL/glu.h>
|
||
#else
|
||
#include <GL/gl.h>
|
||
#include <GL/glu.h>
|
||
#endif
|
||
|
||
PointCloudGLWidget::PointCloudGLWidget(QWidget* parent)
|
||
: PointCloudGLWidget(parent, true)
|
||
{
|
||
}
|
||
|
||
namespace
|
||
{
|
||
constexpr float kDefaultPseudoColorPeriod = 500.0f;
|
||
constexpr int kPseudoColorHueCount = 360;
|
||
|
||
// 将角度归一化到 [-180, 180]，避免往同一方向旋转时无限累加
|
||
float NormalizeAngleDegrees(float angle)
|
||
{
|
||
float wrapped = std::fmod(angle, 360.0f);
|
||
if (wrapped > 180.0f) {
|
||
wrapped -= 360.0f;
|
||
} else if (wrapped < -180.0f) {
|
||
wrapped += 360.0f;
|
||
}
|
||
return wrapped;
|
||
}
|
||
|
||
QVector3D clampPseudoColor(const QVector3D& color)
|
||
{
|
||
const auto clampComponent = [](float component)
|
||
{
|
||
return std::isfinite(component) ? qBound(0.0f, component, 1.0f) : 0.0f;
|
||
};
|
||
return QVector3D(clampComponent(color.x()),
|
||
clampComponent(color.y()),
|
||
clampComponent(color.z()));
|
||
}
|
||
|
||
// 颜色选择器色相条（S/V 取最大）的 360 色查找表。
|
||
const std::array<QVector3D, kPseudoColorHueCount>& defaultPseudoColorLut()
|
||
{
|
||
static const std::array<QVector3D, kPseudoColorHueCount> lut = []()
|
||
{
|
||
std::array<QVector3D, kPseudoColorHueCount> colors{};
|
||
for (int hue = 0; hue < kPseudoColorHueCount; ++hue)
|
||
{
|
||
const QColor color = QColor::fromHsv(hue, 255, 255);
|
||
colors[static_cast<size_t>(hue)] = QVector3D(color.redF(),
|
||
color.greenF(),
|
||
color.blueF());
|
||
}
|
||
return colors;
|
||
}();
|
||
return lut;
|
||
}
|
||
|
||
QVector3D defaultHuePseudoColor(float t)
|
||
{
|
||
const float clamped = qBound(0.0f, t, 1.0f);
|
||
// 与颜色选择器色相条一致：红 → 品红 → 蓝 → 青 → 绿 → 黄 → 红。
|
||
int hue = static_cast<int>(std::floor(360.0f * (1.0f - clamped))) % kPseudoColorHueCount;
|
||
if (hue < 0)
|
||
{
|
||
hue += kPseudoColorHueCount;
|
||
}
|
||
return defaultPseudoColorLut()[static_cast<size_t>(hue)];
|
||
}
|
||
}
|
||
|
||
PointCloudGLWidget::PointCloudGLWidget(QWidget* parent, bool showControlButtons)
|
||
: QOpenGLWidget(parent)
|
||
, m_shader(nullptr)
|
||
, m_shaderReady(false)
|
||
, m_mvpLocation(0)
|
||
, m_positionLocation(0)
|
||
, m_colorLocation(0)
|
||
, m_pointSizeLocation(0)
|
||
, m_alphaLocation(0)
|
||
, m_useVertexColorLocation(0)
|
||
, m_uniformColorLocation(0)
|
||
, m_distance(100.0f)
|
||
, m_pinchStartDistance(100.0f)
|
||
, m_rotationX(0.0f)
|
||
, m_rotationY(0.0f)
|
||
, m_rotationZ(0.0f)
|
||
, m_rotation(QQuaternion()) // 初始化为单位四元数
|
||
, m_center(0, 0, 0)
|
||
, m_pan(0, 0, 0)
|
||
, m_minBound(-50, -50, -50)
|
||
, m_maxBound(50, 50, 50)
|
||
, m_leftButtonPressed(false)
|
||
, m_rightButtonPressed(false)
|
||
, m_middleButtonPressed(false)
|
||
, m_currentColor(PointCloudColor::White)
|
||
, m_pointSize(1.0f)
|
||
// 纯 XYZ 点云默认启用参考色环；带 RGB 的点云仍使用原始颜色。
|
||
, m_pseudoColorEnabled(true)
|
||
, m_pseudoColorPeriod(kDefaultPseudoColorPeriod)
|
||
, m_pseudoColorStartColor(0.0f, 0.0f, 1.0f)
|
||
, m_pseudoColorEndColor(1.0f, 0.0f, 0.0f)
|
||
, m_pseudoColorDefaultPalette(true)
|
||
, m_lineSelectMode(LineSelectMode::Vertical)
|
||
, m_eulerRotationOrder(EulerRotationOrder::ZYX)
|
||
, m_measureDistanceEnabled(false)
|
||
, m_hasListHighlightPoint(false)
|
||
, m_colorIndex(0)
|
||
, m_showControlButtons(showControlButtons)
|
||
, m_controlPanel(nullptr)
|
||
, m_viewToolbar(nullptr)
|
||
, m_viewAngleButton(nullptr)
|
||
, m_zoomInButton(nullptr)
|
||
, m_zoomOutButton(nullptr)
|
||
{
|
||
setFocusPolicy(Qt::StrongFocus);
|
||
setMouseTracking(true);
|
||
setAttribute(Qt::WA_AcceptTouchEvents, true);
|
||
grabGesture(Qt::PinchGesture);
|
||
|
||
// 确保 m_model 是单位矩阵
|
||
m_model.setToIdentity();
|
||
|
||
// 默认按固定 Z 周期走颜色选择器色相条。
|
||
createOverlayControls();
|
||
connect(this, &PointCloudGLWidget::viewAnglesChanged,
|
||
this, [this](float, float, float) { updateViewAngleDisplay(); });
|
||
}
|
||
|
||
bool PointCloudGLWidget::event(QEvent* event)
|
||
{
|
||
if (event->type() == QEvent::Gesture) {
|
||
auto* gestureEvent = static_cast<QGestureEvent*>(event);
|
||
if (auto* pinch = static_cast<QPinchGesture*>(
|
||
gestureEvent->gesture(Qt::PinchGesture))) {
|
||
const bool handled = handlePinchGesture(pinch);
|
||
if (handled) {
|
||
gestureEvent->accept(pinch);
|
||
return true;
|
||
}
|
||
}
|
||
}
|
||
return QOpenGLWidget::event(event);
|
||
}
|
||
|
||
bool PointCloudGLWidget::handlePinchGesture(QPinchGesture* gesture)
|
||
{
|
||
if (!gesture) {
|
||
return false;
|
||
}
|
||
|
||
if (gesture->state() == Qt::GestureStarted) {
|
||
m_pinchStartDistance = m_distance;
|
||
}
|
||
|
||
const qreal scale = gesture->totalScaleFactor();
|
||
if (scale <= 0.0 || !std::isfinite(static_cast<double>(scale))) {
|
||
return false;
|
||
}
|
||
|
||
m_distance = qBound(minimumCameraDistance(),
|
||
m_pinchStartDistance / static_cast<float>(scale),
|
||
1.0e7f);
|
||
update();
|
||
return true;
|
||
}
|
||
|
||
PointCloudGLWidget::~PointCloudGLWidget()
|
||
{
|
||
// 在 GL 上下文中释放所有 VBO
|
||
makeCurrent();
|
||
for (auto& cloudData : m_pointClouds) {
|
||
releaseVBO(cloudData);
|
||
}
|
||
if (m_shader) {
|
||
delete m_shader;
|
||
m_shader = nullptr;
|
||
}
|
||
doneCurrent();
|
||
}
|
||
|
||
void PointCloudGLWidget::uploadToVBO(PointCloudData& data)
|
||
{
|
||
// 先释放旧的 VBO
|
||
releaseVBO(data);
|
||
|
||
if (data.vertices.empty()) {
|
||
return;
|
||
}
|
||
|
||
// 创建并上传顶点 VBO
|
||
data.vertexBuffer.create();
|
||
data.vertexBuffer.bind();
|
||
data.vertexBuffer.setUsagePattern(QOpenGLBuffer::StaticDraw);
|
||
data.vertexBuffer.allocate(data.vertices.data(),
|
||
static_cast<int>(data.vertices.size() * sizeof(float)));
|
||
data.vertexBuffer.release();
|
||
|
||
// 如果有颜色数据，创建并上传颜色 VBO
|
||
if ((data.hasColor || data.pseudoColor) && !data.colors.empty()) {
|
||
data.colorBuffer.create();
|
||
data.colorBuffer.bind();
|
||
data.colorBuffer.setUsagePattern(QOpenGLBuffer::StaticDraw);
|
||
data.colorBuffer.allocate(data.colors.data(),
|
||
static_cast<int>(data.colors.size() * sizeof(float)));
|
||
data.colorBuffer.release();
|
||
}
|
||
|
||
data.vboCreated = true;
|
||
}
|
||
|
||
void PointCloudGLWidget::releaseVBO(PointCloudData& data)
|
||
{
|
||
if (!data.vboCreated) {
|
||
return;
|
||
}
|
||
|
||
if (data.vertexBuffer.isCreated()) {
|
||
data.vertexBuffer.destroy();
|
||
}
|
||
if (data.colorBuffer.isCreated()) {
|
||
data.colorBuffer.destroy();
|
||
}
|
||
|
||
data.vboCreated = false;
|
||
}
|
||
|
||
void PointCloudGLWidget::createOverlayControls()
|
||
{
|
||
const int buttonSize = 32;
|
||
const QString toolButtonStyle =
|
||
"QToolButton {"
|
||
" background-color: rgba(20, 36, 58, 200);"
|
||
" color: #d8e8f4;"
|
||
" border: 1px solid #2b4f77;"
|
||
" border-radius: 4px;"
|
||
" font-size: 13px;"
|
||
"}"
|
||
"QToolButton:hover { background-color: #204a72; }"
|
||
"QToolButton:pressed { background-color: #10243a; }";
|
||
|
||
// ── 左侧视图方向工具栏（纵向排布） ──
|
||
struct ViewPreset {
|
||
const char* label;
|
||
const char* tooltip;
|
||
float rx, ry, rz;
|
||
};
|
||
// 坐标系定义：X向右，Y向下，Z朝后（与 CloudView 一致）
|
||
const ViewPreset presets[] = {
|
||
{"前", "正视 (XY面)", 180.0f, 0.0f, 0.0f},
|
||
{"后", "后视", 180.0f, 180.0f, 0.0f},
|
||
{"左", "左视 (YZ面)", 180.0f, 90.0f, 0.0f},
|
||
{"右", "右视", 180.0f, -90.0f, 0.0f},
|
||
{"上", "俯视 (XZ面)", 90.0f, 0.0f, 0.0f},
|
||
{"下", "仰视", -90.0f, 0.0f, 0.0f},
|
||
{"机", "机械臂", -90.0f, 90.0f, 0.0f},
|
||
};
|
||
|
||
m_viewToolbar = new QWidget(this);
|
||
m_viewToolbar->setStyleSheet("background: transparent;");
|
||
auto* toolbarLayout = new QVBoxLayout(m_viewToolbar);
|
||
toolbarLayout->setContentsMargins(4, 4, 4, 4);
|
||
toolbarLayout->setSpacing(4);
|
||
// 工具栏尺寸跟随内容，便于垂直居中定位
|
||
toolbarLayout->setSizeConstraint(QLayout::SetFixedSize);
|
||
|
||
for (const ViewPreset& preset : presets) {
|
||
auto* button = new QToolButton(m_viewToolbar);
|
||
button->setText(QString::fromUtf8(preset.label));
|
||
button->setToolTip(QString::fromUtf8(preset.tooltip));
|
||
button->setFixedSize(buttonSize, buttonSize);
|
||
button->setStyleSheet(toolButtonStyle);
|
||
const float rx = preset.rx;
|
||
const float ry = preset.ry;
|
||
const float rz = preset.rz;
|
||
connect(button, &QToolButton::clicked, this, [this, rx, ry, rz]() {
|
||
setViewAngles(rx, ry, rz);
|
||
});
|
||
m_viewButtons.append(button);
|
||
toolbarLayout->addWidget(button);
|
||
}
|
||
toolbarLayout->addStretch(1);
|
||
m_viewToolbar->setFixedWidth(buttonSize + 12);
|
||
m_viewToolbar->setVisible(m_showControlButtons);
|
||
|
||
// ── 左下角旋转角度显示按钮（点击弹窗输入角度） ──
|
||
m_viewAngleButton = new QPushButton(this);
|
||
m_viewAngleButton->setStyleSheet(
|
||
"QPushButton {"
|
||
" background-color: rgba(20, 36, 58, 200);"
|
||
" color: #d8e8f4;"
|
||
" border: 1px solid #2b4f77;"
|
||
" border-radius: 4px;"
|
||
" padding: 4px 8px;"
|
||
" text-align: left;"
|
||
"}"
|
||
"QPushButton:hover { background-color: #204a72; }");
|
||
connect(m_viewAngleButton, &QPushButton::clicked,
|
||
this, &PointCloudGLWidget::showViewAngleDialog);
|
||
// 显式指定字体，保证 fontMetrics 度量与实际渲染一致
|
||
QFont angleFont = m_viewAngleButton->font();
|
||
angleFont.setPixelSize(12);
|
||
m_viewAngleButton->setFont(angleFont);
|
||
// 竖向三行显示 RX/RY/RZ：宽度按最长一行算，高度按三行算
|
||
const QString longestAngleLine = QStringLiteral("RX:-360.0");
|
||
const int angleTextWidth =
|
||
m_viewAngleButton->fontMetrics().horizontalAdvance(longestAngleLine);
|
||
const int angleLineHeight = m_viewAngleButton->fontMetrics().lineSpacing();
|
||
m_viewAngleButton->setFixedSize(angleTextWidth + 24, angleLineHeight * 3 + 16);
|
||
m_viewAngleButton->setVisible(m_showControlButtons);
|
||
|
||
// ── 右侧悬浮缩放按钮（垂直居中，与 CloudView 位置一致） ──
|
||
const QString zoomButtonStyle =
|
||
"QToolButton {"
|
||
" background-color: rgba(20, 36, 58, 200);"
|
||
" color: #e8f2fb;"
|
||
" border: 1px solid #2b4f77;"
|
||
" border-radius: 4px;"
|
||
" font-size: 26px;"
|
||
" font-weight: bold;"
|
||
"}"
|
||
"QToolButton:hover { background-color: #204a72; }"
|
||
"QToolButton:pressed { background-color: #10243a; }";
|
||
const int zoomButtonSize = 28;
|
||
m_zoomInButton = new QToolButton(this);
|
||
m_zoomInButton->setText("+");
|
||
m_zoomInButton->setFixedSize(zoomButtonSize, zoomButtonSize);
|
||
m_zoomInButton->setStyleSheet(zoomButtonStyle);
|
||
m_zoomInButton->setAutoRepeat(true);
|
||
m_zoomInButton->setAutoRepeatDelay(300);
|
||
m_zoomInButton->setAutoRepeatInterval(50);
|
||
connect(m_zoomInButton, &QToolButton::clicked, this, &PointCloudGLWidget::zoomIn);
|
||
m_zoomInButton->setVisible(m_showControlButtons);
|
||
|
||
m_zoomOutButton = new QToolButton(this);
|
||
m_zoomOutButton->setText("-");
|
||
m_zoomOutButton->setFixedSize(zoomButtonSize, zoomButtonSize);
|
||
m_zoomOutButton->setStyleSheet(zoomButtonStyle);
|
||
m_zoomOutButton->setAutoRepeat(true);
|
||
m_zoomOutButton->setAutoRepeatDelay(300);
|
||
m_zoomOutButton->setAutoRepeatInterval(50);
|
||
connect(m_zoomOutButton, &QToolButton::clicked, this, &PointCloudGLWidget::zoomOut);
|
||
m_zoomOutButton->setVisible(m_showControlButtons);
|
||
|
||
updateViewAngleDisplay();
|
||
repositionOverlayControls();
|
||
}
|
||
|
||
void PointCloudGLWidget::repositionOverlayControls()
|
||
{
|
||
const int widgetWidth = width();
|
||
const int widgetHeight = height();
|
||
if (widgetWidth <= 0 || widgetHeight <= 0) {
|
||
return;
|
||
}
|
||
|
||
// 左侧视图工具栏：垂直居中
|
||
if (m_viewToolbar) {
|
||
m_viewToolbar->move(8, qMax(0, (widgetHeight - m_viewToolbar->height()) / 2));
|
||
}
|
||
|
||
// 左下角旋转角度按钮
|
||
if (m_viewAngleButton) {
|
||
m_viewAngleButton->adjustSize();
|
||
const int margin = 8;
|
||
m_viewAngleButton->move(margin,
|
||
widgetHeight - m_viewAngleButton->height() - margin);
|
||
}
|
||
|
||
// 右侧缩放按钮：垂直居中
|
||
if (m_zoomInButton && m_zoomOutButton) {
|
||
const int buttonWidth = m_zoomInButton->width();
|
||
const int buttonHeight = m_zoomInButton->height();
|
||
const int margin = 8;
|
||
const int gap = 4;
|
||
const int x = widgetWidth - buttonWidth - margin;
|
||
const int y = qMax(0, (widgetHeight - buttonHeight * 2 - gap) / 2);
|
||
m_zoomInButton->move(x, y);
|
||
m_zoomOutButton->move(x, y + buttonHeight + gap);
|
||
}
|
||
}
|
||
|
||
|
||
void PointCloudGLWidget::updateViewAngleDisplay()
|
||
{
|
||
if (!m_viewAngleButton) {
|
||
return;
|
||
}
|
||
m_viewAngleButton->setText(QStringLiteral("RX:%1\nRY:%2\nRZ:%3")
|
||
.arg(m_rotationX, 0, 'f', 1)
|
||
.arg(m_rotationY, 0, 'f', 1)
|
||
.arg(m_rotationZ, 0, 'f', 1));
|
||
}
|
||
|
||
void PointCloudGLWidget::showViewAngleDialog()
|
||
{
|
||
QDialog dialog(this);
|
||
dialog.setWindowTitle(QStringLiteral("设置视图角度"));
|
||
dialog.setMinimumWidth(240);
|
||
|
||
auto* form = new QFormLayout(&dialog);
|
||
auto* spinX = new QDoubleSpinBox(&dialog);
|
||
auto* spinY = new QDoubleSpinBox(&dialog);
|
||
auto* spinZ = new QDoubleSpinBox(&dialog);
|
||
for (QDoubleSpinBox* spin : {spinX, spinY, spinZ}) {
|
||
spin->setRange(-180.0, 180.0);
|
||
spin->setDecimals(1);
|
||
spin->setSingleStep(1.0);
|
||
spin->setSuffix(QStringLiteral(" °"));
|
||
}
|
||
spinX->setValue(m_rotationX);
|
||
spinY->setValue(m_rotationY);
|
||
spinZ->setValue(m_rotationZ);
|
||
form->addRow(QStringLiteral("RX"), spinX);
|
||
form->addRow(QStringLiteral("RY"), spinY);
|
||
form->addRow(QStringLiteral("RZ"), spinZ);
|
||
|
||
auto* buttons = new QDialogButtonBox(
|
||
QDialogButtonBox::Ok | QDialogButtonBox::Cancel, &dialog);
|
||
buttons->button(QDialogButtonBox::Ok)->setText(QStringLiteral("确认"));
|
||
buttons->button(QDialogButtonBox::Cancel)->setText(QStringLiteral("取消"));
|
||
connect(buttons, &QDialogButtonBox::accepted, &dialog, &QDialog::accept);
|
||
connect(buttons, &QDialogButtonBox::rejected, &dialog, &QDialog::reject);
|
||
form->addRow(buttons);
|
||
|
||
if (dialog.exec() == QDialog::Accepted) {
|
||
setViewAngles(static_cast<float>(spinX->value()),
|
||
static_cast<float>(spinY->value()),
|
||
static_cast<float>(spinZ->value()));
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::initializeGL()
|
||
{
|
||
initializeOpenGLFunctions();
|
||
|
||
LOG_INFO("[CloudView] OpenGL initialized, version: %s\n", reinterpret_cast<const char*>(glGetString(GL_VERSION)));
|
||
|
||
glClearColor(0.15f, 0.15f, 0.15f, 1.0f);
|
||
glEnable(GL_DEPTH_TEST);
|
||
// 桌面 GL（GL 3.2+）下，顶点着色器输出的 gl_PointSize 需显式开启才生效；
|
||
// GLES2 总是使用 shader 点大小，无需此开关。
|
||
if (QOpenGLContext::currentContext() &&
|
||
!QOpenGLContext::currentContext()->isOpenGLES()) {
|
||
#ifdef GL_PROGRAM_POINT_SIZE
|
||
glEnable(GL_PROGRAM_POINT_SIZE);
|
||
#endif
|
||
}
|
||
if (!m_dynamicVbo.isCreated()) {
|
||
m_dynamicVbo.create();
|
||
}
|
||
m_shaderReady = false;
|
||
ensureShader();
|
||
}
|
||
|
||
bool PointCloudGLWidget::ensureShader()
|
||
{
|
||
if (m_shaderReady) {
|
||
return m_shader != nullptr;
|
||
}
|
||
m_shaderReady = true;
|
||
|
||
if (!m_shader) {
|
||
auto* program = new QOpenGLShaderProgram();
|
||
program->bindAttributeLocation("aPosition", 0);
|
||
program->bindAttributeLocation("aColor", 1);
|
||
|
||
if (!program->addShaderFromSourceCode(
|
||
QOpenGLShader::Vertex,
|
||
// GLSL ES 1.00 的 vertex shader 有默认 float 精度（highp），
|
||
// 但显式声明更稳妥，兼容性更好。
|
||
"precision highp float;\n"
|
||
"attribute vec3 aPosition;\n"
|
||
"attribute vec3 aColor;\n"
|
||
"uniform mat4 uMvp;\n"
|
||
"uniform float uPointSize;\n"
|
||
"uniform float uAlpha;\n"
|
||
"uniform float uUseVertexColor;\n"
|
||
"uniform vec3 uColor;\n"
|
||
"varying vec3 vColor;\n"
|
||
"varying float vAlpha;\n"
|
||
"void main() {\n"
|
||
" gl_Position = uMvp * vec4(aPosition, 1.0);\n"
|
||
" gl_PointSize = uPointSize;\n"
|
||
" vColor = (uUseVertexColor > 0.5) ? aColor : uColor;\n"
|
||
" vAlpha = uAlpha;\n"
|
||
"}\n") ||
|
||
!program->addShaderFromSourceCode(
|
||
QOpenGLShader::Fragment,
|
||
// GLSL ES 1.00 的 fragment shader 没有默认 float 精度，
|
||
// 必须显式声明，否则 Mali 等 GLES 驱动编译失败（S0032）。
|
||
"precision highp float;\n"
|
||
"varying vec3 vColor;\n"
|
||
"varying float vAlpha;\n"
|
||
"void main() {\n"
|
||
" gl_FragColor = vec4(vColor, vAlpha);\n"
|
||
"}\n")) {
|
||
LOG_ERROR("[CloudView] Shader compile failed: %s\n",
|
||
program->log().isEmpty()
|
||
? "unknown error"
|
||
: qPrintable(program->log()));
|
||
delete program;
|
||
return false;
|
||
}
|
||
|
||
if (!program->link()) {
|
||
LOG_ERROR("[CloudView] Shader link failed: %s\n",
|
||
program->log().isEmpty()
|
||
? "unknown error"
|
||
: qPrintable(program->log()));
|
||
delete program;
|
||
return false;
|
||
}
|
||
|
||
m_shader = program;
|
||
m_positionLocation = m_shader->attributeLocation("aPosition");
|
||
m_colorLocation = m_shader->attributeLocation("aColor");
|
||
m_mvpLocation = m_shader->uniformLocation("uMvp");
|
||
m_pointSizeLocation = m_shader->uniformLocation("uPointSize");
|
||
m_alphaLocation = m_shader->uniformLocation("uAlpha");
|
||
m_useVertexColorLocation = m_shader->uniformLocation("uUseVertexColor");
|
||
m_uniformColorLocation = m_shader->uniformLocation("uColor");
|
||
}
|
||
return m_shader != nullptr;
|
||
}
|
||
|
||
QVector3D PointCloudGLWidget::colorForIndex(int colorIndex) const
|
||
{
|
||
static const QVector3D colorTable[COLOR_COUNT] = {
|
||
QVector3D(0.75f, 0.75f, 0.75f), // 浅灰
|
||
QVector3D(0.3f, 1.0f, 0.3f), // 浅绿
|
||
QVector3D(0.3f, 0.3f, 1.0f), // 浅蓝
|
||
QVector3D(1.0f, 1.0f, 0.3f), // 黄
|
||
QVector3D(0.3f, 1.0f, 1.0f), // 青
|
||
QVector3D(1.0f, 0.3f, 1.0f), // 品红
|
||
QVector3D(1.0f, 1.0f, 1.0f) // 白
|
||
};
|
||
return colorTable[colorIndex % COLOR_COUNT];
|
||
}
|
||
|
||
void PointCloudGLWidget::drawCloud(PointCloudData& data, float pointSize)
|
||
{
|
||
if (data.vertices.empty() || !m_shader) {
|
||
return;
|
||
}
|
||
m_shader->bind();
|
||
glUniformMatrix4fv(m_mvpLocation, 1, GL_FALSE, m_mvp.constData());
|
||
glUniform1f(m_pointSizeLocation, pointSize);
|
||
glUniform1f(m_alphaLocation, 1.0f);
|
||
|
||
data.vertexBuffer.bind();
|
||
glEnableVertexAttribArray(m_positionLocation);
|
||
glVertexAttribPointer(m_positionLocation, 3, GL_FLOAT, GL_FALSE, 0, nullptr);
|
||
|
||
const bool useVertexColor = (data.hasColor || data.pseudoColor) &&
|
||
data.colorBuffer.isCreated() && !data.colors.empty();
|
||
if (useVertexColor) {
|
||
data.colorBuffer.bind();
|
||
glEnableVertexAttribArray(m_colorLocation);
|
||
glVertexAttribPointer(m_colorLocation, 3, GL_FLOAT, GL_FALSE, 0, nullptr);
|
||
glUniform1f(m_useVertexColorLocation, 1.0f);
|
||
} else {
|
||
glDisableVertexAttribArray(m_colorLocation);
|
||
const QVector3D color = colorForIndex(data.colorIndex);
|
||
glUniform3f(m_uniformColorLocation, color.x(), color.y(), color.z());
|
||
glUniform1f(m_useVertexColorLocation, 0.0f);
|
||
}
|
||
|
||
glDrawArrays(GL_POINTS, 0, static_cast<GLsizei>(data.vertices.size() / 3));
|
||
|
||
glDisableVertexAttribArray(m_positionLocation);
|
||
if (useVertexColor) {
|
||
glDisableVertexAttribArray(m_colorLocation);
|
||
}
|
||
data.vertexBuffer.release();
|
||
data.colorBuffer.release();
|
||
}
|
||
|
||
void PointCloudGLWidget::drawPrimitives(const QMatrix4x4& mvp,
|
||
const float* positions,
|
||
int vertexCount,
|
||
GLenum mode,
|
||
const float* colors,
|
||
const QVector3D& uniformColor,
|
||
float pointSize,
|
||
float alpha,
|
||
bool depthTest)
|
||
{
|
||
if (!positions || vertexCount <= 0 || !m_shader) {
|
||
return;
|
||
}
|
||
m_shader->bind();
|
||
glUniformMatrix4fv(m_mvpLocation, 1, GL_FALSE, mvp.constData());
|
||
glUniform1f(m_pointSizeLocation, pointSize);
|
||
glUniform1f(m_alphaLocation, alpha);
|
||
if (!m_dynamicVbo.isCreated())
|
||
{
|
||
m_dynamicVbo.create();
|
||
}
|
||
m_dynamicVbo.setUsagePattern(QOpenGLBuffer::DynamicDraw);
|
||
if (depthTest) {
|
||
glEnable(GL_DEPTH_TEST);
|
||
} else {
|
||
glDisable(GL_DEPTH_TEST);
|
||
}
|
||
|
||
const GLsizeiptr positionBytes =
|
||
static_cast<GLsizeiptr>(vertexCount) * 3 * sizeof(float);
|
||
const GLsizeiptr colorBytes =
|
||
colors ? static_cast<GLsizeiptr>(vertexCount) * 3 * sizeof(float) : 0;
|
||
|
||
m_dynamicVbo.bind();
|
||
m_dynamicVbo.allocate(positionBytes + colorBytes);
|
||
m_dynamicVbo.write(0, positions, positionBytes);
|
||
if (colors) {
|
||
m_dynamicVbo.write(positionBytes, colors, colorBytes);
|
||
}
|
||
|
||
glEnableVertexAttribArray(m_positionLocation);
|
||
glVertexAttribPointer(m_positionLocation, 3, GL_FLOAT, GL_FALSE, 0, nullptr);
|
||
|
||
if (colors) {
|
||
glEnableVertexAttribArray(m_colorLocation);
|
||
glVertexAttribPointer(m_colorLocation, 3, GL_FLOAT, GL_FALSE, 0,
|
||
reinterpret_cast<void*>(positionBytes));
|
||
glUniform1f(m_useVertexColorLocation, 1.0f);
|
||
} else {
|
||
glDisableVertexAttribArray(m_colorLocation);
|
||
glUniform3f(m_uniformColorLocation,
|
||
uniformColor.x(), uniformColor.y(), uniformColor.z());
|
||
glUniform1f(m_useVertexColorLocation, 0.0f);
|
||
}
|
||
|
||
glDrawArrays(mode, 0, vertexCount);
|
||
|
||
m_dynamicVbo.release();
|
||
glDisableVertexAttribArray(m_positionLocation);
|
||
if (colors) {
|
||
glDisableVertexAttribArray(m_colorLocation);
|
||
}
|
||
glEnable(GL_DEPTH_TEST);
|
||
}
|
||
|
||
void PointCloudGLWidget::resetRenderState()
|
||
{
|
||
glDisableVertexAttribArray(m_positionLocation);
|
||
glDisableVertexAttribArray(m_colorLocation);
|
||
m_dynamicVbo.release();
|
||
if (m_shader) {
|
||
m_shader->release();
|
||
}
|
||
glEnable(GL_DEPTH_TEST);
|
||
}
|
||
|
||
void PointCloudGLWidget::resizeGL(int w, int h)
|
||
{
|
||
glViewport(0, 0, w, h);
|
||
|
||
m_projection.setToIdentity();
|
||
float aspect = static_cast<float>(w) / static_cast<float>(h > 0 ? h : 1);
|
||
// 放大裁剪范围以容纳激光雷达点云（最大可能跨越数十万 mm）
|
||
// near 取 1.0 保留 depth buffer 精度，far 放到 1e7 避免远点被裁掉
|
||
m_projection.perspective(45.0f, aspect, 1.0f, 1.0e7f);
|
||
}
|
||
|
||
void PointCloudGLWidget::resizeEvent(QResizeEvent* event)
|
||
{
|
||
QOpenGLWidget::resizeEvent(event);
|
||
updateControlButtonGeometry();
|
||
}
|
||
|
||
void PointCloudGLWidget::paintGL()
|
||
{
|
||
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
|
||
if (!ensureShader()) {
|
||
return;
|
||
}
|
||
|
||
m_view.setToIdentity();
|
||
// 平移在相机空间中进行，使鼠标拖动方向与屏幕方向一致
|
||
m_view.translate(-m_pan.x(), -m_pan.y(), -m_distance);
|
||
|
||
// 使用四元数旋转（物体坐标系旋转）
|
||
m_view.rotate(m_rotation);
|
||
|
||
m_view.translate(-m_center);
|
||
|
||
// 调试输出：每100帧输出一次当前旋转角度
|
||
static int frameCount = 0;
|
||
if (++frameCount % 100 == 0) {
|
||
// 从四元数转换为欧拉角用于显示
|
||
QVector3D euler = m_rotation.toEulerAngles();
|
||
LOG_INFO("[CloudView] Current rotation: pitch=%.1f, yaw=%.1f, roll=%.1f\n",
|
||
euler.x(), euler.y(), euler.z());
|
||
}
|
||
|
||
QMatrix4x4 modelView = m_view * m_model;
|
||
updateProjection(modelView);
|
||
m_mvp = m_projection * modelView;
|
||
|
||
for (size_t cloudIdx = 0; cloudIdx < m_pointClouds.size(); ++cloudIdx) {
|
||
auto& cloudData = m_pointClouds[cloudIdx];
|
||
if (cloudData.vertices.empty()) {
|
||
continue;
|
||
}
|
||
|
||
// 确保 VBO 已创建（延迟上传，因为 addPointCloud 可能在 GL 上下文外调用）
|
||
if (!cloudData.vboCreated) {
|
||
uploadToVBO(cloudData);
|
||
}
|
||
|
||
drawCloud(cloudData, m_pointSize);
|
||
|
||
// 绘制自定义大小的点（RGBA 中 A > 1 的点）
|
||
if (cloudData.hasCustomPointSizes) {
|
||
glDepthFunc(GL_LEQUAL); // 允许在相同深度覆盖绘制
|
||
const bool cloudHasColor = (cloudData.hasColor || cloudData.pseudoColor) &&
|
||
cloudData.colorBuffer.isCreated() &&
|
||
!cloudData.colors.empty();
|
||
for (const auto& group : cloudData.customPointSizeGroups) {
|
||
QVector<float> positions;
|
||
positions.reserve(static_cast<int>(group.indices.size()) * 3);
|
||
QVector<float> colors;
|
||
if (cloudHasColor) {
|
||
colors.reserve(static_cast<int>(group.indices.size()) * 3);
|
||
}
|
||
for (size_t idx : group.indices) {
|
||
const size_t vi = idx * 3;
|
||
if (vi + 2 < cloudData.vertices.size()) {
|
||
positions.append(cloudData.vertices[vi]);
|
||
positions.append(cloudData.vertices[vi + 1]);
|
||
positions.append(cloudData.vertices[vi + 2]);
|
||
if (cloudHasColor && vi + 2 < cloudData.colors.size()) {
|
||
colors.append(cloudData.colors[vi]);
|
||
colors.append(cloudData.colors[vi + 1]);
|
||
colors.append(cloudData.colors[vi + 2]);
|
||
}
|
||
}
|
||
}
|
||
if (!positions.isEmpty()) {
|
||
drawPrimitives(m_mvp,
|
||
positions.constData(),
|
||
positions.size() / 3,
|
||
GL_POINTS,
|
||
cloudHasColor ? colors.constData() : nullptr,
|
||
colorForIndex(cloudData.colorIndex),
|
||
group.pointSize,
|
||
1.0f,
|
||
true);
|
||
}
|
||
}
|
||
glDepthFunc(GL_LESS); // 恢复默认深度测试
|
||
}
|
||
}
|
||
|
||
drawSelectedPoints();
|
||
drawMeasurementLine();
|
||
drawSelectedLine();
|
||
drawLineSegments();
|
||
drawBasicShapeSurfaces();
|
||
drawBasicShapeSegments();
|
||
drawBasicShapePoints();
|
||
drawPosePoints();
|
||
|
||
// 最后绘制坐标系指示器（覆盖在所有内容之上）
|
||
drawAxis();
|
||
|
||
// 清理 GL 状态，避免影响 QPainter 标注
|
||
resetRenderState();
|
||
|
||
// 使用 QPainter 绘制坐标轴 XYZ 标注
|
||
drawAxisLabels();
|
||
}
|
||
|
||
void PointCloudGLWidget::addPointCloud(const PointCloudXYZ& cloud, const QString& name, bool resetViewAfterAdd)
|
||
{
|
||
LOG_INFO("[CloudView] addPointCloud called, cloud size: %zu\n", cloud.size());
|
||
|
||
if (cloud.empty()) {
|
||
LOG_WARN("[CloudView] Cloud is empty, returning\n");
|
||
return;
|
||
}
|
||
|
||
PointCloudData data;
|
||
data.name = name;
|
||
data.hasColor = false;
|
||
data.hasLineInfo = !cloud.lineIndices.empty();
|
||
data.colorIndex = m_colorIndex; // 分配颜色索引
|
||
m_colorIndex = (m_colorIndex + 1) % COLOR_COUNT; // 轮换到下一个颜色
|
||
data.vertices.reserve(cloud.size() * 3);
|
||
data.totalLines = 0;
|
||
data.pointsPerLine = 0;
|
||
|
||
const float EPSILON = 1e-6f;
|
||
int prevLineIdx = -1;
|
||
int ptInLineCounter = 0;
|
||
int maxPointCount = 0;
|
||
for (size_t i = 0; i < cloud.points.size(); ++i) {
|
||
// 跟踪线内索引（对所有点递增，包括被过滤的零点）
|
||
int lineIdx = -1;
|
||
if (data.hasLineInfo && i < cloud.lineIndices.size()) {
|
||
lineIdx = cloud.lineIndices[i];
|
||
}
|
||
if (lineIdx != prevLineIdx) {
|
||
ptInLineCounter = 0;
|
||
prevLineIdx = lineIdx;
|
||
}
|
||
|
||
int pointInLineIndex = (i < cloud.pointInLineIndices.size())
|
||
? cloud.pointInLineIndices[i] : -1;
|
||
if (pointInLineIndex < 0) {
|
||
pointInLineIndex = ptInLineCounter;
|
||
}
|
||
maxPointCount = qMax(maxPointCount, pointInLineIndex + 1);
|
||
|
||
const auto& pt = cloud.points[i];
|
||
if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
|
||
ptInLineCounter++;
|
||
continue;
|
||
}
|
||
// 显示时过滤 (0,0,0) 点
|
||
if (std::fabs(pt.x) < EPSILON && std::fabs(pt.y) < EPSILON && std::fabs(pt.z) < EPSILON) {
|
||
ptInLineCounter++;
|
||
continue;
|
||
}
|
||
data.vertices.push_back(pt.x);
|
||
data.vertices.push_back(pt.y);
|
||
data.vertices.push_back(pt.z);
|
||
|
||
// 保存原始索引
|
||
data.originalIndices.push_back(static_cast<int>(i));
|
||
|
||
// 保存线内索引（点在所属线中的位置）
|
||
data.pointInLineIndices.push_back(pointInLineIndex);
|
||
|
||
// 线索引数组必须和顶点数组保持一一对应；未知线号用 -1 占位。
|
||
if (data.hasLineInfo) {
|
||
data.lineIndices.push_back(lineIdx);
|
||
if (lineIdx >= 0 && lineIdx + 1 > data.totalLines) {
|
||
data.totalLines = lineIdx + 1;
|
||
}
|
||
}
|
||
|
||
ptInLineCounter++;
|
||
}
|
||
|
||
// 计算每线点数（假设网格化点云）
|
||
if (data.totalLines > 0) {
|
||
data.pointsPerLine = maxPointCount;
|
||
}
|
||
|
||
// 新建纯 XYZ 点云时直接生成颜色，避免随后再次上传同一份 VBO。
|
||
rebuildPseudoColors(data, 0);
|
||
|
||
LOG_INFO("[CloudView] Valid vertices count: %zu, colorIndex: %d, totalLines: %d, pointsPerLine: %d\n",
|
||
data.vertices.size() / 3, data.colorIndex, data.totalLines, data.pointsPerLine);
|
||
|
||
m_pointClouds.push_back(std::move(data));
|
||
|
||
// 上传 VBO（如果当前在 GL 上下文中）
|
||
makeCurrent();
|
||
uploadToVBO(m_pointClouds.back());
|
||
doneCurrent();
|
||
|
||
computeBoundingBox();
|
||
LOG_INFO("[CloudView] BoundingBox min: (%.3f, %.3f, %.3f) max: (%.3f, %.3f, %.3f)\n",
|
||
m_minBound.x(), m_minBound.y(), m_minBound.z(),
|
||
m_maxBound.x(), m_maxBound.y(), m_maxBound.z());
|
||
LOG_INFO("[CloudView] Center: (%.3f, %.3f, %.3f) Distance: %.3f\n",
|
||
m_center.x(), m_center.y(), m_center.z(), m_distance);
|
||
|
||
if (resetViewAfterAdd) {
|
||
resetView();
|
||
}
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::setControlButtonsVisible(bool visible)
|
||
{
|
||
m_showControlButtons = visible;
|
||
if (m_controlPanel) {
|
||
m_controlPanel->setVisible(visible);
|
||
}
|
||
if (m_viewToolbar) {
|
||
m_viewToolbar->setVisible(visible);
|
||
}
|
||
if (m_viewAngleButton) {
|
||
m_viewAngleButton->setVisible(visible);
|
||
}
|
||
if (m_zoomInButton) {
|
||
m_zoomInButton->setVisible(visible);
|
||
}
|
||
if (m_zoomOutButton) {
|
||
m_zoomOutButton->setVisible(visible);
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::createControlButtons()
|
||
{
|
||
m_controlPanel = new QWidget(this);
|
||
m_controlPanel->setObjectName("cloudView3DControlPanel");
|
||
m_controlPanel->setStyleSheet(
|
||
"QWidget#cloudView3DControlPanel {"
|
||
" background-color: rgba(40, 40, 40, 185);"
|
||
" border: 1px solid rgba(130, 130, 130, 150);"
|
||
" border-radius: 4px;"
|
||
"}"
|
||
"QToolButton {"
|
||
" background-color: rgba(70, 70, 70, 210);"
|
||
" color: white;"
|
||
" border: 1px solid rgba(150, 150, 150, 120);"
|
||
" border-radius: 3px;"
|
||
" font-size: 10px;"
|
||
"}"
|
||
"QToolButton:hover { background-color: rgba(100, 100, 100, 230); }"
|
||
"QToolButton:pressed { background-color: rgba(25, 25, 25, 240); }");
|
||
|
||
QVBoxLayout* panelLayout = new QVBoxLayout(m_controlPanel);
|
||
panelLayout->setContentsMargins(4, 4, 4, 4);
|
||
panelLayout->setSpacing(3);
|
||
|
||
auto makeRow = [this, panelLayout]() {
|
||
QHBoxLayout* row = new QHBoxLayout();
|
||
row->setContentsMargins(0, 0, 0, 0);
|
||
row->setSpacing(2);
|
||
panelLayout->addLayout(row);
|
||
return row;
|
||
};
|
||
|
||
auto addButton = [this](QHBoxLayout* row, const QString& text, const QString& tooltip) {
|
||
QToolButton* button = new QToolButton(m_controlPanel);
|
||
button->setText(text);
|
||
button->setToolTip(tooltip);
|
||
button->setFixedSize(26, 24);
|
||
button->setAutoRaise(false);
|
||
row->addWidget(button);
|
||
return button;
|
||
};
|
||
|
||
struct ViewPreset {
|
||
const char* text;
|
||
const char* tooltip;
|
||
float rotX;
|
||
float rotY;
|
||
float rotZ;
|
||
};
|
||
const ViewPreset presets[] = {
|
||
{"F", "正视", 180.0f, 0.0f, 0.0f},
|
||
{"B", "后视", 180.0f, 180.0f, 0.0f},
|
||
{"L", "左视", 180.0f, 90.0f, 0.0f},
|
||
{"R", "右视", 180.0f, -90.0f, 0.0f},
|
||
{"T", "俯视", 90.0f, 0.0f, 0.0f},
|
||
{"D", "仰视", -90.0f, 0.0f, 0.0f},
|
||
{"A", "机械臂视角", -90.0f, 90.0f, 0.0f}
|
||
};
|
||
|
||
QHBoxLayout* viewRow = makeRow();
|
||
for (const ViewPreset& preset : presets) {
|
||
QToolButton* button = addButton(viewRow, preset.text, preset.tooltip);
|
||
connect(button, &QToolButton::clicked, this, [this, preset]() {
|
||
setViewAngles(preset.rotX, preset.rotY, preset.rotZ);
|
||
});
|
||
}
|
||
|
||
QHBoxLayout* rotateRow = makeRow();
|
||
auto addRotationButton = [this, &addButton, rotateRow](const QString& text,
|
||
const QString& tooltip,
|
||
float deltaX,
|
||
float deltaY,
|
||
float deltaZ) {
|
||
QToolButton* button = addButton(rotateRow, text, tooltip);
|
||
connect(button, &QToolButton::clicked, this, [this, deltaX, deltaY, deltaZ]() {
|
||
setViewAngles(m_rotationX + deltaX,
|
||
m_rotationY + deltaY,
|
||
m_rotationZ + deltaZ);
|
||
});
|
||
};
|
||
addRotationButton("X-", "绕 X 轴反向旋转 5 度", -5.0f, 0.0f, 0.0f);
|
||
addRotationButton("X+", "绕 X 轴正向旋转 5 度", 5.0f, 0.0f, 0.0f);
|
||
addRotationButton("Y-", "绕 Y 轴反向旋转 5 度", 0.0f, -5.0f, 0.0f);
|
||
addRotationButton("Y+", "绕 Y 轴正向旋转 5 度", 0.0f, 5.0f, 0.0f);
|
||
addRotationButton("Z-", "绕 Z 轴反向旋转 5 度", 0.0f, 0.0f, -5.0f);
|
||
addRotationButton("Z+", "绕 Z 轴正向旋转 5 度", 0.0f, 0.0f, 5.0f);
|
||
|
||
QToolButton* resetButton = addButton(rotateRow, "0", "重置视图");
|
||
connect(resetButton, &QToolButton::clicked, this, &PointCloudGLWidget::resetView);
|
||
|
||
QHBoxLayout* zoomRow = makeRow();
|
||
m_zoomInButton = addButton(zoomRow, "+", "放大");
|
||
m_zoomOutButton = addButton(zoomRow, "-", "缩小");
|
||
m_zoomInButton->setAutoRepeat(true);
|
||
m_zoomOutButton->setAutoRepeat(true);
|
||
connect(m_zoomInButton, &QToolButton::clicked, this, &PointCloudGLWidget::zoomIn);
|
||
connect(m_zoomOutButton, &QToolButton::clicked, this, &PointCloudGLWidget::zoomOut);
|
||
zoomRow->addStretch();
|
||
|
||
m_controlPanel->adjustSize();
|
||
m_controlPanel->setVisible(m_showControlButtons);
|
||
updateControlButtonGeometry();
|
||
}
|
||
|
||
void PointCloudGLWidget::updateControlButtonGeometry()
|
||
{
|
||
if (!m_controlPanel) {
|
||
return;
|
||
}
|
||
m_controlPanel->move(8, 8);
|
||
m_controlPanel->raise();
|
||
}
|
||
|
||
void PointCloudGLWidget::addPointCloud(const PointCloudXYZRGB& cloud, const QString& name, bool resetViewAfterAdd)
|
||
{
|
||
if (cloud.empty()) {
|
||
return;
|
||
}
|
||
|
||
PointCloudData data;
|
||
data.name = name;
|
||
data.hasColor = true;
|
||
data.hasLineInfo = !cloud.lineIndices.empty();
|
||
data.colorIndex = m_colorIndex; // 即使有颜色也分配索引（备用）
|
||
m_colorIndex = (m_colorIndex + 1) % COLOR_COUNT;
|
||
data.vertices.reserve(cloud.size() * 3);
|
||
data.colors.reserve(cloud.size() * 3);
|
||
data.totalLines = 0;
|
||
data.pointsPerLine = 0;
|
||
data.hasCustomPointSizes = false;
|
||
|
||
// 用于按点大小分组的临时 map
|
||
std::map<float, std::vector<size_t>> sizeGroupMap;
|
||
|
||
const float EPSILON = 1e-6f;
|
||
int prevLineIdx = -1;
|
||
int ptInLineCounter = 0;
|
||
int maxPointCount = 0;
|
||
|
||
for (size_t i = 0; i < cloud.points.size(); ++i) {
|
||
const auto& pt = cloud.points[i];
|
||
|
||
// 跟踪线内索引（对所有点递增，包括被过滤的零点）
|
||
int lineIdx = -1;
|
||
if (data.hasLineInfo && i < cloud.lineIndices.size()) {
|
||
lineIdx = cloud.lineIndices[i];
|
||
}
|
||
if (lineIdx != prevLineIdx) {
|
||
ptInLineCounter = 0;
|
||
prevLineIdx = lineIdx;
|
||
}
|
||
|
||
int pointInLineIndex = (i < cloud.pointInLineIndices.size())
|
||
? cloud.pointInLineIndices[i] : -1;
|
||
if (pointInLineIndex < 0) {
|
||
pointInLineIndex = ptInLineCounter;
|
||
}
|
||
maxPointCount = qMax(maxPointCount, pointInLineIndex + 1);
|
||
|
||
if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
|
||
ptInLineCounter++;
|
||
continue;
|
||
}
|
||
|
||
// 显示时过滤 (0,0,0) 点
|
||
if (std::fabs(pt.x) < EPSILON && std::fabs(pt.y) < EPSILON && std::fabs(pt.z) < EPSILON) {
|
||
ptInLineCounter++;
|
||
continue;
|
||
}
|
||
|
||
size_t pointIndex = data.vertices.size() / 3;
|
||
|
||
data.vertices.push_back(pt.x);
|
||
data.vertices.push_back(pt.y);
|
||
data.vertices.push_back(pt.z);
|
||
|
||
data.colors.push_back(pt.r / 255.0f);
|
||
data.colors.push_back(pt.g / 255.0f);
|
||
data.colors.push_back(pt.b / 255.0f);
|
||
|
||
// 保存原始索引
|
||
data.originalIndices.push_back(static_cast<int>(i));
|
||
|
||
// 保存线内索引（点在所属线中的位置）
|
||
data.pointInLineIndices.push_back(pointInLineIndex);
|
||
|
||
// 线索引数组必须和顶点数组保持一一对应；未知线号用 -1 占位。
|
||
if (data.hasLineInfo) {
|
||
data.lineIndices.push_back(lineIdx);
|
||
if (lineIdx >= 0 && lineIdx + 1 > data.totalLines) {
|
||
data.totalLines = lineIdx + 1;
|
||
}
|
||
}
|
||
|
||
// 收集自定义点大小（A > 1）
|
||
if (pt.pointSize > 1.0f) {
|
||
sizeGroupMap[pt.pointSize].push_back(pointIndex);
|
||
}
|
||
|
||
ptInLineCounter++;
|
||
}
|
||
|
||
// 构建自定义点大小分组
|
||
if (!sizeGroupMap.empty()) {
|
||
data.hasCustomPointSizes = true;
|
||
for (auto& pair : sizeGroupMap) {
|
||
PointCloudData::PointSizeGroup group;
|
||
group.pointSize = pair.first;
|
||
group.indices = std::move(pair.second);
|
||
data.customPointSizeGroups.push_back(std::move(group));
|
||
}
|
||
LOG_INFO("[CloudView] Found %zu custom point size groups\n", data.customPointSizeGroups.size());
|
||
}
|
||
|
||
// 计算每线点数
|
||
if (data.totalLines > 0) {
|
||
data.pointsPerLine = maxPointCount;
|
||
}
|
||
|
||
m_pointClouds.push_back(std::move(data));
|
||
|
||
// 上传 VBO
|
||
makeCurrent();
|
||
uploadToVBO(m_pointClouds.back());
|
||
doneCurrent();
|
||
|
||
computeBoundingBox();
|
||
if (resetViewAfterAdd) {
|
||
resetView();
|
||
}
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::appendPointCloud(const PointCloudXYZRGB& cloud,
|
||
const QString& name)
|
||
{
|
||
if (cloud.empty()) {
|
||
return;
|
||
}
|
||
if (m_pointClouds.empty() || !m_pointClouds.back().hasColor) {
|
||
addPointCloud(cloud, name);
|
||
return;
|
||
}
|
||
|
||
PointCloudData& data = m_pointClouds.back();
|
||
if (!name.isEmpty()) {
|
||
data.name = name;
|
||
}
|
||
|
||
const bool incomingHasLineInfo = !cloud.lineIndices.empty();
|
||
if (incomingHasLineInfo && !data.hasLineInfo) {
|
||
// 后续数据首次带线信息时，为已有顶点补齐未知线号，避免索引数组错位。
|
||
data.lineIndices.resize(data.vertices.size() / 3, -1);
|
||
data.hasLineInfo = true;
|
||
} else {
|
||
data.hasLineInfo = data.hasLineInfo || incomingHasLineInfo;
|
||
}
|
||
const size_t originalIndexOffset = data.vertices.size() / 3;
|
||
const float epsilon = 1e-6f;
|
||
int previousLineIndex = -1;
|
||
int pointInLineIndex = 0;
|
||
int maxPointCount = data.pointsPerLine;
|
||
|
||
for (size_t index = 0; index < cloud.points.size(); ++index) {
|
||
const Point3DRGB& point = cloud.points[index];
|
||
int lineIndex = -1;
|
||
if (incomingHasLineInfo && index < cloud.lineIndices.size()) {
|
||
lineIndex = cloud.lineIndices[index];
|
||
}
|
||
if (lineIndex != previousLineIndex) {
|
||
pointInLineIndex = 0;
|
||
previousLineIndex = lineIndex;
|
||
}
|
||
|
||
int sourcePointIndex = (index < cloud.pointInLineIndices.size())
|
||
? cloud.pointInLineIndices[index] : -1;
|
||
if (sourcePointIndex < 0) {
|
||
sourcePointIndex = pointInLineIndex;
|
||
}
|
||
maxPointCount = qMax(maxPointCount, sourcePointIndex + 1);
|
||
|
||
if (!std::isfinite(point.x) || !std::isfinite(point.y) ||
|
||
!std::isfinite(point.z) ||
|
||
(std::fabs(point.x) < epsilon &&
|
||
std::fabs(point.y) < epsilon &&
|
||
std::fabs(point.z) < epsilon)) {
|
||
++pointInLineIndex;
|
||
continue;
|
||
}
|
||
|
||
const size_t pointIndex = data.vertices.size() / 3;
|
||
data.vertices.push_back(point.x);
|
||
data.vertices.push_back(point.y);
|
||
data.vertices.push_back(point.z);
|
||
data.colors.push_back(point.r / 255.0f);
|
||
data.colors.push_back(point.g / 255.0f);
|
||
data.colors.push_back(point.b / 255.0f);
|
||
data.originalIndices.push_back(
|
||
static_cast<int>(originalIndexOffset + index));
|
||
data.pointInLineIndices.push_back(sourcePointIndex);
|
||
|
||
if (data.hasLineInfo) {
|
||
data.lineIndices.push_back(lineIndex);
|
||
if (lineIndex >= 0) {
|
||
data.totalLines = qMax(data.totalLines, lineIndex + 1);
|
||
}
|
||
}
|
||
|
||
if (point.pointSize > 1.0f) {
|
||
auto sizeGroup = std::find_if(
|
||
data.customPointSizeGroups.begin(),
|
||
data.customPointSizeGroups.end(),
|
||
[&point](const PointCloudData::PointSizeGroup& group) {
|
||
return group.pointSize == point.pointSize;
|
||
});
|
||
if (sizeGroup == data.customPointSizeGroups.end()) {
|
||
PointCloudData::PointSizeGroup group;
|
||
group.pointSize = point.pointSize;
|
||
data.customPointSizeGroups.push_back(std::move(group));
|
||
sizeGroup = data.customPointSizeGroups.end() - 1;
|
||
}
|
||
sizeGroup->indices.push_back(pointIndex);
|
||
data.hasCustomPointSizes = true;
|
||
}
|
||
++pointInLineIndex;
|
||
}
|
||
|
||
if (data.totalLines > 0) {
|
||
data.pointsPerLine = maxPointCount;
|
||
}
|
||
|
||
makeCurrent();
|
||
uploadToVBO(data);
|
||
doneCurrent();
|
||
|
||
computeBoundingBox();
|
||
m_distance = qMax(m_distance, fittedCameraDistance());
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::appendPointCloud(const PointCloudXYZ& cloud,
|
||
const QString& name)
|
||
{
|
||
if (cloud.empty()) {
|
||
return;
|
||
}
|
||
if (m_pointClouds.empty() || m_pointClouds.back().hasColor) {
|
||
addPointCloud(cloud, name, false);
|
||
return;
|
||
}
|
||
|
||
PointCloudData& data = m_pointClouds.back();
|
||
if (!name.isEmpty()) {
|
||
data.name = name;
|
||
}
|
||
|
||
const bool incomingHasLineInfo = !cloud.lineIndices.empty();
|
||
if (incomingHasLineInfo && !data.hasLineInfo) {
|
||
// 后续数据首次带线信息时，为已有顶点补齐未知线号，避免索引数组错位。
|
||
data.lineIndices.resize(data.vertices.size() / 3, -1);
|
||
data.hasLineInfo = true;
|
||
} else {
|
||
data.hasLineInfo = data.hasLineInfo || incomingHasLineInfo;
|
||
}
|
||
const size_t originalIndexOffset = data.vertices.size() / 3;
|
||
const size_t firstNewVertex = originalIndexOffset;
|
||
const float epsilon = 1e-6f;
|
||
int previousLineIndex = -1;
|
||
int pointInLineIndex = 0;
|
||
int maxPointCount = data.pointsPerLine;
|
||
|
||
for (size_t index = 0; index < cloud.points.size(); ++index) {
|
||
const Point3D& point = cloud.points[index];
|
||
int lineIndex = -1;
|
||
if (incomingHasLineInfo && index < cloud.lineIndices.size()) {
|
||
lineIndex = cloud.lineIndices[index];
|
||
}
|
||
if (lineIndex != previousLineIndex) {
|
||
pointInLineIndex = 0;
|
||
previousLineIndex = lineIndex;
|
||
}
|
||
|
||
int sourcePointIndex = (index < cloud.pointInLineIndices.size())
|
||
? cloud.pointInLineIndices[index] : -1;
|
||
if (sourcePointIndex < 0) {
|
||
sourcePointIndex = pointInLineIndex;
|
||
}
|
||
maxPointCount = qMax(maxPointCount, sourcePointIndex + 1);
|
||
|
||
if (!std::isfinite(point.x) || !std::isfinite(point.y) ||
|
||
!std::isfinite(point.z) ||
|
||
(std::fabs(point.x) < epsilon &&
|
||
std::fabs(point.y) < epsilon &&
|
||
std::fabs(point.z) < epsilon)) {
|
||
++pointInLineIndex;
|
||
continue;
|
||
}
|
||
|
||
data.vertices.push_back(point.x);
|
||
data.vertices.push_back(point.y);
|
||
data.vertices.push_back(point.z);
|
||
data.originalIndices.push_back(static_cast<int>(originalIndexOffset + index));
|
||
data.pointInLineIndices.push_back(sourcePointIndex);
|
||
if (data.hasLineInfo) {
|
||
data.lineIndices.push_back(lineIndex);
|
||
if (lineIndex >= 0) {
|
||
data.totalLines = qMax(data.totalLines, lineIndex + 1);
|
||
}
|
||
}
|
||
++pointInLineIndex;
|
||
}
|
||
|
||
if (data.totalLines > 0) {
|
||
// 过滤后各扫描线可能不等长，使用原始线内索引的最大值保留网格位置。
|
||
data.pointsPerLine = maxPointCount;
|
||
}
|
||
|
||
// 流式追加时只为新增顶点生成伪彩，避免重复扫描历史点云。
|
||
rebuildPseudoColors(data, firstNewVertex);
|
||
|
||
makeCurrent();
|
||
uploadToVBO(data);
|
||
doneCurrent();
|
||
computeBoundingBox();
|
||
m_distance = qMax(m_distance, fittedCameraDistance());
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::setNewestPointCloudLineLayout(int totalLines, int pointsPerLine)
|
||
{
|
||
if (m_pointClouds.empty())
|
||
{
|
||
return;
|
||
}
|
||
|
||
PointCloudData& data = m_pointClouds.back();
|
||
if (totalLines > 0)
|
||
{
|
||
data.hasLineInfo = true;
|
||
data.totalLines = qMax(data.totalLines, totalLines);
|
||
}
|
||
if (pointsPerLine > 0)
|
||
{
|
||
data.pointsPerLine = qMax(data.pointsPerLine, pointsPerLine);
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::clearPointClouds()
|
||
{
|
||
// 释放所有 VBO
|
||
makeCurrent();
|
||
for (auto& cloudData : m_pointClouds) {
|
||
releaseVBO(cloudData);
|
||
}
|
||
doneCurrent();
|
||
|
||
m_pointClouds.clear();
|
||
m_selectedPoints.clear();
|
||
m_selectedLine = SelectedLineInfo();
|
||
m_lineSegments.clear();
|
||
m_basicShapeSegments.clear();
|
||
m_basicShapeSurfaces.clear();
|
||
m_basicShapePoints.clear();
|
||
m_posePoints.clear();
|
||
m_minBound = QVector3D(-50, -50, -50);
|
||
m_maxBound = QVector3D(50, 50, 50);
|
||
m_center = QVector3D(0, 0, 0);
|
||
m_colorIndex = 0; // 重置颜色索引
|
||
resetView();
|
||
update();
|
||
}
|
||
|
||
bool PointCloudGLWidget::removeOldestPointCloud()
|
||
{
|
||
if (m_pointClouds.empty()) {
|
||
return false;
|
||
}
|
||
|
||
makeCurrent();
|
||
releaseVBO(m_pointClouds.front());
|
||
doneCurrent();
|
||
m_pointClouds.pop_front();
|
||
|
||
// Cloud indices change after removing the first item, so stale selection
|
||
// information must not be kept.
|
||
m_selectedPoints.clear();
|
||
m_selectedLine = SelectedLineInfo();
|
||
computeBoundingBox();
|
||
updatePseudoColors();
|
||
update();
|
||
return true;
|
||
}
|
||
|
||
bool PointCloudGLWidget::removeNewestPointCloud()
|
||
{
|
||
if (m_pointClouds.empty()) {
|
||
return false;
|
||
}
|
||
|
||
makeCurrent();
|
||
releaseVBO(m_pointClouds.back());
|
||
doneCurrent();
|
||
m_pointClouds.pop_back();
|
||
m_selectedPoints.clear();
|
||
m_selectedLine = SelectedLineInfo();
|
||
computeBoundingBox();
|
||
updatePseudoColors();
|
||
update();
|
||
return true;
|
||
}
|
||
|
||
void PointCloudGLWidget::setPointCloudColor(PointCloudColor color)
|
||
{
|
||
m_currentColor = color;
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::setPointSize(float size)
|
||
{
|
||
m_pointSize = qBound(1.0f, size, 10.0f);
|
||
update();
|
||
}
|
||
|
||
QVector3D PointCloudGLWidget::pseudoColorForZ(float z) const
|
||
{
|
||
if (!std::isfinite(z))
|
||
{
|
||
return QVector3D();
|
||
}
|
||
|
||
if (!std::isfinite(m_pseudoColorPeriod) || m_pseudoColorPeriod <= 1e-6f)
|
||
{
|
||
return m_pseudoColorDefaultPalette
|
||
? defaultHuePseudoColor(0.0f)
|
||
: m_pseudoColorStartColor;
|
||
}
|
||
|
||
const double period = static_cast<double>(m_pseudoColorPeriod);
|
||
const double cycleValue = std::floor(static_cast<double>(z) / period);
|
||
double phase = std::fmod(static_cast<double>(z), period);
|
||
if (phase < 0.0)
|
||
{
|
||
phase += period;
|
||
}
|
||
|
||
float t = static_cast<float>(phase / period);
|
||
if (m_pseudoColorDefaultPalette)
|
||
{
|
||
// 默认方案：每个周期走完整色相条，首尾同为红色，周期边界自然衔接。
|
||
return defaultHuePseudoColor(t);
|
||
}
|
||
|
||
// 自定义起止色：固定周期内连续梯度，奇数周期反向避免边界突变。
|
||
if (std::fmod(std::fabs(cycleValue), 2.0) >= 1.0)
|
||
{
|
||
t = 1.0f - t;
|
||
}
|
||
return m_pseudoColorStartColor * (1.0f - t) + m_pseudoColorEndColor * t;
|
||
}
|
||
|
||
void PointCloudGLWidget::rebuildPseudoColors(PointCloudData& data, size_t firstVertex)
|
||
{
|
||
if (data.hasColor)
|
||
{
|
||
return;
|
||
}
|
||
|
||
const size_t vertexCount = data.vertices.size() / 3;
|
||
if (!m_pseudoColorEnabled)
|
||
{
|
||
data.colors.clear();
|
||
data.pseudoColor = false;
|
||
return;
|
||
}
|
||
|
||
if (firstVertex > vertexCount)
|
||
{
|
||
firstVertex = 0;
|
||
}
|
||
|
||
data.colors.resize(vertexCount * 3);
|
||
for (size_t vertexIndex = firstVertex; vertexIndex < vertexCount; ++vertexIndex)
|
||
{
|
||
const size_t offset = vertexIndex * 3;
|
||
const QVector3D color = pseudoColorForZ(data.vertices[offset + 2]);
|
||
data.colors[offset] = color.x();
|
||
data.colors[offset + 1] = color.y();
|
||
data.colors[offset + 2] = color.z();
|
||
}
|
||
data.pseudoColor = true;
|
||
}
|
||
|
||
void PointCloudGLWidget::updatePseudoColors()
|
||
{
|
||
bool hasUncoloredCloud = false;
|
||
for (auto& data : m_pointClouds)
|
||
{
|
||
if (!data.hasColor)
|
||
{
|
||
hasUncoloredCloud = true;
|
||
rebuildPseudoColors(data, 0);
|
||
}
|
||
}
|
||
|
||
if (!hasUncoloredCloud)
|
||
{
|
||
return;
|
||
}
|
||
|
||
makeCurrent();
|
||
for (auto& data : m_pointClouds)
|
||
{
|
||
if (!data.hasColor)
|
||
{
|
||
uploadToVBO(data);
|
||
}
|
||
}
|
||
doneCurrent();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::setPseudoColorEnabled(bool enabled)
|
||
{
|
||
setPseudoColorSettings(enabled,
|
||
m_pseudoColorPeriod,
|
||
m_pseudoColorStartColor,
|
||
m_pseudoColorEndColor,
|
||
m_pseudoColorDefaultPalette);
|
||
}
|
||
|
||
void PointCloudGLWidget::setPseudoColorSettings(bool enabled, float period, const QVector3D& startColor, const QVector3D& endColor, bool defaultPalette)
|
||
{
|
||
const float safePeriod = std::isfinite(period) && period > 0.0f
|
||
? qMax(1.0f, period)
|
||
: defaultPseudoColorPeriod();
|
||
const QVector3D clampedStart = clampPseudoColor(startColor);
|
||
const QVector3D clampedEnd = clampPseudoColor(endColor);
|
||
const bool changed = m_pseudoColorEnabled != enabled ||
|
||
!qFuzzyCompare(m_pseudoColorPeriod, safePeriod) ||
|
||
m_pseudoColorStartColor != clampedStart ||
|
||
m_pseudoColorEndColor != clampedEnd ||
|
||
m_pseudoColorDefaultPalette != defaultPalette;
|
||
if (!changed)
|
||
{
|
||
return;
|
||
}
|
||
|
||
m_pseudoColorEnabled = enabled;
|
||
m_pseudoColorPeriod = safePeriod;
|
||
m_pseudoColorStartColor = clampedStart;
|
||
m_pseudoColorEndColor = clampedEnd;
|
||
m_pseudoColorDefaultPalette = defaultPalette;
|
||
updatePseudoColors();
|
||
}
|
||
|
||
void PointCloudGLWidget::setPseudoColorPeriod(float period)
|
||
{
|
||
setPseudoColorSettings(m_pseudoColorEnabled,
|
||
period,
|
||
m_pseudoColorStartColor,
|
||
m_pseudoColorEndColor,
|
||
m_pseudoColorDefaultPalette);
|
||
}
|
||
|
||
void PointCloudGLWidget::setPseudoColorColorRange(const QVector3D& startColor,
|
||
const QVector3D& endColor)
|
||
{
|
||
// 直接指定起止色表示切换到自定义梯度；默认蓝→红可通过重置恢复。
|
||
setPseudoColorSettings(m_pseudoColorEnabled,
|
||
m_pseudoColorPeriod,
|
||
startColor,
|
||
endColor,
|
||
false);
|
||
}
|
||
|
||
void PointCloudGLWidget::setPseudoColorDefaultPalette(bool enabled)
|
||
{
|
||
setPseudoColorSettings(m_pseudoColorEnabled,
|
||
m_pseudoColorPeriod,
|
||
m_pseudoColorStartColor,
|
||
m_pseudoColorEndColor,
|
||
enabled);
|
||
}
|
||
|
||
void PointCloudGLWidget::resetPseudoColorDefaults()
|
||
{
|
||
setPseudoColorSettings(true,
|
||
defaultPseudoColorPeriod(),
|
||
defaultPseudoColorStartColor(),
|
||
defaultPseudoColorEndColor(),
|
||
true);
|
||
}
|
||
|
||
float PointCloudGLWidget::defaultPseudoColorPeriod()
|
||
{
|
||
return kDefaultPseudoColorPeriod;
|
||
}
|
||
|
||
QVector3D PointCloudGLWidget::defaultPseudoColorStartColor()
|
||
{
|
||
return QVector3D(0.0f, 0.0f, 1.0f);
|
||
}
|
||
|
||
QVector3D PointCloudGLWidget::defaultPseudoColorEndColor()
|
||
{
|
||
return QVector3D(1.0f, 0.0f, 0.0f);
|
||
}
|
||
|
||
void PointCloudGLWidget::resetView()
|
||
{
|
||
QVector3D size = m_maxBound - m_minBound;
|
||
float maxSize = qMax(qMax(size.x(), size.y()), size.z());
|
||
|
||
// 默认视角：面向 XY 平面，X 向右，Y 向下
|
||
// 绕 X 轴旋转 180°：Y 轴从向上翻转为向下
|
||
m_rotationX = 180.0f;
|
||
m_rotationY = 0.0f;
|
||
m_rotationZ = 0.0f;
|
||
m_rotation = QQuaternion::fromEulerAngles(m_rotationX, m_rotationY, m_rotationZ);
|
||
m_pan = QVector3D(0, 0, 0);
|
||
m_distance = fittedCameraDistance();
|
||
|
||
LOG_INFO("[CloudView] resetView: size=(%.3f, %.3f, %.3f) maxSize=%.3f distance=%.3f\n",
|
||
size.x(), size.y(), size.z(), maxSize, m_distance);
|
||
|
||
emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ);
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::fitToContent()
|
||
{
|
||
computeBoundingBox();
|
||
updatePseudoColors();
|
||
resetView();
|
||
}
|
||
|
||
void PointCloudGLWidget::updateProjection(const QMatrix4x4& modelView)
|
||
{
|
||
const QVector3D corners[] = {
|
||
{m_minBound.x(), m_minBound.y(), m_minBound.z()},
|
||
{m_minBound.x(), m_minBound.y(), m_maxBound.z()},
|
||
{m_minBound.x(), m_maxBound.y(), m_minBound.z()},
|
||
{m_minBound.x(), m_maxBound.y(), m_maxBound.z()},
|
||
{m_maxBound.x(), m_minBound.y(), m_minBound.z()},
|
||
{m_maxBound.x(), m_minBound.y(), m_maxBound.z()},
|
||
{m_maxBound.x(), m_maxBound.y(), m_minBound.z()},
|
||
{m_maxBound.x(), m_maxBound.y(), m_maxBound.z()}
|
||
};
|
||
|
||
float minimumDepth = FLT_MAX;
|
||
float maximumDepth = -FLT_MAX;
|
||
for (const QVector3D& corner : corners) {
|
||
const float depth = -modelView.map(corner).z();
|
||
minimumDepth = qMin(minimumDepth, depth);
|
||
maximumDepth = qMax(maximumDepth, depth);
|
||
}
|
||
|
||
constexpr float kMinimumNearPlane = 0.01f;
|
||
float nearPlane = kMinimumNearPlane;
|
||
float farPlane = 1.0f;
|
||
if (maximumDepth > kMinimumNearPlane) {
|
||
// 点云贴近或跨过相机时保持很小的近裁剪距离，避免前侧点被裁掉。
|
||
if (minimumDepth > kMinimumNearPlane) {
|
||
nearPlane = qMax(kMinimumNearPlane, minimumDepth * 0.5f);
|
||
}
|
||
// 远裁剪面仅覆盖当前内容，并限制 near/far 比值以保证深度缓冲精度。
|
||
farPlane = qMax(nearPlane + 1.0f, maximumDepth * 1.5f);
|
||
nearPlane = qMax(nearPlane, farPlane / 100000.0f);
|
||
}
|
||
|
||
const float aspect = static_cast<float>(width()) /
|
||
static_cast<float>(height() > 0 ? height() : 1);
|
||
m_projection.setToIdentity();
|
||
m_projection.perspective(45.0f, aspect, nearPlane, farPlane);
|
||
}
|
||
|
||
float PointCloudGLWidget::fittedCameraDistance(float padding) const
|
||
{
|
||
const QVector3D corners[] = {
|
||
{m_minBound.x(), m_minBound.y(), m_minBound.z()},
|
||
{m_minBound.x(), m_minBound.y(), m_maxBound.z()},
|
||
{m_minBound.x(), m_maxBound.y(), m_minBound.z()},
|
||
{m_minBound.x(), m_maxBound.y(), m_maxBound.z()},
|
||
{m_maxBound.x(), m_minBound.y(), m_minBound.z()},
|
||
{m_maxBound.x(), m_minBound.y(), m_maxBound.z()},
|
||
{m_maxBound.x(), m_maxBound.y(), m_minBound.z()},
|
||
{m_maxBound.x(), m_maxBound.y(), m_maxBound.z()}};
|
||
|
||
const float aspect = qMax(0.01f,
|
||
static_cast<float>(width()) /
|
||
static_cast<float>(height() > 0 ? height() : 1));
|
||
const float tangent = std::tan(qDegreesToRadians(45.0f * 0.5f));
|
||
const float safePadding = qMax(1.0f, padding);
|
||
float distance = minimumCameraDistance();
|
||
for (const QVector3D& corner : corners) {
|
||
const QVector3D relative =
|
||
m_rotation.rotatedVector(corner - m_center);
|
||
const float horizontalDistance =
|
||
relative.z() + std::fabs(relative.x()) * safePadding / (tangent * aspect);
|
||
const float verticalDistance =
|
||
relative.z() + std::fabs(relative.y()) * safePadding / tangent;
|
||
distance = qMax(distance, qMax(horizontalDistance, verticalDistance));
|
||
}
|
||
return qBound(10.0f, distance, 1.0e7f);
|
||
}
|
||
|
||
float PointCloudGLWidget::minimumCameraDistance() const
|
||
{
|
||
const QVector3D corners[] = {
|
||
{m_minBound.x(), m_minBound.y(), m_minBound.z()},
|
||
{m_minBound.x(), m_minBound.y(), m_maxBound.z()},
|
||
{m_minBound.x(), m_maxBound.y(), m_minBound.z()},
|
||
{m_minBound.x(), m_maxBound.y(), m_maxBound.z()},
|
||
{m_maxBound.x(), m_minBound.y(), m_minBound.z()},
|
||
{m_maxBound.x(), m_minBound.y(), m_maxBound.z()},
|
||
{m_maxBound.x(), m_maxBound.y(), m_minBound.z()},
|
||
{m_maxBound.x(), m_maxBound.y(), m_maxBound.z()}
|
||
};
|
||
|
||
float frontExtent = 0.0f;
|
||
for (const QVector3D& corner : corners) {
|
||
const QVector3D relativePosition = corner - m_center;
|
||
frontExtent = qMax(frontExtent,
|
||
m_rotation.rotatedVector(relativePosition).z());
|
||
}
|
||
|
||
const float margin = qMax(0.1f, (m_maxBound - m_minBound).length() * 0.002f);
|
||
return qBound(0.1f, frontExtent + margin, 1.0e7f);
|
||
}
|
||
|
||
void PointCloudGLWidget::zoomIn()
|
||
{
|
||
m_distance *= 0.9f;
|
||
m_distance = qBound(minimumCameraDistance(), m_distance, 1.0e7f);
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::zoomOut()
|
||
{
|
||
m_distance *= 1.1f;
|
||
m_distance = qBound(minimumCameraDistance(), m_distance, 1.0e7f);
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::setViewAngles(float rotX, float rotY, float rotZ)
|
||
{
|
||
// 归一化到 ±180°，避免往同一方向旋转时角度无限累加
|
||
m_rotationX = NormalizeAngleDegrees(rotX);
|
||
m_rotationY = NormalizeAngleDegrees(rotY);
|
||
m_rotationZ = NormalizeAngleDegrees(rotZ);
|
||
m_pan = QVector3D(0, 0, 0);
|
||
|
||
// 从欧拉角转换为四元数（ZYX顺序）
|
||
m_rotation = QQuaternion::fromEulerAngles(m_rotationX, m_rotationY, m_rotationZ);
|
||
m_distance = qMax(m_distance, minimumCameraDistance());
|
||
|
||
LOG_INFO("[CloudView] setViewAngles: rotX=%.1f, rotY=%.1f, rotZ=%.1f\n", rotX, rotY, rotZ);
|
||
|
||
emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ);
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::clearSelectedPoints()
|
||
{
|
||
m_selectedPoints.clear();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::clearSelectedLine()
|
||
{
|
||
m_selectedLine = SelectedLineInfo();
|
||
update();
|
||
}
|
||
|
||
bool PointCloudGLWidget::selectLineByIndex(int lineIndex)
|
||
{
|
||
if (m_pointClouds.empty()) {
|
||
return false;
|
||
}
|
||
|
||
const auto& cloudData = m_pointClouds[0];
|
||
if (!cloudData.hasLineInfo) {
|
||
return false;
|
||
}
|
||
|
||
// 检查线索引是否有效
|
||
if (lineIndex < 0 || lineIndex >= cloudData.totalLines) {
|
||
return false;
|
||
}
|
||
|
||
// 计算该线上的点数
|
||
int pointCount = 0;
|
||
for (int idx : cloudData.lineIndices) {
|
||
if (idx == lineIndex) {
|
||
pointCount++;
|
||
}
|
||
}
|
||
|
||
if (pointCount == 0) {
|
||
return false;
|
||
}
|
||
|
||
// 设置选中的线
|
||
m_selectedLine.valid = true;
|
||
m_selectedLine.cloudIndex = 0;
|
||
m_selectedLine.lineIndex = lineIndex;
|
||
m_selectedLine.pointIndex = -1;
|
||
m_selectedLine.pointCount = pointCount;
|
||
m_selectedLine.mode = LineSelectMode::Vertical;
|
||
|
||
emit lineSelected(m_selectedLine);
|
||
update();
|
||
return true;
|
||
}
|
||
|
||
bool PointCloudGLWidget::selectHorizontalLineByIndex(int pointIndex)
|
||
{
|
||
if (m_pointClouds.empty()) {
|
||
return false;
|
||
}
|
||
|
||
const auto& cloudData = m_pointClouds[0];
|
||
if (!cloudData.hasLineInfo || cloudData.totalLines <= 0 || cloudData.pointsPerLine <= 0) {
|
||
return false;
|
||
}
|
||
|
||
// 检查点索引是否有效（使用原始点云的每线点数）
|
||
if (pointIndex < 0 || pointIndex >= cloudData.pointsPerLine) {
|
||
return false;
|
||
}
|
||
|
||
bool hasPointAtIndex = false;
|
||
for (const int sourceIndex : cloudData.pointInLineIndices) {
|
||
if (sourceIndex == pointIndex) {
|
||
hasPointAtIndex = true;
|
||
break;
|
||
}
|
||
}
|
||
if (!hasPointAtIndex) {
|
||
return false;
|
||
}
|
||
|
||
// 设置选中的横向线
|
||
m_selectedLine.valid = true;
|
||
m_selectedLine.cloudIndex = 0;
|
||
m_selectedLine.lineIndex = -1;
|
||
m_selectedLine.pointIndex = pointIndex;
|
||
m_selectedLine.pointCount = cloudData.totalLines; // 横向线的点数等于总线数
|
||
m_selectedLine.mode = LineSelectMode::Horizontal;
|
||
|
||
emit lineSelected(m_selectedLine);
|
||
update();
|
||
return true;
|
||
}
|
||
|
||
float PointCloudGLWidget::calculateDistance(const SelectedPointInfo& p1, const SelectedPointInfo& p2)
|
||
{
|
||
if (!p1.valid || !p2.valid) {
|
||
return 0.0f;
|
||
}
|
||
|
||
float dx = p2.x - p1.x;
|
||
float dy = p2.y - p1.y;
|
||
float dz = p2.z - p1.z;
|
||
|
||
return std::sqrt(dx * dx + dy * dy + dz * dz);
|
||
}
|
||
|
||
void PointCloudGLWidget::updateSelectedPointCoord(int index, float x, float y, float z)
|
||
{
|
||
if (index < 0 || index >= m_selectedPoints.size()) {
|
||
return;
|
||
}
|
||
|
||
if (!m_selectedPoints[index].valid) {
|
||
return;
|
||
}
|
||
|
||
// 更新选中点的坐标
|
||
m_selectedPoints[index].x = x;
|
||
m_selectedPoints[index].y = y;
|
||
m_selectedPoints[index].z = z;
|
||
|
||
LOG_INFO("[CloudView] Updated selected point %d to (%.3f, %.3f, %.3f)\n", index, x, y, z);
|
||
|
||
// 刷新显示
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::setSelectedPointCoord(int index, float x, float y, float z)
|
||
{
|
||
if (index < 0 || index >= MAX_SELECTED_POINTS) {
|
||
return;
|
||
}
|
||
|
||
// 如果索引位置不存在，扩展列表
|
||
while (m_selectedPoints.size() <= index) {
|
||
m_selectedPoints.append(SelectedPointInfo());
|
||
}
|
||
|
||
m_selectedPoints[index].valid = true;
|
||
m_selectedPoints[index].x = x;
|
||
m_selectedPoints[index].y = y;
|
||
m_selectedPoints[index].z = z;
|
||
m_selectedPoints[index].cloudIndex = -1;
|
||
m_selectedPoints[index].lineIndex = -1;
|
||
m_selectedPoints[index].pointIndexInLine = -1;
|
||
|
||
LOG_INFO("[CloudView] Set selected point %d to (%.3f, %.3f, %.3f)\n", index, x, y, z);
|
||
|
||
// 刷新显示
|
||
update();
|
||
}
|
||
|
||
QVector<QVector3D> PointCloudGLWidget::getSelectedLinePoints() const
|
||
{
|
||
QVector<QVector3D> points;
|
||
|
||
if (!m_selectedLine.valid || m_selectedLine.cloudIndex < 0) {
|
||
return points;
|
||
}
|
||
|
||
if (m_selectedLine.cloudIndex >= static_cast<int>(m_pointClouds.size())) {
|
||
return points;
|
||
}
|
||
|
||
const auto& cloudData = m_pointClouds[m_selectedLine.cloudIndex];
|
||
if (!cloudData.hasLineInfo) {
|
||
return points;
|
||
}
|
||
|
||
if (m_selectedLine.mode == LineSelectMode::Vertical) {
|
||
// 纵向选线：获取同一条扫描线上的所有点
|
||
for (size_t i = 0; i < cloudData.lineIndices.size(); ++i) {
|
||
if (cloudData.lineIndices[i] == m_selectedLine.lineIndex) {
|
||
size_t vertIdx = i * 3;
|
||
if (vertIdx + 2 < cloudData.vertices.size()) {
|
||
points.append(QVector3D(
|
||
cloudData.vertices[vertIdx],
|
||
cloudData.vertices[vertIdx + 1],
|
||
cloudData.vertices[vertIdx + 2]));
|
||
}
|
||
}
|
||
}
|
||
} else {
|
||
// 横向选线：获取所有线的相同索引点
|
||
if (cloudData.pointsPerLine > 0 && m_selectedLine.pointIndex >= 0) {
|
||
for (size_t i = 0; i < cloudData.pointInLineIndices.size(); ++i) {
|
||
if (cloudData.pointInLineIndices[i] == m_selectedLine.pointIndex) {
|
||
size_t vertIdx = i * 3;
|
||
if (vertIdx + 2 < cloudData.vertices.size()) {
|
||
points.append(QVector3D(
|
||
cloudData.vertices[vertIdx],
|
||
cloudData.vertices[vertIdx + 1],
|
||
cloudData.vertices[vertIdx + 2]));
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return points;
|
||
}
|
||
|
||
void PointCloudGLWidget::setListHighlightPoint(const QVector3D& point)
|
||
{
|
||
m_hasListHighlightPoint = true;
|
||
m_listHighlightPoint = point;
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::clearListHighlightPoint()
|
||
{
|
||
m_hasListHighlightPoint = false;
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::transformAllClouds(const QMatrix4x4& matrix)
|
||
{
|
||
for (auto& cloudData : m_pointClouds) {
|
||
for (size_t i = 0; i + 2 < cloudData.vertices.size(); i += 3) {
|
||
QVector3D pt(cloudData.vertices[i], cloudData.vertices[i + 1], cloudData.vertices[i + 2]);
|
||
QVector3D transformed = matrix.map(pt);
|
||
cloudData.vertices[i] = transformed.x();
|
||
cloudData.vertices[i + 1] = transformed.y();
|
||
cloudData.vertices[i + 2] = transformed.z();
|
||
}
|
||
}
|
||
|
||
// 重新上传 VBO（顶点数据已变更）
|
||
makeCurrent();
|
||
for (auto& cloudData : m_pointClouds) {
|
||
uploadToVBO(cloudData);
|
||
}
|
||
doneCurrent();
|
||
|
||
computeBoundingBox();
|
||
updatePseudoColors();
|
||
resetView();
|
||
update();
|
||
}
|
||
|
||
size_t PointCloudGLWidget::rotateCloudsByZThreshold(const QMatrix4x4& rotMatrix, float zThreshold)
|
||
{
|
||
size_t rotatedCount = 0;
|
||
|
||
for (auto& cloudData : m_pointClouds) {
|
||
for (size_t i = 0; i + 2 < cloudData.vertices.size(); i += 3) {
|
||
if (cloudData.vertices[i + 2] < zThreshold) {
|
||
QVector3D pt(cloudData.vertices[i], cloudData.vertices[i + 1], cloudData.vertices[i + 2]);
|
||
QVector3D transformed = rotMatrix.map(pt);
|
||
cloudData.vertices[i] = transformed.x();
|
||
cloudData.vertices[i + 1] = transformed.y();
|
||
cloudData.vertices[i + 2] = transformed.z();
|
||
++rotatedCount;
|
||
}
|
||
}
|
||
}
|
||
|
||
if (rotatedCount > 0) {
|
||
makeCurrent();
|
||
for (auto& cloudData : m_pointClouds) {
|
||
uploadToVBO(cloudData);
|
||
}
|
||
doneCurrent();
|
||
computeBoundingBox();
|
||
updatePseudoColors();
|
||
resetView();
|
||
update();
|
||
}
|
||
|
||
return rotatedCount;
|
||
}
|
||
|
||
size_t PointCloudGLWidget::getAllCloudsByLines(std::vector<std::vector<SVzNL3DPosition>>& scanLines) const
|
||
{
|
||
scanLines.clear();
|
||
size_t totalPoints = 0;
|
||
|
||
for (const auto& cloudData : m_pointClouds) {
|
||
const size_t displayCount = cloudData.vertices.size() / 3;
|
||
if (displayCount == 0) continue;
|
||
|
||
if (cloudData.hasLineInfo && cloudData.totalLines > 0 && cloudData.pointsPerLine > 0) {
|
||
// 有线信息：用 pointsPerLine 还原每条线的完整点数，过滤掉的 (0,0,0) 点补零
|
||
const int lines = cloudData.totalLines;
|
||
const int ptsPerLine = cloudData.pointsPerLine;
|
||
|
||
// 初始化所有线，每条线 pointsPerLine 个零点
|
||
const size_t baseIdx = scanLines.size();
|
||
for (int i = 0; i < lines; ++i) {
|
||
std::vector<SVzNL3DPosition> line(ptsPerLine);
|
||
memset(line.data(), 0, sizeof(SVzNL3DPosition) * ptsPerLine);
|
||
for (int j = 0; j < ptsPerLine; ++j) {
|
||
line[j].nPointIdx = j;
|
||
}
|
||
scanLines.push_back(std::move(line));
|
||
}
|
||
|
||
// 用实际显示的点填充对应位置
|
||
for (size_t i = 0; i < displayCount; ++i) {
|
||
const int lineIdx = (i < cloudData.lineIndices.size()) ? cloudData.lineIndices[i] : 0;
|
||
const int ptInLine = (i < cloudData.pointInLineIndices.size()) ? cloudData.pointInLineIndices[i] : static_cast<int>(i);
|
||
|
||
if (lineIdx >= 0 && lineIdx < lines && ptInLine >= 0 && ptInLine < ptsPerLine) {
|
||
auto& pos = scanLines[baseIdx + lineIdx][ptInLine];
|
||
pos.pt3D.x = cloudData.vertices[i * 3];
|
||
pos.pt3D.y = cloudData.vertices[i * 3 + 1];
|
||
pos.pt3D.z = cloudData.vertices[i * 3 + 2];
|
||
pos.nPointIdx = ptInLine;
|
||
}
|
||
}
|
||
|
||
totalPoints += static_cast<size_t>(lines) * ptsPerLine;
|
||
} else {
|
||
// 无线信息，每个点云作为一条线
|
||
std::vector<SVzNL3DPosition> line;
|
||
line.reserve(displayCount);
|
||
for (size_t i = 0; i < displayCount; ++i) {
|
||
SVzNL3DPosition pos;
|
||
memset(&pos, 0, sizeof(pos));
|
||
pos.nPointIdx = static_cast<int>(i);
|
||
pos.pt3D.x = cloudData.vertices[i * 3];
|
||
pos.pt3D.y = cloudData.vertices[i * 3 + 1];
|
||
pos.pt3D.z = cloudData.vertices[i * 3 + 2];
|
||
line.push_back(pos);
|
||
}
|
||
totalPoints += displayCount;
|
||
scanLines.push_back(std::move(line));
|
||
}
|
||
}
|
||
|
||
return totalPoints;
|
||
}
|
||
|
||
void PointCloudGLWidget::computeBoundingBox()
|
||
{
|
||
float minX = FLT_MAX, minY = FLT_MAX, minZ = FLT_MAX;
|
||
float maxX = -FLT_MAX, maxY = -FLT_MAX, maxZ = -FLT_MAX;
|
||
bool hasContent = false;
|
||
|
||
auto includePoint = [&](float x, float y, float z) {
|
||
minX = qMin(minX, x);
|
||
minY = qMin(minY, y);
|
||
minZ = qMin(minZ, z);
|
||
maxX = qMax(maxX, x);
|
||
maxY = qMax(maxY, y);
|
||
maxZ = qMax(maxZ, z);
|
||
hasContent = true;
|
||
};
|
||
|
||
auto includeSegment = [&](const LineSegment& segment) {
|
||
includePoint(segment.x1, segment.y1, segment.z1);
|
||
includePoint(segment.x2, segment.y2, segment.z2);
|
||
};
|
||
|
||
for (const auto& cloudData : m_pointClouds) {
|
||
for (size_t i = 0; i < cloudData.vertices.size(); i += 3) {
|
||
includePoint(cloudData.vertices[i], cloudData.vertices[i + 1], cloudData.vertices[i + 2]);
|
||
}
|
||
}
|
||
|
||
for (const LineSegment& segment : m_lineSegments) {
|
||
includeSegment(segment);
|
||
}
|
||
|
||
for (const LineSegment& segment : m_basicShapeSegments) {
|
||
includeSegment(segment);
|
||
}
|
||
|
||
for (const SurfaceQuad& surface : m_basicShapeSurfaces) {
|
||
includePoint(surface.p1.x(), surface.p1.y(), surface.p1.z());
|
||
includePoint(surface.p2.x(), surface.p2.y(), surface.p2.z());
|
||
includePoint(surface.p3.x(), surface.p3.y(), surface.p3.z());
|
||
includePoint(surface.p4.x(), surface.p4.y(), surface.p4.z());
|
||
}
|
||
|
||
for (const BasicShapePoint& point : m_basicShapePoints) {
|
||
includePoint(point.x, point.y, point.z);
|
||
}
|
||
|
||
for (const PosePoint& pose : m_posePoints) {
|
||
const float scale = qMax(0.0f, pose.scale);
|
||
includePoint(pose.x - scale, pose.y - scale, pose.z - scale);
|
||
includePoint(pose.x + scale, pose.y + scale, pose.z + scale);
|
||
}
|
||
|
||
if (!hasContent) {
|
||
m_minBound = QVector3D(-50, -50, -50);
|
||
m_maxBound = QVector3D(50, 50, 50);
|
||
m_center = QVector3D(0, 0, 0);
|
||
return;
|
||
}
|
||
|
||
m_minBound = QVector3D(minX, minY, minZ);
|
||
m_maxBound = QVector3D(maxX, maxY, maxZ);
|
||
m_center = (m_minBound + m_maxBound) / 2.0f;
|
||
}
|
||
|
||
SelectedPointInfo PointCloudGLWidget::pickPoint(int screenX, int screenY)
|
||
{
|
||
SelectedPointInfo result;
|
||
|
||
if (m_pointClouds.empty()) {
|
||
return result;
|
||
}
|
||
|
||
// 视口（Qt 窗口即视口）；GLES2 无 GL_MODELVIEW_MATRIX / gluProject，用与渲染一致的矩阵手动投影
|
||
const GLint viewport[4] = {0, 0, static_cast<GLint>(width()), static_cast<GLint>(height())};
|
||
const QMatrix4x4 modelView = m_view * m_model;
|
||
const QMatrix4x4 mvp = m_projection * modelView;
|
||
|
||
// 转换屏幕 Y 坐标（OpenGL Y 轴向上）
|
||
const int glScreenY = viewport[3] - screenY;
|
||
|
||
float minScreenDist = FLT_MAX;
|
||
size_t bestIndex = 0;
|
||
int bestCloudIndex = -1;
|
||
int bestLineIndex = -1;
|
||
float bestX = 0, bestY = 0, bestZ = 0;
|
||
|
||
// 遍历所有点，计算屏幕投影距离
|
||
for (size_t cloudIdx = 0; cloudIdx < m_pointClouds.size(); ++cloudIdx) {
|
||
const auto& cloudData = m_pointClouds[cloudIdx];
|
||
for (size_t i = 0; i < cloudData.vertices.size(); i += 3) {
|
||
const float wx = cloudData.vertices[i];
|
||
const float wy = cloudData.vertices[i + 1];
|
||
const float wz = cloudData.vertices[i + 2];
|
||
|
||
// 将世界坐标投影到屏幕（等价于 gluProject）
|
||
const QVector4D clip = mvp * QVector4D(wx, wy, wz, 1.0f);
|
||
if (std::fabs(clip.w()) < 1e-9f) {
|
||
continue;
|
||
}
|
||
const float ndcX = clip.x() / clip.w();
|
||
const float ndcY = clip.y() / clip.w();
|
||
const float sx = viewport[0] + (ndcX + 1.0f) * 0.5f * viewport[2];
|
||
const float sy = viewport[1] + (ndcY + 1.0f) * 0.5f * viewport[3];
|
||
|
||
// 计算屏幕距离
|
||
const float dx = sx - screenX;
|
||
const float dy = sy - glScreenY;
|
||
const float screenDist = dx * dx + dy * dy;
|
||
|
||
if (screenDist < minScreenDist) {
|
||
minScreenDist = screenDist;
|
||
bestIndex = i / 3;
|
||
bestCloudIndex = static_cast<int>(cloudIdx);
|
||
bestX = wx;
|
||
bestY = wy;
|
||
bestZ = wz;
|
||
|
||
// 获取线索引
|
||
if (cloudData.hasLineInfo && bestIndex < cloudData.lineIndices.size()) {
|
||
bestLineIndex = cloudData.lineIndices[bestIndex];
|
||
} else {
|
||
bestLineIndex = -1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 屏幕距离阈值（像素），20像素内认为选中
|
||
float threshold = 20.0f * 20.0f;
|
||
if (minScreenDist < threshold) {
|
||
result.valid = true;
|
||
result.index = bestIndex;
|
||
result.cloudIndex = bestCloudIndex;
|
||
result.lineIndex = bestLineIndex;
|
||
result.x = bestX;
|
||
result.y = bestY;
|
||
result.z = bestZ;
|
||
|
||
// 计算点在线中的原始索引
|
||
if (bestLineIndex >= 0 && bestCloudIndex >= 0) {
|
||
const auto& cloudData = m_pointClouds[bestCloudIndex];
|
||
// 使用预计算的线内索引（支持不等长线）
|
||
if (bestIndex < cloudData.pointInLineIndices.size()) {
|
||
result.pointIndexInLine = cloudData.pointInLineIndices[bestIndex];
|
||
}
|
||
}
|
||
|
||
LOG_INFO("[CloudView] Point selected: (%.3f, %.3f, %.3f) lineIndex=%d pointIndexInLine=%d screenDist=%.1f\n",
|
||
bestX, bestY, bestZ, bestLineIndex, result.pointIndexInLine, std::sqrt(minScreenDist));
|
||
} else {
|
||
LOG_INFO("[CloudView] No point selected, minScreenDist=%.1f\n", std::sqrt(minScreenDist));
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
void PointCloudGLWidget::drawSelectedPoints()
|
||
{
|
||
QVector<float> positions;
|
||
QVector<float> colors;
|
||
|
||
// 绘制选中的点（橙色）
|
||
for (const auto& pt : m_selectedPoints) {
|
||
if (pt.valid) {
|
||
positions.append(pt.x);
|
||
positions.append(pt.y);
|
||
positions.append(pt.z);
|
||
colors.append(1.0f);
|
||
colors.append(0.5f);
|
||
colors.append(0.0f);
|
||
}
|
||
}
|
||
|
||
// 绘制列表高亮点（蓝色，与选点区分）
|
||
if (m_hasListHighlightPoint) {
|
||
positions.append(m_listHighlightPoint.x());
|
||
positions.append(m_listHighlightPoint.y());
|
||
positions.append(m_listHighlightPoint.z());
|
||
colors.append(0.0f);
|
||
colors.append(0.5f);
|
||
colors.append(1.0f);
|
||
}
|
||
|
||
if (positions.isEmpty()) {
|
||
return;
|
||
}
|
||
drawPrimitives(m_mvp,
|
||
positions.constData(),
|
||
positions.size() / 3,
|
||
GL_POINTS,
|
||
colors.constData(),
|
||
QVector3D(1.0f, 1.0f, 1.0f),
|
||
10.0f,
|
||
1.0f,
|
||
false);
|
||
}
|
||
|
||
void PointCloudGLWidget::drawMeasurementLine()
|
||
{
|
||
if (m_selectedPoints.size() < 2) {
|
||
return;
|
||
}
|
||
|
||
const auto& p1 = m_selectedPoints[0];
|
||
const auto& p2 = m_selectedPoints[1];
|
||
|
||
if (!p1.valid || !p2.valid) {
|
||
return;
|
||
}
|
||
|
||
const float positions[] = {
|
||
p1.x, p1.y, p1.z,
|
||
p2.x, p2.y, p2.z};
|
||
drawPrimitives(m_mvp,
|
||
positions,
|
||
2,
|
||
GL_LINES,
|
||
nullptr,
|
||
QVector3D(0.0f, 1.0f, 0.0f),
|
||
1.0f,
|
||
1.0f,
|
||
false);
|
||
}
|
||
|
||
void PointCloudGLWidget::drawSelectedLine()
|
||
{
|
||
if (!m_selectedLine.valid || m_selectedLine.cloudIndex < 0) {
|
||
return;
|
||
}
|
||
|
||
if (m_selectedLine.cloudIndex >= static_cast<int>(m_pointClouds.size())) {
|
||
return;
|
||
}
|
||
|
||
const auto& cloudData = m_pointClouds[m_selectedLine.cloudIndex];
|
||
if (!cloudData.hasLineInfo) {
|
||
return;
|
||
}
|
||
|
||
// 高亮显示选中线上的所有点（黄色）
|
||
QVector<float> positions;
|
||
|
||
if (m_selectedLine.mode == LineSelectMode::Vertical) {
|
||
// 纵向选线：显示同一条扫描线上的所有点
|
||
for (size_t i = 0; i < cloudData.lineIndices.size(); ++i) {
|
||
if (cloudData.lineIndices[i] == m_selectedLine.lineIndex) {
|
||
const size_t vertIdx = i * 3;
|
||
if (vertIdx + 2 < cloudData.vertices.size()) {
|
||
positions.append(cloudData.vertices[vertIdx]);
|
||
positions.append(cloudData.vertices[vertIdx + 1]);
|
||
positions.append(cloudData.vertices[vertIdx + 2]);
|
||
}
|
||
}
|
||
}
|
||
} else {
|
||
// 横向选线：显示所有线的相同原始索引点
|
||
if (cloudData.pointsPerLine > 0 && m_selectedLine.pointIndex >= 0) {
|
||
for (size_t i = 0; i < cloudData.pointInLineIndices.size(); ++i) {
|
||
if (cloudData.pointInLineIndices[i] == m_selectedLine.pointIndex) {
|
||
const size_t vertIdx = i * 3;
|
||
if (vertIdx + 2 < cloudData.vertices.size()) {
|
||
positions.append(cloudData.vertices[vertIdx]);
|
||
positions.append(cloudData.vertices[vertIdx + 1]);
|
||
positions.append(cloudData.vertices[vertIdx + 2]);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
if (positions.isEmpty()) {
|
||
return;
|
||
}
|
||
drawPrimitives(m_mvp,
|
||
positions.constData(),
|
||
positions.size() / 3,
|
||
GL_POINTS,
|
||
nullptr,
|
||
QVector3D(1.0f, 1.0f, 0.0f),
|
||
3.0f,
|
||
1.0f,
|
||
false);
|
||
}
|
||
|
||
void PointCloudGLWidget::drawAxis()
|
||
{
|
||
// 在右下角绘制坐标系指示器
|
||
// 坐标系定义：X向右，Y向下，Z朝后
|
||
// X轴：红色，指向右
|
||
// Y轴：绿色，指向下
|
||
// Z轴：蓝色，指向后（远离观察者）
|
||
GLint viewport[4];
|
||
glGetIntegerv(GL_VIEWPORT, viewport);
|
||
|
||
int axisSize = 60; // 坐标系区域大小（像素）
|
||
int margin = 10; // 距离边缘的边距
|
||
int axisX = viewport[2] - axisSize - margin; // 右下角 X
|
||
int axisY = margin; // 右下角 Y（OpenGL Y 轴向上）
|
||
|
||
// 正交投影 + 视图变换（等价于原 glOrtho + glTranslatef + glMultMatrixf）
|
||
QMatrix4x4 projection;
|
||
projection.setToIdentity();
|
||
projection.ortho(0, viewport[2], 0, viewport[3], -100, 100);
|
||
|
||
QMatrix4x4 modelView;
|
||
modelView.setToIdentity();
|
||
modelView.translate(axisX + axisSize / 2.0f, axisY + axisSize / 2.0f, 0.0f);
|
||
QMatrix4x4 rotation;
|
||
rotation.rotate(m_rotation);
|
||
modelView *= rotation;
|
||
|
||
const QMatrix4x4 mvp = projection * modelView;
|
||
|
||
const float axisLength = axisSize * 0.4f; // 坐标轴长度
|
||
const float positions[] = {
|
||
0.0f, 0.0f, 0.0f, axisLength, 0.0f, 0.0f, // X 轴 - 红色（向右）
|
||
0.0f, 0.0f, 0.0f, 0.0f, axisLength, 0.0f, // Y 轴 - 绿色（向下）
|
||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, axisLength // Z 轴 - 蓝色（向后）
|
||
};
|
||
const float colors[] = {
|
||
1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
|
||
0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
|
||
0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f
|
||
};
|
||
|
||
// 关闭深度测试，确保坐标系始终可见
|
||
drawPrimitives(mvp, positions, 6, GL_LINES, colors,
|
||
QVector3D(1.0f, 1.0f, 1.0f), 1.0f, 1.0f, false);
|
||
}
|
||
|
||
void PointCloudGLWidget::drawAxisLabels()
|
||
{
|
||
GLint viewport[4];
|
||
glGetIntegerv(GL_VIEWPORT, viewport);
|
||
|
||
int axisSize = 60;
|
||
int margin = 10;
|
||
float axisLength = axisSize * 0.4f;
|
||
|
||
// 坐标轴中心在 OpenGL 坐标系中的位置
|
||
float centerGlX = viewport[2] - axisSize / 2.0f - margin;
|
||
float centerGlY = axisSize / 2.0f + margin;
|
||
|
||
// 对各轴端点应用当前视图旋转
|
||
QMatrix4x4 rotMatrix;
|
||
rotMatrix.rotate(m_rotation);
|
||
|
||
QVector3D xEnd = rotMatrix.map(QVector3D(axisLength, 0, 0));
|
||
QVector3D yEnd = rotMatrix.map(QVector3D(0, axisLength, 0));
|
||
QVector3D zEnd = rotMatrix.map(QVector3D(0, 0, axisLength));
|
||
|
||
// 转换为 Qt widget 坐标（Y 轴翻转：OpenGL Y 向上 → Qt Y 向下）
|
||
auto toWidgetPos = [&](const QVector3D& v) -> QPointF {
|
||
float glX = centerGlX + v.x();
|
||
float glY = centerGlY + v.y();
|
||
return QPointF(glX, viewport[3] - glY);
|
||
};
|
||
|
||
QPointF xPos = toWidgetPos(xEnd);
|
||
QPointF yPos = toWidgetPos(yEnd);
|
||
QPointF zPos = toWidgetPos(zEnd);
|
||
|
||
// 使用 QPainter 绘制文字标注
|
||
QPainter painter(this);
|
||
painter.setRenderHint(QPainter::TextAntialiasing);
|
||
QFont font = painter.font();
|
||
font.setBold(true);
|
||
font.setPointSize(14);
|
||
painter.setFont(font);
|
||
|
||
// 文字偏移：向轴方向外侧偏移，避免与轴线重叠
|
||
int offset = 4;
|
||
|
||
painter.setPen(QColor(255, 80, 80)); // 红色
|
||
painter.drawText(xPos.x() + offset, xPos.y() + offset, "X");
|
||
|
||
painter.setPen(QColor(80, 255, 80)); // 绿色
|
||
painter.drawText(yPos.x() + offset, yPos.y() + offset, "Y");
|
||
|
||
painter.setPen(QColor(80, 128, 255)); // 蓝色
|
||
painter.drawText(zPos.x() + offset, zPos.y() + offset, "Z");
|
||
|
||
painter.end();
|
||
}
|
||
|
||
void PointCloudGLWidget::mousePressEvent(QMouseEvent* event)
|
||
{
|
||
m_lastMousePos = event->pos();
|
||
|
||
if (event->button() == Qt::LeftButton) {
|
||
// Ctrl+左键：选点
|
||
if (event->modifiers() & Qt::ControlModifier) {
|
||
makeCurrent();
|
||
SelectedPointInfo point = pickPoint(event->pos().x(), event->pos().y());
|
||
doneCurrent();
|
||
|
||
if (point.valid) {
|
||
if (m_measureDistanceEnabled) {
|
||
// 启用测距：最多保留两个点
|
||
if (m_selectedPoints.size() >= MAX_SELECTED_POINTS) {
|
||
m_selectedPoints.clear();
|
||
}
|
||
m_selectedPoints.append(point);
|
||
|
||
emit pointSelected(point);
|
||
|
||
if (m_selectedPoints.size() == 2) {
|
||
const float distance = calculateDistance(m_selectedPoints[0], m_selectedPoints[1]);
|
||
const float dx = m_selectedPoints[1].x - m_selectedPoints[0].x;
|
||
const float dy = m_selectedPoints[1].y - m_selectedPoints[0].y;
|
||
const float dz = m_selectedPoints[1].z - m_selectedPoints[0].z;
|
||
emit twoPointsSelected(m_selectedPoints[0], m_selectedPoints[1], distance);
|
||
emit measurementFinished(m_selectedPoints[0], m_selectedPoints[1], distance, dx, dy, dz);
|
||
}
|
||
} else {
|
||
// 未启用测距：只保留一个点
|
||
m_selectedPoints.clear();
|
||
m_selectedPoints.append(point);
|
||
emit pointSelected(point);
|
||
}
|
||
|
||
update();
|
||
}
|
||
} else if (event->modifiers() & Qt::ShiftModifier) {
|
||
// Shift+左键：选线
|
||
makeCurrent();
|
||
SelectedPointInfo point = pickPoint(event->pos().x(), event->pos().y());
|
||
doneCurrent();
|
||
|
||
if (point.valid && point.lineIndex >= 0) {
|
||
if (m_lineSelectMode == LineSelectMode::Vertical) {
|
||
// 纵向选线：选中该点所在的线
|
||
m_selectedLine.valid = true;
|
||
m_selectedLine.cloudIndex = point.cloudIndex;
|
||
m_selectedLine.lineIndex = point.lineIndex;
|
||
m_selectedLine.pointIndex = -1;
|
||
m_selectedLine.mode = LineSelectMode::Vertical;
|
||
|
||
// 计算该线上的点数
|
||
int pointCount = 0;
|
||
if (point.cloudIndex >= 0 && point.cloudIndex < static_cast<int>(m_pointClouds.size())) {
|
||
const auto& cloudData = m_pointClouds[point.cloudIndex];
|
||
for (int idx : cloudData.lineIndices) {
|
||
if (idx == point.lineIndex) {
|
||
pointCount++;
|
||
}
|
||
}
|
||
}
|
||
m_selectedLine.pointCount = pointCount;
|
||
} else {
|
||
// 横向选线：选中所有线的相同索引点
|
||
m_selectedLine.valid = true;
|
||
m_selectedLine.cloudIndex = point.cloudIndex;
|
||
m_selectedLine.lineIndex = -1;
|
||
m_selectedLine.pointIndex = point.pointIndexInLine;
|
||
m_selectedLine.mode = LineSelectMode::Horizontal;
|
||
|
||
// 横向线的点数等于总线数
|
||
if (point.cloudIndex >= 0 && point.cloudIndex < static_cast<int>(m_pointClouds.size())) {
|
||
m_selectedLine.pointCount = m_pointClouds[point.cloudIndex].totalLines;
|
||
}
|
||
}
|
||
|
||
emit lineSelected(m_selectedLine);
|
||
update();
|
||
}
|
||
} else {
|
||
m_leftButtonPressed = true;
|
||
}
|
||
} else if (event->button() == Qt::RightButton) {
|
||
m_rightButtonPressed = true;
|
||
} else if (event->button() == Qt::MiddleButton) {
|
||
m_middleButtonPressed = true;
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::mouseDoubleClickEvent(QMouseEvent* event)
|
||
{
|
||
if (event->button() == Qt::LeftButton) {
|
||
// 双击左键：选点（与 Ctrl+左键 相同逻辑）
|
||
makeCurrent();
|
||
SelectedPointInfo point = pickPoint(event->pos().x(), event->pos().y());
|
||
doneCurrent();
|
||
|
||
if (point.valid) {
|
||
if (m_measureDistanceEnabled) {
|
||
// 启用测距：最多保留两个点
|
||
if (m_selectedPoints.size() >= MAX_SELECTED_POINTS) {
|
||
m_selectedPoints.clear();
|
||
}
|
||
m_selectedPoints.append(point);
|
||
|
||
emit pointSelected(point);
|
||
|
||
if (m_selectedPoints.size() == 2) {
|
||
const float distance = calculateDistance(m_selectedPoints[0], m_selectedPoints[1]);
|
||
const float dx = m_selectedPoints[1].x - m_selectedPoints[0].x;
|
||
const float dy = m_selectedPoints[1].y - m_selectedPoints[0].y;
|
||
const float dz = m_selectedPoints[1].z - m_selectedPoints[0].z;
|
||
emit twoPointsSelected(m_selectedPoints[0], m_selectedPoints[1], distance);
|
||
emit measurementFinished(m_selectedPoints[0], m_selectedPoints[1], distance, dx, dy, dz);
|
||
}
|
||
} else {
|
||
// 未启用测距：只保留一个点
|
||
m_selectedPoints.clear();
|
||
m_selectedPoints.append(point);
|
||
emit pointSelected(point);
|
||
}
|
||
|
||
update();
|
||
}
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::mouseMoveEvent(QMouseEvent* event)
|
||
{
|
||
QPoint delta = event->pos() - m_lastMousePos;
|
||
m_lastMousePos = event->pos();
|
||
|
||
if (m_leftButtonPressed) {
|
||
// Alt+左键拖动：绕视线方向旋转（滚转）
|
||
if (event->modifiers() & Qt::AltModifier) {
|
||
// 绕Z轴（视线方向）旋转（修正方向）
|
||
float angle = -delta.x() * 0.5f; // 取反修正方向
|
||
QQuaternion deltaRotation = QQuaternion::fromAxisAndAngle(QVector3D(0, 0, 1), angle);
|
||
m_rotation = deltaRotation * m_rotation;
|
||
m_rotationZ = NormalizeAngleDegrees(m_rotationZ + angle);
|
||
} else {
|
||
// 普通左键拖动：基于当前物体坐标系的旋转
|
||
// 鼠标水平移动 -> 绕当前Y轴旋转
|
||
// 鼠标垂直移动 -> 绕当前X轴旋转
|
||
|
||
// 计算旋转角度
|
||
float angleX = delta.y() * 0.5f; // 垂直移动
|
||
float angleY = delta.x() * 0.5f; // 水平移动
|
||
|
||
// 创建增量旋转四元数（在相机空间中）
|
||
// 先绕X轴旋转（俯仰），再绕Y轴旋转（偏航）
|
||
QQuaternion deltaRotationX = QQuaternion::fromAxisAndAngle(QVector3D(1, 0, 0), angleX);
|
||
QQuaternion deltaRotationY = QQuaternion::fromAxisAndAngle(QVector3D(0, 1, 0), angleY);
|
||
QQuaternion deltaRotation = deltaRotationY * deltaRotationX;
|
||
|
||
// 应用增量旋转（左乘，相机空间旋转）
|
||
m_rotation = deltaRotation * m_rotation;
|
||
|
||
// 更新欧拉角（用于显示），归一化到 ±180°
|
||
m_rotationX = NormalizeAngleDegrees(m_rotationX + angleX);
|
||
m_rotationY = NormalizeAngleDegrees(m_rotationY + angleY);
|
||
}
|
||
|
||
// 发送角度变化信号
|
||
m_distance = qMax(m_distance, minimumCameraDistance());
|
||
emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ);
|
||
update();
|
||
} else if (m_middleButtonPressed) {
|
||
float factor = m_distance * 0.002f;
|
||
m_pan.setX(m_pan.x() - delta.x() * factor);
|
||
m_pan.setY(m_pan.y() + delta.y() * factor);
|
||
update();
|
||
} else if (m_rightButtonPressed) {
|
||
// 右键拖动：绕视线方向旋转（滚转）（修正方向）
|
||
float angle = -delta.x() * 0.5f; // 取反修正方向
|
||
QQuaternion deltaRotation = QQuaternion::fromAxisAndAngle(QVector3D(0, 0, 1), angle);
|
||
m_rotation = deltaRotation * m_rotation;
|
||
m_rotationZ = NormalizeAngleDegrees(m_rotationZ + angle);
|
||
m_distance = qMax(m_distance, minimumCameraDistance());
|
||
emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ);
|
||
update();
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::mouseReleaseEvent(QMouseEvent* event)
|
||
{
|
||
if (event->button() == Qt::LeftButton) {
|
||
m_leftButtonPressed = false;
|
||
} else if (event->button() == Qt::RightButton) {
|
||
m_rightButtonPressed = false;
|
||
} else if (event->button() == Qt::MiddleButton) {
|
||
m_middleButtonPressed = false;
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::wheelEvent(QWheelEvent* event)
|
||
{
|
||
float delta = event->angleDelta().y() / 120.0f;
|
||
m_distance *= (1.0f - delta * 0.1f);
|
||
m_distance = qBound(minimumCameraDistance(), m_distance, 1.0e7f);
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::keyPressEvent(QKeyEvent* event)
|
||
{
|
||
if (event->key() == Qt::Key_Space) {
|
||
resetView();
|
||
} else {
|
||
QOpenGLWidget::keyPressEvent(event);
|
||
}
|
||
}
|
||
|
||
bool PointCloudGLWidget::getFirstCloudData(PointCloudXYZ& cloud) const
|
||
{
|
||
if (m_pointClouds.empty()) {
|
||
return false;
|
||
}
|
||
|
||
const auto& cloudData = m_pointClouds[0];
|
||
cloud.clear();
|
||
cloud.reserve(cloudData.vertices.size() / 3);
|
||
|
||
for (size_t i = 0; i < cloudData.vertices.size(); i += 3) {
|
||
Point3D pt;
|
||
pt.x = cloudData.vertices[i];
|
||
pt.y = cloudData.vertices[i + 1];
|
||
pt.z = cloudData.vertices[i + 2];
|
||
|
||
int lineIdx = 0;
|
||
if (cloudData.hasLineInfo && (i / 3) < cloudData.lineIndices.size()) {
|
||
lineIdx = cloudData.lineIndices[i / 3];
|
||
}
|
||
const int pointIdx = ((i / 3) < cloudData.pointInLineIndices.size())
|
||
? cloudData.pointInLineIndices[i / 3] : static_cast<int>(i / 3);
|
||
cloud.push_back(pt, lineIdx, pointIdx);
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
void PointCloudGLWidget::replaceFirstCloud(const PointCloudXYZ& cloud, const QString& name)
|
||
{
|
||
if (m_pointClouds.empty()) {
|
||
addPointCloud(cloud, name);
|
||
return;
|
||
}
|
||
|
||
// 保留第一个点云的颜色索引
|
||
int colorIndex = m_pointClouds[0].colorIndex;
|
||
|
||
// 重新构建数据
|
||
PointCloudData data;
|
||
data.name = name;
|
||
data.hasColor = false;
|
||
data.hasLineInfo = !cloud.lineIndices.empty();
|
||
data.colorIndex = colorIndex;
|
||
data.vertices.reserve(cloud.size() * 3);
|
||
data.totalLines = 0;
|
||
data.pointsPerLine = 0;
|
||
|
||
const float EPSILON = 1e-6f;
|
||
int previousLineIndex = -1;
|
||
int fallbackPointIndex = 0;
|
||
int maxPointCount = 0;
|
||
for (size_t i = 0; i < cloud.points.size(); ++i) {
|
||
const auto& pt = cloud.points[i];
|
||
const int lineIdx = (i < cloud.lineIndices.size()) ? cloud.lineIndices[i] : -1;
|
||
if (lineIdx != previousLineIndex) {
|
||
fallbackPointIndex = 0;
|
||
previousLineIndex = lineIdx;
|
||
}
|
||
int pointIdx = (i < cloud.pointInLineIndices.size())
|
||
? cloud.pointInLineIndices[i] : -1;
|
||
if (pointIdx < 0) {
|
||
pointIdx = fallbackPointIndex;
|
||
}
|
||
maxPointCount = qMax(maxPointCount, pointIdx + 1);
|
||
if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
|
||
++fallbackPointIndex;
|
||
continue;
|
||
}
|
||
// 显示时过滤 (0,0,0) 点
|
||
if (std::fabs(pt.x) < EPSILON && std::fabs(pt.y) < EPSILON && std::fabs(pt.z) < EPSILON) {
|
||
++fallbackPointIndex;
|
||
continue;
|
||
}
|
||
data.vertices.push_back(pt.x);
|
||
data.vertices.push_back(pt.y);
|
||
data.vertices.push_back(pt.z);
|
||
|
||
// 保存原始索引
|
||
data.originalIndices.push_back(static_cast<int>(i));
|
||
data.pointInLineIndices.push_back(pointIdx);
|
||
|
||
if (data.hasLineInfo) {
|
||
// 与顶点保持一一对应；缺失的来源线号以 -1 表示。
|
||
data.lineIndices.push_back(lineIdx);
|
||
if (lineIdx >= 0 && lineIdx + 1 > data.totalLines) {
|
||
data.totalLines = lineIdx + 1;
|
||
}
|
||
}
|
||
++fallbackPointIndex;
|
||
}
|
||
|
||
// 计算每线点数
|
||
if (data.totalLines > 0) {
|
||
data.pointsPerLine = maxPointCount;
|
||
}
|
||
|
||
// 释放旧的 VBO
|
||
makeCurrent();
|
||
releaseVBO(m_pointClouds[0]);
|
||
|
||
m_pointClouds[0] = std::move(data);
|
||
|
||
// 上传新的 VBO
|
||
uploadToVBO(m_pointClouds[0]);
|
||
doneCurrent();
|
||
|
||
computeBoundingBox();
|
||
updatePseudoColors();
|
||
resetView();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::addLineSegments(const QVector<LineSegment>& segments)
|
||
{
|
||
m_lineSegments.append(segments);
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::clearLineSegments()
|
||
{
|
||
m_lineSegments.clear();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::setBasicShapeSegments(const QVector<LineSegment>& segments)
|
||
{
|
||
m_basicShapeSegments = segments;
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::appendBasicShapeSegments(const QVector<LineSegment>& segments)
|
||
{
|
||
if (segments.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
for (const LineSegment& segment : segments) {
|
||
m_basicShapeSegments.append(segment);
|
||
}
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::appendBasicShapeSurfaces(const QVector<SurfaceQuad>& surfaces)
|
||
{
|
||
if (surfaces.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
for (const SurfaceQuad& surface : surfaces) {
|
||
m_basicShapeSurfaces.append(surface);
|
||
}
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::appendBasicShapePoints(const QVector<BasicShapePoint>& points)
|
||
{
|
||
if (points.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
for (const BasicShapePoint& point : points) {
|
||
m_basicShapePoints.append(point);
|
||
}
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::clearBasicShapeSegments()
|
||
{
|
||
m_basicShapeSegments.clear();
|
||
m_basicShapeSurfaces.clear();
|
||
m_basicShapePoints.clear();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::addPosePoints(const QVector<PosePoint>& poses)
|
||
{
|
||
m_posePoints.append(poses);
|
||
computeBoundingBox();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::clearPosePoints()
|
||
{
|
||
m_posePoints.clear();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::drawLineSegments()
|
||
{
|
||
drawLineSegmentList(m_lineSegments);
|
||
}
|
||
|
||
void PointCloudGLWidget::drawBasicShapeSurfaces()
|
||
{
|
||
if (m_basicShapeSurfaces.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
glEnable(GL_BLEND);
|
||
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
|
||
glDepthMask(GL_FALSE);
|
||
|
||
for (const SurfaceQuad& surface : m_basicShapeSurfaces) {
|
||
// GLES2 无 GL_QUADS，四边形拆成两个三角形 (p1,p2,p3) 和 (p1,p3,p4)
|
||
const float positions[] = {
|
||
surface.p1.x(), surface.p1.y(), surface.p1.z(),
|
||
surface.p2.x(), surface.p2.y(), surface.p2.z(),
|
||
surface.p3.x(), surface.p3.y(), surface.p3.z(),
|
||
surface.p1.x(), surface.p1.y(), surface.p1.z(),
|
||
surface.p3.x(), surface.p3.y(), surface.p3.z(),
|
||
surface.p4.x(), surface.p4.y(), surface.p4.z(),
|
||
};
|
||
drawPrimitives(m_mvp, positions, 6, GL_TRIANGLES, nullptr,
|
||
QVector3D(surface.r, surface.g, surface.b),
|
||
1.0f, surface.a, true);
|
||
}
|
||
|
||
glDepthMask(GL_TRUE);
|
||
glDisable(GL_BLEND);
|
||
}
|
||
|
||
void PointCloudGLWidget::drawBasicShapeSegments()
|
||
{
|
||
drawLineSegmentList(m_basicShapeSegments);
|
||
}
|
||
|
||
void PointCloudGLWidget::drawBasicShapePoints()
|
||
{
|
||
if (m_basicShapePoints.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
for (const BasicShapePoint& point : m_basicShapePoints) {
|
||
const float positions[] = {point.x, point.y, point.z};
|
||
drawPrimitives(m_mvp, positions, 1, GL_POINTS, nullptr,
|
||
QVector3D(point.r, point.g, point.b),
|
||
point.pointSize, 1.0f, false);
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::drawLineSegmentList(const QVector<LineSegment>& segments)
|
||
{
|
||
if (segments.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
// 按线宽分组绘制
|
||
// 收集所有不同的线宽值（0 表示默认 2.0f）
|
||
std::map<float, QVector<int>> widthGroups;
|
||
for (int i = 0; i < segments.size(); ++i) {
|
||
const float w = segments[i].lineWidth > 0 ? segments[i].lineWidth : 2.0f;
|
||
widthGroups[w].append(i);
|
||
}
|
||
|
||
for (const auto& group : widthGroups) {
|
||
QVector<float> positions;
|
||
QVector<float> colors;
|
||
positions.reserve(group.second.size() * 6);
|
||
colors.reserve(group.second.size() * 6);
|
||
for (int idx : group.second) {
|
||
const LineSegment& seg = segments[idx];
|
||
positions.append(seg.x1); positions.append(seg.y1); positions.append(seg.z1);
|
||
positions.append(seg.x2); positions.append(seg.y2); positions.append(seg.z2);
|
||
colors.append(seg.r); colors.append(seg.g); colors.append(seg.b);
|
||
colors.append(seg.r); colors.append(seg.g); colors.append(seg.b);
|
||
}
|
||
if (positions.isEmpty()) {
|
||
continue;
|
||
}
|
||
// GLES2 下 glLineWidth 有效宽度常受限（部分 Mali 仅支持 1.0），
|
||
// 仍按原线宽设置，驱动不支持时自动退化为最接近的可用宽度。
|
||
glLineWidth(group.first);
|
||
drawPrimitives(m_mvp, positions.constData(), positions.size() / 3,
|
||
GL_LINES, colors.constData(),
|
||
QVector3D(1.0f, 1.0f, 1.0f), 1.0f, 1.0f, true);
|
||
}
|
||
|
||
glLineWidth(1.0f);
|
||
}
|
||
|
||
void PointCloudGLWidget::drawPosePoints()
|
||
{
|
||
if (m_posePoints.isEmpty())
|
||
{
|
||
return;
|
||
}
|
||
|
||
// 从视图矩阵取相机右/上方向，把轴线拉成面向屏幕的窄带，避免 GLES2 线宽被钳成 1 像素。
|
||
const QMatrix4x4 invView = m_view.inverted();
|
||
const QVector3D camRight = invView.column(0).toVector3D().normalized();
|
||
const QVector3D camUp = invView.column(1).toVector3D().normalized();
|
||
const float pixelWorld = m_distance *
|
||
std::tan(qDegreesToRadians(22.5f)) /
|
||
static_cast<float>(qMax(1, height() / 2));
|
||
|
||
auto applyEulerRotation = [this](QMatrix4x4& poseLocal, const PosePoint& pose)
|
||
{
|
||
switch (m_eulerRotationOrder)
|
||
{
|
||
case EulerRotationOrder::ZYX:
|
||
poseLocal.rotate(pose.rx, 1, 0, 0);
|
||
poseLocal.rotate(pose.ry, 0, 1, 0);
|
||
poseLocal.rotate(pose.rz, 0, 0, 1);
|
||
break;
|
||
case EulerRotationOrder::XYZ:
|
||
poseLocal.rotate(pose.rz, 0, 0, 1);
|
||
poseLocal.rotate(pose.ry, 0, 1, 0);
|
||
poseLocal.rotate(pose.rx, 1, 0, 0);
|
||
break;
|
||
case EulerRotationOrder::ZXY:
|
||
poseLocal.rotate(pose.ry, 0, 1, 0);
|
||
poseLocal.rotate(pose.rx, 1, 0, 0);
|
||
poseLocal.rotate(pose.rz, 0, 0, 1);
|
||
break;
|
||
case EulerRotationOrder::YXZ:
|
||
poseLocal.rotate(pose.rz, 0, 0, 1);
|
||
poseLocal.rotate(pose.rx, 1, 0, 0);
|
||
poseLocal.rotate(pose.ry, 0, 1, 0);
|
||
break;
|
||
case EulerRotationOrder::XZY:
|
||
poseLocal.rotate(pose.ry, 0, 1, 0);
|
||
poseLocal.rotate(pose.rz, 0, 0, 1);
|
||
poseLocal.rotate(pose.rx, 1, 0, 0);
|
||
break;
|
||
case EulerRotationOrder::YZX:
|
||
poseLocal.rotate(pose.rx, 1, 0, 0);
|
||
poseLocal.rotate(pose.rz, 0, 0, 1);
|
||
poseLocal.rotate(pose.ry, 0, 1, 0);
|
||
break;
|
||
}
|
||
};
|
||
|
||
auto appendWorld = [](QVector<float>& dst, const QVector3D& p)
|
||
{
|
||
dst.append(p.x());
|
||
dst.append(p.y());
|
||
dst.append(p.z());
|
||
};
|
||
|
||
auto appendAxisBand = [&](QVector<float>& positions,
|
||
QVector<float>& colors,
|
||
const QVector3D& start,
|
||
const QVector3D& end,
|
||
const QVector3D& color,
|
||
float halfWidth)
|
||
{
|
||
QVector3D side = QVector3D::crossProduct(end - start, camUp);
|
||
if (side.lengthSquared() < 1.0e-12f)
|
||
{
|
||
side = QVector3D::crossProduct(end - start, camRight);
|
||
}
|
||
if (!side.isNull())
|
||
{
|
||
side.normalize();
|
||
side *= halfWidth;
|
||
}
|
||
const QVector3D s0 = start - side;
|
||
const QVector3D s1 = start + side;
|
||
const QVector3D e0 = end - side;
|
||
const QVector3D e1 = end + side;
|
||
const QVector3D quad[6] = {s0, e0, e1, s0, e1, s1};
|
||
for (const QVector3D& p : quad)
|
||
{
|
||
appendWorld(positions, p);
|
||
appendWorld(colors, color);
|
||
}
|
||
};
|
||
|
||
for (const auto& pose : m_posePoints)
|
||
{
|
||
QMatrix4x4 poseLocal;
|
||
poseLocal.setToIdentity();
|
||
poseLocal.translate(pose.x, pose.y, pose.z);
|
||
applyEulerRotation(poseLocal, pose);
|
||
|
||
const float axisLength = qMax(1.0f, pose.scale);
|
||
const QVector3D origin = poseLocal.map(QVector3D(0.0f, 0.0f, 0.0f));
|
||
const QVector3D xEnd = poseLocal.map(QVector3D(axisLength, 0.0f, 0.0f));
|
||
const QVector3D yEnd = poseLocal.map(QVector3D(0.0f, axisLength, 0.0f));
|
||
const QVector3D zEnd = poseLocal.map(QVector3D(0.0f, 0.0f, axisLength));
|
||
const float halfWidth = qMax(axisLength * 0.02f, pixelWorld * 2.0f);
|
||
|
||
QVector<float> bandPositions;
|
||
QVector<float> bandColors;
|
||
bandPositions.reserve(54);
|
||
bandColors.reserve(54);
|
||
appendAxisBand(bandPositions, bandColors, origin, xEnd, QVector3D(1.0f, 0.0f, 0.0f), halfWidth);
|
||
appendAxisBand(bandPositions, bandColors, origin, yEnd, QVector3D(0.0f, 1.0f, 0.0f), halfWidth);
|
||
appendAxisBand(bandPositions, bandColors, origin, zEnd, QVector3D(0.0f, 0.0f, 1.0f), halfWidth);
|
||
|
||
// 关闭深度测试，避免姿态轴被周围点云挡住。
|
||
drawPrimitives(m_mvp,
|
||
bandPositions.constData(),
|
||
bandPositions.size() / 3,
|
||
GL_TRIANGLES,
|
||
bandColors.constData(),
|
||
QVector3D(1.0f, 1.0f, 1.0f),
|
||
1.0f,
|
||
1.0f,
|
||
false);
|
||
|
||
const float tipPositions[] = {
|
||
origin.x(), origin.y(), origin.z(),
|
||
xEnd.x(), xEnd.y(), xEnd.z(),
|
||
yEnd.x(), yEnd.y(), yEnd.z(),
|
||
zEnd.x(), zEnd.y(), zEnd.z()
|
||
};
|
||
const float tipColors[] = {
|
||
1.0f, 1.0f, 1.0f,
|
||
1.0f, 0.0f, 0.0f,
|
||
0.0f, 1.0f, 0.0f,
|
||
0.0f, 0.0f, 1.0f
|
||
};
|
||
drawPrimitives(m_mvp, tipPositions, 4, GL_POINTS, tipColors,
|
||
QVector3D(1.0f, 1.0f, 1.0f), 10.0f, 1.0f, false);
|
||
}
|
||
}
|