353 lines
10 KiB
C++
353 lines
10 KiB
C++
#include "gui/PointCloudGLWidget.h"
|
||
#include <QDebug>
|
||
#include <QPushButton>
|
||
#include <cmath>
|
||
#include <cfloat>
|
||
|
||
PointCloudGLWidget::PointCloudGLWidget(QWidget *parent)
|
||
: QOpenGLWidget(parent)
|
||
, m_program(nullptr)
|
||
, m_vertexBuffer(nullptr)
|
||
, m_vao(nullptr)
|
||
, m_pointCount(0)
|
||
, m_minZ(0.0f)
|
||
, m_maxZ(1000.0f)
|
||
, m_fov(60.0f) // 透视投影视场角
|
||
, m_rotationX(0.0f) // 从正面看（0度）
|
||
, m_rotationY(0.0f) // 不旋转Y轴
|
||
, m_cloudCenter(0.0f, 0.0f, 0.0f)
|
||
, m_viewDistance(1000.0f)
|
||
, m_panOffset(0.0f, 0.0f, 0.0f)
|
||
, m_zoom(1.0f) // 缩放因子
|
||
, m_leftButtonPressed(false)
|
||
, m_rightButtonPressed(false)
|
||
, m_firstFrame(true)
|
||
, m_colorMode(0) // 默认黑白模式
|
||
{
|
||
setMinimumSize(400, 400);
|
||
|
||
// 添加重置视角按钮
|
||
QPushButton *resetBtn = new QPushButton("重置", this);
|
||
resetBtn->setFixedSize(60, 30);
|
||
resetBtn->move(10, 10);
|
||
resetBtn->setStyleSheet(
|
||
"QPushButton { background-color: rgba(50, 50, 50, 180); color: white; border: 1px solid #555; border-radius: 4px; }"
|
||
"QPushButton:hover { background-color: rgba(70, 70, 70, 200); }"
|
||
"QPushButton:pressed { background-color: rgba(40, 40, 40, 220); }"
|
||
);
|
||
connect(resetBtn, &QPushButton::clicked, this, &PointCloudGLWidget::resetView);
|
||
}
|
||
|
||
PointCloudGLWidget::~PointCloudGLWidget()
|
||
{
|
||
makeCurrent();
|
||
if (m_vao) {
|
||
m_vao->destroy();
|
||
delete m_vao;
|
||
}
|
||
if (m_vertexBuffer) {
|
||
m_vertexBuffer->destroy();
|
||
delete m_vertexBuffer;
|
||
}
|
||
if (m_program) {
|
||
delete m_program;
|
||
}
|
||
doneCurrent();
|
||
}
|
||
|
||
void PointCloudGLWidget::initializeGL()
|
||
{
|
||
initializeOpenGLFunctions();
|
||
|
||
glClearColor(0.1f, 0.1f, 0.15f, 1.0f); // 深灰色背景
|
||
glEnable(GL_DEPTH_TEST);
|
||
glEnable(GL_PROGRAM_POINT_SIZE);
|
||
|
||
setupShaders();
|
||
|
||
// 创建VAO和VBO
|
||
m_vao = new QOpenGLVertexArrayObject(this);
|
||
m_vao->create();
|
||
|
||
m_vertexBuffer = new QOpenGLBuffer(QOpenGLBuffer::VertexBuffer);
|
||
m_vertexBuffer->create();
|
||
|
||
qDebug() << "[PointCloudGLWidget] OpenGL initialized";
|
||
}
|
||
|
||
void PointCloudGLWidget::setupShaders()
|
||
{
|
||
m_program = new QOpenGLShaderProgram(this);
|
||
|
||
// 顶点着色器
|
||
const char *vertexShaderSource = R"(
|
||
#version 330 core
|
||
layout(location = 0) in vec3 position;
|
||
uniform mat4 mvp;
|
||
uniform float minZ;
|
||
uniform float maxZ;
|
||
out float depth;
|
||
void main() {
|
||
gl_Position = mvp * vec4(position, 1.0);
|
||
gl_PointSize = 1.0;
|
||
depth = (position.z - minZ) / (maxZ - minZ);
|
||
}
|
||
)";
|
||
|
||
// 片段着色器 - 支持黑白和彩色两种模式
|
||
const char *fragmentShaderSource = R"(
|
||
#version 330 core
|
||
in float depth;
|
||
uniform int colorMode;
|
||
out vec4 fragColor;
|
||
void main() {
|
||
float d = clamp(depth, 0.0, 1.0);
|
||
if (colorMode == 0) {
|
||
fragColor = vec4(1.0, 1.0, 1.0, 1.0);
|
||
} else {
|
||
vec3 color;
|
||
if (d < 0.25) {
|
||
color = mix(vec3(0.0, 0.0, 1.0), vec3(0.0, 1.0, 1.0), d * 4.0);
|
||
} else if (d < 0.5) {
|
||
color = mix(vec3(0.0, 1.0, 1.0), vec3(0.0, 1.0, 0.0), (d - 0.25) * 4.0);
|
||
} else if (d < 0.75) {
|
||
color = mix(vec3(0.0, 1.0, 0.0), vec3(1.0, 1.0, 0.0), (d - 0.5) * 4.0);
|
||
} else {
|
||
color = mix(vec3(1.0, 1.0, 0.0), vec3(1.0, 0.0, 0.0), (d - 0.75) * 4.0);
|
||
}
|
||
fragColor = vec4(color, 1.0);
|
||
}
|
||
}
|
||
)";
|
||
|
||
if (!m_program->addShaderFromSourceCode(QOpenGLShader::Vertex, vertexShaderSource)) {
|
||
qDebug() << "[PointCloudGLWidget] Vertex shader error:" << m_program->log();
|
||
}
|
||
if (!m_program->addShaderFromSourceCode(QOpenGLShader::Fragment, fragmentShaderSource)) {
|
||
qDebug() << "[PointCloudGLWidget] Fragment shader error:" << m_program->log();
|
||
}
|
||
if (!m_program->link()) {
|
||
qDebug() << "[PointCloudGLWidget] Shader link error:" << m_program->log();
|
||
} else {
|
||
qDebug() << "[PointCloudGLWidget] Shaders compiled and linked successfully";
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::resizeGL(int w, int h)
|
||
{
|
||
m_projection.setToIdentity();
|
||
|
||
// 使用正交投影，避免透视变形
|
||
float aspect = float(w) / float(h);
|
||
float orthoSize = m_viewDistance * 0.5f / m_zoom;
|
||
m_projection.ortho(-orthoSize * aspect, orthoSize * aspect,
|
||
-orthoSize, orthoSize,
|
||
-50000.0f, 50000.0f);
|
||
}
|
||
|
||
void PointCloudGLWidget::paintGL()
|
||
{
|
||
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
|
||
|
||
if (m_pointCount == 0 || !m_program) {
|
||
return;
|
||
}
|
||
|
||
// 重新计算正交投影矩阵
|
||
m_projection.setToIdentity();
|
||
float aspect = float(width()) / float(height());
|
||
float orthoSize = m_viewDistance * 0.5f / m_zoom;
|
||
m_projection.ortho(-orthoSize * aspect, orthoSize * aspect,
|
||
-orthoSize, orthoSize,
|
||
-50000.0f, 50000.0f);
|
||
|
||
// 设置view矩阵 - 轨道相机模式（围绕点云中心旋转）
|
||
m_view.setToIdentity();
|
||
// 1. 用户平移偏移
|
||
m_view.translate(m_panOffset);
|
||
// 2. 相机后退到观察距离
|
||
m_view.translate(0.0f, 0.0f, -m_viewDistance);
|
||
// 3. 应用旋转（围绕原点，即点云中心）
|
||
m_view.rotate(m_rotationX, 1.0f, 0.0f, 0.0f);
|
||
m_view.rotate(m_rotationY, 0.0f, 1.0f, 0.0f);
|
||
// 4. 将点云中心移到原点
|
||
m_view.translate(-m_cloudCenter);
|
||
|
||
// 设置model矩阵
|
||
m_model.setToIdentity();
|
||
|
||
// 计算MVP矩阵
|
||
QMatrix4x4 mvp = m_projection * m_view * m_model;
|
||
|
||
// 绑定shader和设置uniform
|
||
m_program->bind();
|
||
m_program->setUniformValue("mvp", mvp);
|
||
m_program->setUniformValue("minZ", m_minZ);
|
||
m_program->setUniformValue("maxZ", m_maxZ);
|
||
m_program->setUniformValue("colorMode", m_colorMode);
|
||
|
||
// 绑定VAO和绘制
|
||
m_vao->bind();
|
||
glDrawArrays(GL_POINTS, 0, m_pointCount);
|
||
m_vao->release();
|
||
|
||
m_program->release();
|
||
}
|
||
|
||
void PointCloudGLWidget::mousePressEvent(QMouseEvent *event)
|
||
{
|
||
m_lastMousePos = event->pos();
|
||
if (event->button() == Qt::LeftButton) {
|
||
m_leftButtonPressed = true;
|
||
} else if (event->button() == Qt::RightButton) {
|
||
m_rightButtonPressed = true;
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::mouseMoveEvent(QMouseEvent *event)
|
||
{
|
||
QPoint delta = event->pos() - m_lastMousePos;
|
||
m_lastMousePos = event->pos();
|
||
|
||
if (m_leftButtonPressed) {
|
||
// 左键：旋转
|
||
m_rotationX += delta.y() * 0.5f;
|
||
m_rotationY += delta.x() * 0.5f;
|
||
update();
|
||
} else if (m_rightButtonPressed) {
|
||
// 右键：平移（根据观察距离调整平移速度）
|
||
float scale = m_viewDistance * 0.002f;
|
||
m_panOffset.setX(m_panOffset.x() + delta.x() * scale);
|
||
m_panOffset.setY(m_panOffset.y() - delta.y() * scale);
|
||
update();
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::mouseReleaseEvent(QMouseEvent *event)
|
||
{
|
||
if (event->button() == Qt::LeftButton) {
|
||
m_leftButtonPressed = false;
|
||
} else if (event->button() == Qt::RightButton) {
|
||
m_rightButtonPressed = false;
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::wheelEvent(QWheelEvent *event)
|
||
{
|
||
// 滚轮：缩放（调整zoom因子）
|
||
float delta = event->angleDelta().y() / 120.0f;
|
||
m_zoom *= (1.0f + delta * 0.1f);
|
||
m_zoom = qMax(0.1f, qMin(m_zoom, 10.0f)); // 范围：0.1-10倍
|
||
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::updatePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
|
||
{
|
||
if (!cloud) {
|
||
return;
|
||
}
|
||
|
||
// 过滤全零点并转换为顶点数组，同时计算包围盒
|
||
m_vertices.clear();
|
||
|
||
float minX = FLT_MAX, maxX = -FLT_MAX;
|
||
float minY = FLT_MAX, maxY = -FLT_MAX;
|
||
float minZ = FLT_MAX, maxZ = -FLT_MAX;
|
||
|
||
for (const auto& point : cloud->points) {
|
||
if (point.z > 0.01f) { // 过滤掉无效的零点
|
||
const float displayZ = -point.z; // Flip front/back axis for viewer convention.
|
||
m_vertices.push_back(point.x);
|
||
m_vertices.push_back(-point.y);
|
||
m_vertices.push_back(displayZ);
|
||
|
||
// 更新包围盒
|
||
if (point.x < minX) minX = point.x;
|
||
if (point.x > maxX) maxX = point.x;
|
||
if (point.y < minY) minY = point.y;
|
||
if (point.y > maxY) maxY = point.y;
|
||
if (displayZ < minZ) minZ = displayZ;
|
||
if (displayZ > maxZ) maxZ = displayZ;
|
||
}
|
||
}
|
||
|
||
m_pointCount = m_vertices.size() / 3;
|
||
|
||
// 保存深度范围用于着色
|
||
if (m_pointCount > 0) {
|
||
m_minZ = minZ;
|
||
m_maxZ = maxZ;
|
||
}
|
||
|
||
// 只在首帧时自动调整相机位置，之后保持用户交互状态
|
||
if (m_pointCount > 0 && m_firstFrame) {
|
||
// 计算点云中心
|
||
float centerX = (minX + maxX) / 2.0f;
|
||
float centerY = (minY + maxY) / 2.0f;
|
||
float centerZ = (minZ + maxZ) / 2.0f;
|
||
|
||
// 计算点云尺寸
|
||
float depthRange = maxZ - minZ;
|
||
float sizeX = maxX - minX;
|
||
float sizeY = maxY - minY;
|
||
float maxSize = std::max({sizeX, sizeY, depthRange});
|
||
|
||
// 设置点云中心
|
||
m_cloudCenter = QVector3D(centerX, centerY, centerZ);
|
||
|
||
// 计算观察距离，让相机从外部观察点云
|
||
m_viewDistance = maxSize * 1.5f;
|
||
|
||
// 重置平移偏移和旋转角度
|
||
m_panOffset = QVector3D(0.0f, 0.0f, 0.0f);
|
||
m_rotationX = 0.0f;
|
||
m_rotationY = 0.0f;
|
||
|
||
// 设置缩放
|
||
m_zoom = 1.0f;
|
||
|
||
qDebug() << "[PointCloudGLWidget] 首帧自动居中 - 点云中心:" << centerX << centerY << centerZ
|
||
<< "观察距离:" << m_viewDistance;
|
||
m_firstFrame = false; // 标记首帧已处理
|
||
}
|
||
|
||
updateBuffers();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::resetView()
|
||
{
|
||
// 重置所有视角参数到初始状态
|
||
m_rotationX = 0.0f;
|
||
m_rotationY = 0.0f;
|
||
m_panOffset = QVector3D(0.0f, 0.0f, 0.0f);
|
||
m_zoom = 1.0f;
|
||
m_firstFrame = true; // 标记为首帧，下次更新时会重新计算视角
|
||
|
||
update();
|
||
qDebug() << "[PointCloudGLWidget] 视角已重置";
|
||
}
|
||
|
||
void PointCloudGLWidget::updateBuffers()
|
||
{
|
||
if (m_vertices.empty() || !m_vao || !m_vertexBuffer) {
|
||
return;
|
||
}
|
||
|
||
makeCurrent();
|
||
|
||
m_vao->bind();
|
||
m_vertexBuffer->bind();
|
||
m_vertexBuffer->allocate(m_vertices.data(), m_vertices.size() * sizeof(float));
|
||
|
||
// 设置顶点属性指针
|
||
glEnableVertexAttribArray(0);
|
||
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(float), nullptr);
|
||
|
||
m_vertexBuffer->release();
|
||
m_vao->release();
|
||
|
||
doneCurrent();
|
||
}
|