1277 lines
40 KiB
C++
1277 lines
40 KiB
C++
#include "PointCloudGLWidget.h"
|
||
#include <QDebug>
|
||
#include <QtMath>
|
||
#include <cfloat>
|
||
#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)
|
||
: QOpenGLWidget(parent)
|
||
, m_distance(100.0f)
|
||
, m_rotationX(0.0f)
|
||
, m_rotationY(0.0f)
|
||
, m_rotationZ(0.0f)
|
||
, 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)
|
||
, m_lineSelectMode(LineSelectMode::Vertical)
|
||
, m_eulerRotationOrder(EulerRotationOrder::ZYX)
|
||
, m_measureDistanceEnabled(false)
|
||
, m_hasListHighlightPoint(false)
|
||
, m_colorIndex(0)
|
||
{
|
||
setFocusPolicy(Qt::StrongFocus);
|
||
setMouseTracking(true);
|
||
|
||
// 确保 m_model 是单位矩阵
|
||
m_model.setToIdentity();
|
||
}
|
||
|
||
PointCloudGLWidget::~PointCloudGLWidget()
|
||
{
|
||
// 在 GL 上下文中释放所有 VBO
|
||
makeCurrent();
|
||
for (auto& cloudData : m_pointClouds) {
|
||
releaseVBO(cloudData);
|
||
}
|
||
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.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::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);
|
||
glEnable(GL_POINT_SMOOTH);
|
||
glHint(GL_POINT_SMOOTH_HINT, GL_NICEST);
|
||
}
|
||
|
||
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);
|
||
m_projection.perspective(45.0f, aspect, 0.1f, 10000.0f);
|
||
}
|
||
|
||
void PointCloudGLWidget::paintGL()
|
||
{
|
||
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
|
||
|
||
m_view.setToIdentity();
|
||
// 平移在相机空间中进行，使鼠标拖动方向与屏幕方向一致
|
||
m_view.translate(-m_pan.x(), -m_pan.y(), -m_distance);
|
||
|
||
// 使用轨迹球旋转方式：先Y后X后Z，这样更直观
|
||
m_view.rotate(m_rotationY, 0, 1, 0);
|
||
m_view.rotate(m_rotationX, 1, 0, 0);
|
||
m_view.rotate(m_rotationZ, 0, 0, 1);
|
||
|
||
m_view.translate(-m_center);
|
||
|
||
// 调试输出：每100帧输出一次当前旋转角度
|
||
static int frameCount = 0;
|
||
if (++frameCount % 100 == 0) {
|
||
LOG_INFO("[CloudView] Current rotation: rotX=%.1f, rotY=%.1f, rotZ=%.1f\n",
|
||
m_rotationX, m_rotationY, m_rotationZ);
|
||
}
|
||
|
||
glMatrixMode(GL_PROJECTION);
|
||
glLoadMatrixf(m_projection.constData());
|
||
|
||
glMatrixMode(GL_MODELVIEW);
|
||
QMatrix4x4 modelView = m_view * m_model;
|
||
glLoadMatrixf(modelView.constData());
|
||
|
||
// 确保颜色数组禁用
|
||
glDisableClientState(GL_COLOR_ARRAY);
|
||
|
||
glPointSize(m_pointSize);
|
||
|
||
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);
|
||
}
|
||
|
||
glEnableClientState(GL_VERTEX_ARRAY);
|
||
|
||
if (cloudData.vboCreated && cloudData.vertexBuffer.isCreated()) {
|
||
// 使用 VBO 渲染：数据已在 GPU 显存中，无需每帧 CPU→GPU 拷贝
|
||
cloudData.vertexBuffer.bind();
|
||
glVertexPointer(3, GL_FLOAT, 0, nullptr);
|
||
cloudData.vertexBuffer.release();
|
||
} else {
|
||
// 回退到 CPU 指针（VBO 创建失败时）
|
||
glVertexPointer(3, GL_FLOAT, 0, cloudData.vertices.data());
|
||
}
|
||
|
||
// 如果有原始颜色且选择显示原始颜色
|
||
if (cloudData.hasColor && !cloudData.colors.empty()) {
|
||
glEnableClientState(GL_COLOR_ARRAY);
|
||
if (cloudData.vboCreated && cloudData.colorBuffer.isCreated()) {
|
||
cloudData.colorBuffer.bind();
|
||
glColorPointer(3, GL_FLOAT, 0, nullptr);
|
||
cloudData.colorBuffer.release();
|
||
} else {
|
||
glColorPointer(3, GL_FLOAT, 0, cloudData.colors.data());
|
||
}
|
||
} else {
|
||
// 使用点云自己的颜色索引
|
||
glDisableClientState(GL_COLOR_ARRAY);
|
||
setColorByIndex(cloudData.colorIndex);
|
||
}
|
||
|
||
glDrawArrays(GL_POINTS, 0, static_cast<GLsizei>(cloudData.vertices.size() / 3));
|
||
|
||
glDisableClientState(GL_VERTEX_ARRAY);
|
||
glDisableClientState(GL_COLOR_ARRAY);
|
||
}
|
||
|
||
drawSelectedPoints();
|
||
drawMeasurementLine();
|
||
drawSelectedLine();
|
||
drawLineSegments();
|
||
drawPosePoints();
|
||
|
||
// 最后绘制坐标系指示器（覆盖在所有内容之上）
|
||
drawAxis();
|
||
}
|
||
|
||
void PointCloudGLWidget::addPointCloud(const PointCloudXYZ& cloud, const QString& name)
|
||
{
|
||
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;
|
||
for (size_t i = 0; i < cloud.points.size(); ++i) {
|
||
const auto& pt = cloud.points[i];
|
||
if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
|
||
continue;
|
||
}
|
||
// 显示时过滤 (0,0,0) 点
|
||
if (std::fabs(pt.x) < EPSILON && std::fabs(pt.y) < EPSILON && std::fabs(pt.z) < EPSILON) {
|
||
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));
|
||
|
||
// 保存线索引
|
||
if (data.hasLineInfo && i < cloud.lineIndices.size()) {
|
||
int lineIdx = cloud.lineIndices[i];
|
||
data.lineIndices.push_back(lineIdx);
|
||
if (lineIdx + 1 > data.totalLines) {
|
||
data.totalLines = lineIdx + 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 计算每线点数（假设网格化点云）
|
||
if (data.totalLines > 0) {
|
||
data.pointsPerLine = static_cast<int>(cloud.size()) / data.totalLines;
|
||
}
|
||
|
||
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);
|
||
|
||
resetView();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::addPointCloud(const PointCloudXYZRGB& cloud, const QString& name)
|
||
{
|
||
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;
|
||
|
||
for (size_t i = 0; i < cloud.points.size(); ++i) {
|
||
const auto& pt = cloud.points[i];
|
||
if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
|
||
continue;
|
||
}
|
||
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));
|
||
|
||
// 保存线索引
|
||
if (data.hasLineInfo && i < cloud.lineIndices.size()) {
|
||
int lineIdx = cloud.lineIndices[i];
|
||
data.lineIndices.push_back(lineIdx);
|
||
if (lineIdx + 1 > data.totalLines) {
|
||
data.totalLines = lineIdx + 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 计算每线点数
|
||
if (data.totalLines > 0) {
|
||
data.pointsPerLine = static_cast<int>(cloud.size()) / data.totalLines;
|
||
}
|
||
|
||
m_pointClouds.push_back(std::move(data));
|
||
|
||
// 上传 VBO
|
||
makeCurrent();
|
||
uploadToVBO(m_pointClouds.back());
|
||
doneCurrent();
|
||
|
||
computeBoundingBox();
|
||
resetView();
|
||
update();
|
||
}
|
||
|
||
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_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();
|
||
}
|
||
|
||
void PointCloudGLWidget::setPointCloudColor(PointCloudColor color)
|
||
{
|
||
m_currentColor = color;
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::setPointSize(float size)
|
||
{
|
||
m_pointSize = qBound(1.0f, size, 10.0f);
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::resetView()
|
||
{
|
||
QVector3D size = m_maxBound - m_minBound;
|
||
float maxSize = qMax(qMax(size.x(), size.y()), size.z());
|
||
|
||
// 确保最小视距
|
||
m_distance = qMax(maxSize * 2.0f, 10.0f);
|
||
// 默认视角：右手坐标系，X轴朝右，Y轴朝上，Z轴朝向观察者
|
||
m_rotationX = 0.0f;
|
||
m_rotationY = 0.0f;
|
||
m_rotationZ = 0.0f;
|
||
m_pan = QVector3D(0, 0, 0);
|
||
|
||
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::setViewAngles(float rotX, float rotY, float rotZ)
|
||
{
|
||
m_rotationX = rotX;
|
||
m_rotationY = rotY;
|
||
m_rotationZ = rotZ;
|
||
m_pan = QVector3D(0, 0, 0);
|
||
|
||
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;
|
||
}
|
||
|
||
// 设置选中的横向线
|
||
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);
|
||
}
|
||
|
||
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.originalIndices.size(); ++i) {
|
||
int originalIdx = cloudData.originalIndices[i];
|
||
if (originalIdx % cloudData.pointsPerLine == 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();
|
||
resetView();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::computeBoundingBox()
|
||
{
|
||
if (m_pointClouds.empty()) {
|
||
m_minBound = QVector3D(-50, -50, -50);
|
||
m_maxBound = QVector3D(50, 50, 50);
|
||
m_center = QVector3D(0, 0, 0);
|
||
return;
|
||
}
|
||
|
||
float minX = FLT_MAX, minY = FLT_MAX, minZ = FLT_MAX;
|
||
float maxX = -FLT_MAX, maxY = -FLT_MAX, maxZ = -FLT_MAX;
|
||
|
||
for (const auto& cloudData : m_pointClouds) {
|
||
for (size_t i = 0; i < cloudData.vertices.size(); i += 3) {
|
||
float x = cloudData.vertices[i];
|
||
float y = cloudData.vertices[i + 1];
|
||
float z = cloudData.vertices[i + 2];
|
||
|
||
minX = qMin(minX, x);
|
||
minY = qMin(minY, y);
|
||
minZ = qMin(minZ, z);
|
||
maxX = qMax(maxX, x);
|
||
maxY = qMax(maxY, y);
|
||
maxZ = qMax(maxZ, z);
|
||
}
|
||
}
|
||
|
||
m_minBound = QVector3D(minX, minY, minZ);
|
||
m_maxBound = QVector3D(maxX, maxY, maxZ);
|
||
m_center = (m_minBound + m_maxBound) / 2.0f;
|
||
}
|
||
|
||
void PointCloudGLWidget::setCurrentColor(PointCloudColor color)
|
||
{
|
||
switch (color) {
|
||
case PointCloudColor::White: glColor3f(1.0f, 1.0f, 1.0f); break;
|
||
case PointCloudColor::Red: glColor3f(1.0f, 0.0f, 0.0f); break;
|
||
case PointCloudColor::Green: glColor3f(0.0f, 1.0f, 0.0f); break;
|
||
case PointCloudColor::Blue: glColor3f(0.0f, 0.0f, 1.0f); break;
|
||
case PointCloudColor::Yellow: glColor3f(1.0f, 1.0f, 0.0f); break;
|
||
case PointCloudColor::Cyan: glColor3f(0.0f, 1.0f, 1.0f); break;
|
||
case PointCloudColor::Magenta: glColor3f(1.0f, 0.0f, 1.0f); break;
|
||
default: glColor3f(1.0f, 1.0f, 1.0f); break;
|
||
}
|
||
}
|
||
|
||
void PointCloudGLWidget::setColorByIndex(int colorIndex)
|
||
{
|
||
// 颜色表：浅灰、浅绿、浅蓝、黄、青、品红、白
|
||
static const float colorTable[7][3] = {
|
||
{0.75f, 0.75f, 0.75f}, // 浅灰色
|
||
{0.3f, 1.0f, 0.3f}, // 浅绿
|
||
{0.3f, 0.3f, 1.0f}, // 浅蓝
|
||
{1.0f, 1.0f, 0.3f}, // 黄
|
||
{0.3f, 1.0f, 1.0f}, // 青
|
||
{1.0f, 0.3f, 1.0f}, // 品红
|
||
{1.0f, 1.0f, 1.0f}, // 白
|
||
};
|
||
|
||
int idx = colorIndex % COLOR_COUNT;
|
||
glColor3f(colorTable[idx][0], colorTable[idx][1], colorTable[idx][2]);
|
||
}
|
||
|
||
SelectedPointInfo PointCloudGLWidget::pickPoint(int screenX, int screenY)
|
||
{
|
||
SelectedPointInfo result;
|
||
|
||
if (m_pointClouds.empty()) {
|
||
return result;
|
||
}
|
||
|
||
// 获取视口和矩阵
|
||
GLint viewport[4];
|
||
GLdouble modelMatrix[16], projMatrix[16];
|
||
|
||
glGetIntegerv(GL_VIEWPORT, viewport);
|
||
glGetDoublev(GL_MODELVIEW_MATRIX, modelMatrix);
|
||
glGetDoublev(GL_PROJECTION_MATRIX, projMatrix);
|
||
|
||
// 转换屏幕 Y 坐标（OpenGL Y 轴向上）
|
||
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) {
|
||
float wx = cloudData.vertices[i];
|
||
float wy = cloudData.vertices[i + 1];
|
||
float wz = cloudData.vertices[i + 2];
|
||
|
||
// 将世界坐标投影到屏幕
|
||
GLdouble sx, sy, sz;
|
||
gluProject(wx, wy, wz, modelMatrix, projMatrix, viewport, &sx, &sy, &sz);
|
||
|
||
// 计算屏幕距离
|
||
float dx = static_cast<float>(sx) - screenX;
|
||
float dy = static_cast<float>(sy) - glScreenY;
|
||
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.originalIndices.size() && cloudData.pointsPerLine > 0) {
|
||
int originalIdx = cloudData.originalIndices[bestIndex];
|
||
result.pointIndexInLine = originalIdx % cloudData.pointsPerLine;
|
||
}
|
||
}
|
||
|
||
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()
|
||
{
|
||
glPointSize(10.0f);
|
||
glDisable(GL_DEPTH_TEST);
|
||
|
||
glBegin(GL_POINTS);
|
||
|
||
// 绘制选中的点（橙色）
|
||
for (const auto& pt : m_selectedPoints) {
|
||
if (pt.valid) {
|
||
glColor3f(1.0f, 0.5f, 0.0f); // 橙色
|
||
glVertex3f(pt.x, pt.y, pt.z);
|
||
}
|
||
}
|
||
|
||
// 绘制列表高亮点（蓝色，与选点区分）
|
||
if (m_hasListHighlightPoint) {
|
||
glColor3f(0.0f, 0.5f, 1.0f); // 蓝色
|
||
glVertex3f(m_listHighlightPoint.x(), m_listHighlightPoint.y(), m_listHighlightPoint.z());
|
||
}
|
||
|
||
glEnd();
|
||
|
||
glEnable(GL_DEPTH_TEST);
|
||
glPointSize(m_pointSize);
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
glDisable(GL_DEPTH_TEST);
|
||
glLineWidth(2.0f);
|
||
|
||
glBegin(GL_LINES);
|
||
glColor3f(0.0f, 1.0f, 0.0f);
|
||
glVertex3f(p1.x, p1.y, p1.z);
|
||
glVertex3f(p2.x, p2.y, p2.z);
|
||
glEnd();
|
||
|
||
glLineWidth(1.0f);
|
||
glEnable(GL_DEPTH_TEST);
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
// 高亮显示选中线上的所有点
|
||
glPointSize(3.0f);
|
||
glDisable(GL_DEPTH_TEST);
|
||
|
||
glBegin(GL_POINTS);
|
||
glColor3f(1.0f, 1.0f, 0.0f); // 黄色高亮
|
||
|
||
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()) {
|
||
glVertex3f(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.originalIndices.size(); ++i) {
|
||
int originalIdx = cloudData.originalIndices[i];
|
||
// 原始索引 % 每线点数 == 选中的点索引
|
||
if (originalIdx % cloudData.pointsPerLine == m_selectedLine.pointIndex) {
|
||
size_t vertIdx = i * 3;
|
||
if (vertIdx + 2 < cloudData.vertices.size()) {
|
||
glVertex3f(cloudData.vertices[vertIdx],
|
||
cloudData.vertices[vertIdx + 1],
|
||
cloudData.vertices[vertIdx + 2]);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
glEnd();
|
||
|
||
glEnable(GL_DEPTH_TEST);
|
||
glPointSize(m_pointSize);
|
||
}
|
||
|
||
void PointCloudGLWidget::drawAxis()
|
||
{
|
||
// 在右下角绘制坐标系指示器（右手坐标系）
|
||
// 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 轴向上）
|
||
|
||
// 保存当前矩阵状态
|
||
glMatrixMode(GL_PROJECTION);
|
||
glPushMatrix();
|
||
glLoadIdentity();
|
||
// 设置正交投影，覆盖整个视口
|
||
glOrtho(0, viewport[2], 0, viewport[3], -100, 100);
|
||
|
||
glMatrixMode(GL_MODELVIEW);
|
||
glPushMatrix();
|
||
glLoadIdentity();
|
||
|
||
// 移动到右下角中心位置
|
||
glTranslatef(axisX + axisSize / 2.0f, axisY + axisSize / 2.0f, 0.0f);
|
||
|
||
// 应用当前视图的旋转（与主视图同步）
|
||
glRotatef(m_rotationY, 0, 1, 0);
|
||
glRotatef(m_rotationX, 1, 0, 0);
|
||
glRotatef(m_rotationZ, 0, 0, 1);
|
||
|
||
// 关闭深度测试，确保坐标系始终可见
|
||
glDisable(GL_DEPTH_TEST);
|
||
|
||
float axisLength = axisSize * 0.4f; // 坐标轴长度
|
||
glLineWidth(2.0f);
|
||
glBegin(GL_LINES);
|
||
|
||
// X 轴 - 红色（右手坐标系：向右）
|
||
glColor3f(1.0f, 0.0f, 0.0f);
|
||
glVertex3f(0.0f, 0.0f, 0.0f);
|
||
glVertex3f(axisLength, 0.0f, 0.0f);
|
||
|
||
// Y 轴 - 绿色（右手坐标系：向上）
|
||
glColor3f(0.0f, 1.0f, 0.0f);
|
||
glVertex3f(0.0f, 0.0f, 0.0f);
|
||
glVertex3f(0.0f, axisLength, 0.0f);
|
||
|
||
// Z 轴 - 蓝色（右手坐标系：向观察者）
|
||
glColor3f(0.0f, 0.0f, 1.0f);
|
||
glVertex3f(0.0f, 0.0f, 0.0f);
|
||
glVertex3f(0.0f, 0.0f, axisLength);
|
||
|
||
glEnd();
|
||
glLineWidth(1.0f);
|
||
|
||
// 恢复深度测试
|
||
glEnable(GL_DEPTH_TEST);
|
||
|
||
// 恢复矩阵状态
|
||
glPopMatrix();
|
||
glMatrixMode(GL_PROJECTION);
|
||
glPopMatrix();
|
||
glMatrixMode(GL_MODELVIEW);
|
||
}
|
||
|
||
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) {
|
||
float distance = calculateDistance(m_selectedPoints[0], m_selectedPoints[1]);
|
||
emit twoPointsSelected(m_selectedPoints[0], m_selectedPoints[1], distance);
|
||
}
|
||
} 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::mouseMoveEvent(QMouseEvent* event)
|
||
{
|
||
QPoint delta = event->pos() - m_lastMousePos;
|
||
m_lastMousePos = event->pos();
|
||
|
||
if (m_leftButtonPressed) {
|
||
// Alt+左键拖动：旋转rotZ（滚转）
|
||
if (event->modifiers() & Qt::AltModifier) {
|
||
m_rotationZ += delta.x() * 0.5f;
|
||
} else {
|
||
// 普通左键拖动：旋转rotX和rotY
|
||
m_rotationY += delta.x() * 0.5f;
|
||
m_rotationX += delta.y() * 0.5f;
|
||
}
|
||
// 移除角度限制，允许无限旋转
|
||
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) {
|
||
m_rotationZ += delta.x() * 0.5f;
|
||
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(0.1f, m_distance, 10000.0f);
|
||
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];
|
||
}
|
||
cloud.push_back(pt, lineIdx);
|
||
}
|
||
|
||
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;
|
||
for (size_t i = 0; i < cloud.points.size(); ++i) {
|
||
const auto& pt = cloud.points[i];
|
||
if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
|
||
continue;
|
||
}
|
||
// 显示时过滤 (0,0,0) 点
|
||
if (std::fabs(pt.x) < EPSILON && std::fabs(pt.y) < EPSILON && std::fabs(pt.z) < EPSILON) {
|
||
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));
|
||
|
||
if (data.hasLineInfo && i < cloud.lineIndices.size()) {
|
||
int lineIdx = cloud.lineIndices[i];
|
||
data.lineIndices.push_back(lineIdx);
|
||
if (lineIdx + 1 > data.totalLines) {
|
||
data.totalLines = lineIdx + 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 计算每线点数
|
||
if (data.totalLines > 0) {
|
||
data.pointsPerLine = static_cast<int>(cloud.size()) / data.totalLines;
|
||
}
|
||
|
||
// 释放旧的 VBO
|
||
makeCurrent();
|
||
releaseVBO(m_pointClouds[0]);
|
||
|
||
m_pointClouds[0] = std::move(data);
|
||
|
||
// 上传新的 VBO
|
||
uploadToVBO(m_pointClouds[0]);
|
||
doneCurrent();
|
||
|
||
computeBoundingBox();
|
||
resetView();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::addLineSegments(const QVector<LineSegment>& segments)
|
||
{
|
||
m_lineSegments.append(segments);
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::clearLineSegments()
|
||
{
|
||
m_lineSegments.clear();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::addPosePoints(const QVector<PosePoint>& poses)
|
||
{
|
||
m_posePoints.append(poses);
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::clearPosePoints()
|
||
{
|
||
m_posePoints.clear();
|
||
update();
|
||
}
|
||
|
||
void PointCloudGLWidget::drawLineSegments()
|
||
{
|
||
if (m_lineSegments.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
glLineWidth(2.0f);
|
||
glBegin(GL_LINES);
|
||
|
||
for (const auto& seg : m_lineSegments) {
|
||
glColor3f(seg.r, seg.g, seg.b);
|
||
glVertex3f(seg.x1, seg.y1, seg.z1);
|
||
glVertex3f(seg.x2, seg.y2, seg.z2);
|
||
}
|
||
|
||
glEnd();
|
||
glLineWidth(1.0f);
|
||
}
|
||
|
||
void PointCloudGLWidget::drawPosePoints()
|
||
{
|
||
if (m_posePoints.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
glLineWidth(2.0f);
|
||
|
||
for (const auto& pose : m_posePoints) {
|
||
glPushMatrix();
|
||
|
||
// 移动到姿态点位置
|
||
glTranslatef(pose.x, pose.y, pose.z);
|
||
|
||
// 根据选择的欧拉角旋转顺序应用旋转
|
||
// OpenGL的glRotatef是右乘，所以需要反向写
|
||
switch (m_eulerRotationOrder) {
|
||
case EulerRotationOrder::ZYX: // Yaw-Pitch-Roll（最常用）
|
||
glRotatef(pose.rx, 1, 0, 0); // X轴旋转（最后应用）
|
||
glRotatef(pose.ry, 0, 1, 0); // Y轴旋转（第二个应用）
|
||
glRotatef(pose.rz, 0, 0, 1); // Z轴旋转（第一个应用）
|
||
break;
|
||
case EulerRotationOrder::XYZ: // Roll-Pitch-Yaw
|
||
glRotatef(pose.rz, 0, 0, 1);
|
||
glRotatef(pose.ry, 0, 1, 0);
|
||
glRotatef(pose.rx, 1, 0, 0);
|
||
break;
|
||
case EulerRotationOrder::ZXY: // Yaw-Roll-Pitch
|
||
glRotatef(pose.ry, 0, 1, 0);
|
||
glRotatef(pose.rx, 1, 0, 0);
|
||
glRotatef(pose.rz, 0, 0, 1);
|
||
break;
|
||
case EulerRotationOrder::YXZ: // Pitch-Roll-Yaw
|
||
glRotatef(pose.rz, 0, 0, 1);
|
||
glRotatef(pose.rx, 1, 0, 0);
|
||
glRotatef(pose.ry, 0, 1, 0);
|
||
break;
|
||
case EulerRotationOrder::XZY: // Roll-Yaw-Pitch
|
||
glRotatef(pose.ry, 0, 1, 0);
|
||
glRotatef(pose.rz, 0, 0, 1);
|
||
glRotatef(pose.rx, 1, 0, 0);
|
||
break;
|
||
case EulerRotationOrder::YZX: // Pitch-Yaw-Roll
|
||
glRotatef(pose.rx, 1, 0, 0);
|
||
glRotatef(pose.rz, 0, 0, 1);
|
||
glRotatef(pose.ry, 0, 1, 0);
|
||
break;
|
||
}
|
||
|
||
// 绘制坐标系（右手坐标系：X红右，Y绿上，Z蓝前）
|
||
glBegin(GL_LINES);
|
||
|
||
// X轴 - 红色
|
||
glColor3f(1.0f, 0.0f, 0.0f);
|
||
glVertex3f(0.0f, 0.0f, 0.0f);
|
||
glVertex3f(pose.scale, 0.0f, 0.0f);
|
||
|
||
// Y轴 - 绿色
|
||
glColor3f(0.0f, 1.0f, 0.0f);
|
||
glVertex3f(0.0f, 0.0f, 0.0f);
|
||
glVertex3f(0.0f, pose.scale, 0.0f);
|
||
|
||
// Z轴 - 蓝色
|
||
glColor3f(0.0f, 0.0f, 1.0f);
|
||
glVertex3f(0.0f, 0.0f, 0.0f);
|
||
glVertex3f(0.0f, 0.0f, pose.scale);
|
||
|
||
glEnd();
|
||
|
||
glPopMatrix();
|
||
}
|
||
|
||
glLineWidth(1.0f);
|
||
}
|