修改加载文件;错误码范围;视图显示
This commit is contained in:
parent
47fef439e3
commit
c880096802
@ -3,6 +3,7 @@
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <functional>
|
||||
#include "VZNL_Types.h"
|
||||
#include "VrError.h"
|
||||
|
||||
@ -21,6 +22,15 @@ public:
|
||||
class LaserDataLoader
|
||||
{
|
||||
public:
|
||||
// lineData is valid only while the callback is running. Copy it to retain it.
|
||||
using LaserLineLoadCallback =
|
||||
std::function<void(EVzResultDataType, SVzLaserLineData&)>;
|
||||
|
||||
// Called exactly once when loading succeeds or fails.
|
||||
// Arguments: result, lineNum, scanSpeed, maxTimeStamp, clockPerSecond.
|
||||
using LaserLoadFinishedCallback =
|
||||
std::function<void(int, int, float, int, int)>;
|
||||
|
||||
LaserDataLoader();
|
||||
~LaserDataLoader();
|
||||
|
||||
@ -31,6 +41,12 @@ public:
|
||||
float& scanSpeed,
|
||||
int& maxTimeStamp,
|
||||
int& clockPerSecond);
|
||||
|
||||
// Loads one scan line at a time and immediately invokes lineCallback.
|
||||
// finishedCallback is invoked after all lines are read or an error occurs.
|
||||
int LoadLaserScanDataByCallback(const std::string& fileName,
|
||||
const LaserLineLoadCallback& lineCallback,
|
||||
const LaserLoadFinishedCallback& finishedCallback);
|
||||
|
||||
// 保存激光扫描数据到文件 - 统一接口，支持两种类型的数据
|
||||
int SaveLaserScanData(const std::string& fileName,
|
||||
@ -98,6 +114,11 @@ private:
|
||||
int& maxTimeStamp,
|
||||
int& clockPerSecond);
|
||||
|
||||
int _LoadLaserScanDataFromDatFileByCallback(
|
||||
const std::string& fileName,
|
||||
const LaserLineLoadCallback& lineCallback,
|
||||
const LaserLoadFinishedCallback& finishedCallback);
|
||||
|
||||
std::string m_lastError;
|
||||
static const int VZ_LASER_LINE_PT_MAX_NUM = 4096;
|
||||
};
|
||||
|
||||
@ -308,6 +308,198 @@ int LaserDataLoader::LoadLaserScanData(const std::string& fileName,
|
||||
inputFile.close();
|
||||
LOG_INFO("Successfully loaded %d laser scan lines from file: %s\n", lineNum, fileName.c_str());
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
int LaserDataLoader::LoadLaserScanDataByCallback(
|
||||
const std::string& fileName,
|
||||
const LaserLineLoadCallback& lineCallback,
|
||||
const LaserLoadFinishedCallback& finishedCallback)
|
||||
{
|
||||
LOG_INFO("Loading from file by callback: %s\n", fileName.c_str());
|
||||
m_lastError.clear();
|
||||
|
||||
int lineNum = 0;
|
||||
float scanSpeed = 0.0f;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
|
||||
auto finish = [&](int result) -> int {
|
||||
if (finishedCallback) {
|
||||
finishedCallback(result, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
|
||||
}
|
||||
return result;
|
||||
};
|
||||
|
||||
if (HasFileExtension(fileName, ".dat") && !LooksLikeTextLaserFile(fileName)) {
|
||||
return _LoadLaserScanDataFromDatFileByCallback(fileName, lineCallback, finishedCallback);
|
||||
}
|
||||
|
||||
std::ifstream inputFile(fileName);
|
||||
if (!inputFile.is_open()) {
|
||||
m_lastError = "Cannot open file: " + fileName;
|
||||
LOG_ERROR("Cannot open file: %s\n", fileName.c_str());
|
||||
return finish(ERR_CODE(FILE_ERR_NOEXIST));
|
||||
}
|
||||
|
||||
EVzResultDataType dataType = keResultDataType_Invalid;
|
||||
int result = _GetLaserType(fileName, dataType);
|
||||
if (result != SUCCESS) {
|
||||
m_lastError = "Cannot determine laser data type: " + fileName;
|
||||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||||
return finish(result);
|
||||
}
|
||||
|
||||
SVzLaserLineData lineData;
|
||||
std::memset(&lineData, 0, sizeof(lineData));
|
||||
std::unique_ptr<SVzNL3DPosition[]> positionPoints;
|
||||
std::unique_ptr<SVzNL2DPosition[]> position2DPoints;
|
||||
std::unique_ptr<SVzNLPointXYZRGBA[]> rgbaPoints;
|
||||
std::unique_ptr<SVzNL2DLRPoint[]> rgba2DPoints;
|
||||
bool hasLine = false;
|
||||
int pointIndex = 0;
|
||||
|
||||
auto emitLine = [&]() -> int {
|
||||
if (!hasLine) {
|
||||
return SUCCESS;
|
||||
}
|
||||
if (pointIndex != lineData.nPointCount) {
|
||||
std::ostringstream message;
|
||||
message << "Laser line " << lineData.llFrameIdx << " declares "
|
||||
<< lineData.nPointCount << " points but contains " << pointIndex;
|
||||
m_lastError = message.str();
|
||||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||||
return ERR_CODE(FILE_ERR_FORMAT);
|
||||
}
|
||||
if (lineCallback) {
|
||||
lineCallback(dataType, lineData);
|
||||
}
|
||||
positionPoints.reset();
|
||||
position2DPoints.reset();
|
||||
rgbaPoints.reset();
|
||||
rgba2DPoints.reset();
|
||||
std::memset(&lineData, 0, sizeof(lineData));
|
||||
hasLine = false;
|
||||
pointIndex = 0;
|
||||
return SUCCESS;
|
||||
};
|
||||
|
||||
std::string line;
|
||||
while (std::getline(inputFile, line)) {
|
||||
TrimCarriageReturn(line);
|
||||
|
||||
if (line.find("LineNum:") == 0) {
|
||||
sscanf(line.c_str(), "LineNum:%d", &lineNum);
|
||||
} else if (line.find("ScanSpeed:") == 0) {
|
||||
sscanf(line.c_str(), "ScanSpeed:%f", &scanSpeed);
|
||||
} else if (line.find("MaxTimeStamp:") == 0) {
|
||||
sscanf(line.c_str(), "MaxTimeStamp:%d_%d", &maxTimeStamp, &clockPerSecond);
|
||||
} else if (line.find("Line_") == 0) {
|
||||
result = emitLine();
|
||||
if (result != SUCCESS) {
|
||||
return finish(result);
|
||||
}
|
||||
|
||||
int lineIndex = 0;
|
||||
unsigned int timeStamp = 0;
|
||||
int pointCount = 0;
|
||||
if (sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &pointCount) != 3 ||
|
||||
pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) {
|
||||
m_lastError = "Invalid laser line header: " + line;
|
||||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||||
return finish(ERR_CODE(FILE_ERR_FORMAT));
|
||||
}
|
||||
|
||||
lineData.llFrameIdx = lineIndex;
|
||||
lineData.llTimeStamp = timeStamp;
|
||||
lineData.nPointCount = pointCount;
|
||||
|
||||
if (pointCount > 0 && dataType == keResultDataType_PointXYZRGBA) {
|
||||
rgbaPoints.reset(new (std::nothrow) SVzNLPointXYZRGBA[pointCount]);
|
||||
rgba2DPoints.reset(new (std::nothrow) SVzNL2DLRPoint[pointCount]);
|
||||
if (!rgbaPoints || !rgba2DPoints) {
|
||||
m_lastError = "Memory allocation failed while reading: " + fileName;
|
||||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||||
return finish(ERR_CODE(DATA_ERR_MEM));
|
||||
}
|
||||
std::memset(rgbaPoints.get(), 0,
|
||||
sizeof(SVzNLPointXYZRGBA) * static_cast<size_t>(pointCount));
|
||||
std::memset(rgba2DPoints.get(), 0,
|
||||
sizeof(SVzNL2DLRPoint) * static_cast<size_t>(pointCount));
|
||||
lineData.p3DPoint = rgbaPoints.get();
|
||||
lineData.p2DPoint = rgba2DPoints.get();
|
||||
} else if (pointCount > 0 && dataType == keResultDataType_Position) {
|
||||
positionPoints.reset(new (std::nothrow) SVzNL3DPosition[pointCount]);
|
||||
position2DPoints.reset(new (std::nothrow) SVzNL2DPosition[pointCount]);
|
||||
if (!positionPoints || !position2DPoints) {
|
||||
m_lastError = "Memory allocation failed while reading: " + fileName;
|
||||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||||
return finish(ERR_CODE(DATA_ERR_MEM));
|
||||
}
|
||||
std::memset(positionPoints.get(), 0,
|
||||
sizeof(SVzNL3DPosition) * static_cast<size_t>(pointCount));
|
||||
std::memset(position2DPoints.get(), 0,
|
||||
sizeof(SVzNL2DPosition) * static_cast<size_t>(pointCount));
|
||||
lineData.p3DPoint = positionPoints.get();
|
||||
lineData.p2DPoint = position2DPoints.get();
|
||||
}
|
||||
hasLine = true;
|
||||
if (pointCount == 0) {
|
||||
result = emitLine();
|
||||
if (result != SUCCESS) {
|
||||
return finish(result);
|
||||
}
|
||||
}
|
||||
} else if (line.find("Poly_") == 0) {
|
||||
size_t lastUnderscore = line.rfind('_');
|
||||
int polyPointCount = 0;
|
||||
if (lastUnderscore != std::string::npos && lastUnderscore + 1 < line.size()) {
|
||||
polyPointCount = std::atoi(line.c_str() + lastUnderscore + 1);
|
||||
}
|
||||
for (int skipped = 0; skipped < polyPointCount;) {
|
||||
if (!std::getline(inputFile, line)) {
|
||||
break;
|
||||
}
|
||||
TrimCarriageReturn(line);
|
||||
if (!line.empty()) {
|
||||
++skipped;
|
||||
}
|
||||
}
|
||||
} else if (line.find("{") == 0) {
|
||||
if (!hasLine || pointIndex >= lineData.nPointCount ||
|
||||
!lineData.p3DPoint || !lineData.p2DPoint) {
|
||||
m_lastError = "Point data does not match laser line header: " + line;
|
||||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||||
return finish(ERR_CODE(FILE_ERR_FORMAT));
|
||||
}
|
||||
|
||||
if (dataType == keResultDataType_PointXYZRGBA) {
|
||||
result = _ParseLaserScanPoint(line, rgbaPoints[pointIndex], rgba2DPoints[pointIndex]);
|
||||
} else {
|
||||
result = _ParseLaserScanPoint(line, positionPoints[pointIndex], position2DPoints[pointIndex]);
|
||||
positionPoints[pointIndex].nPointIdx = pointIndex;
|
||||
}
|
||||
if (result != SUCCESS) {
|
||||
m_lastError = "Invalid point line: " + line;
|
||||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||||
return finish(result);
|
||||
}
|
||||
++pointIndex;
|
||||
if (pointIndex == lineData.nPointCount) {
|
||||
result = emitLine();
|
||||
if (result != SUCCESS) {
|
||||
return finish(result);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
result = emitLine();
|
||||
if (result != SUCCESS) {
|
||||
return finish(result);
|
||||
}
|
||||
LOG_INFO("Successfully loaded %d laser scan lines by callback from file: %s\n",
|
||||
lineNum, fileName.c_str());
|
||||
return finish(SUCCESS);
|
||||
}
|
||||
|
||||
// Read binary .dat laser scan data.
|
||||
@ -1180,6 +1372,158 @@ int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPositi
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
int LaserDataLoader::_LoadLaserScanDataFromDatFileByCallback(
|
||||
const std::string& fileName,
|
||||
const LaserLineLoadCallback& lineCallback,
|
||||
const LaserLoadFinishedCallback& finishedCallback)
|
||||
{
|
||||
int lineNum = 0;
|
||||
float scanSpeed = 0.0f;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
|
||||
auto finish = [&](int result) -> int {
|
||||
if (finishedCallback) {
|
||||
finishedCallback(result, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
|
||||
}
|
||||
return result;
|
||||
};
|
||||
|
||||
std::ifstream inputFile(fileName, std::ios::binary);
|
||||
if (!inputFile.is_open()) {
|
||||
m_lastError = "Cannot open file: " + fileName;
|
||||
LOG_ERROR("Cannot open dat file: %s\n", fileName.c_str());
|
||||
return finish(ERR_CODE(FILE_ERR_NOEXIST));
|
||||
}
|
||||
|
||||
auto fail = [&](int code, const std::string& message) -> int {
|
||||
m_lastError = message;
|
||||
LOG_ERROR("%s\n", message.c_str());
|
||||
return finish(ERR_CODE(code));
|
||||
};
|
||||
|
||||
inputFile.seekg(0, std::ios::end);
|
||||
const std::streamoff fileSize = inputFile.tellg();
|
||||
inputFile.seekg(0, std::ios::beg);
|
||||
|
||||
DatLaserScan3DLinesHeader header;
|
||||
std::memset(&header, 0, sizeof(header));
|
||||
if (!ReadBinary(inputFile, header)) {
|
||||
return fail(FILE_ERR_READ, "Cannot read dat header: " + fileName);
|
||||
}
|
||||
if (header.scanLineNum < 0 || fileSize < static_cast<std::streamoff>(sizeof(header))) {
|
||||
return fail(FILE_ERR_FORMAT, "Invalid dat header: " + fileName);
|
||||
}
|
||||
|
||||
const int featurePointSize = static_cast<int>(sizeof(Dat3DFeaturePoint));
|
||||
auto validateLayout = [&](int lineRecordSize) -> bool {
|
||||
std::streamoff pos = static_cast<std::streamoff>(sizeof(header));
|
||||
inputFile.clear();
|
||||
inputFile.seekg(pos, std::ios::beg);
|
||||
for (int i = 0; i < header.scanLineNum; ++i) {
|
||||
if (pos + lineRecordSize + static_cast<int>(sizeof(int)) > fileSize) {
|
||||
return false;
|
||||
}
|
||||
inputFile.seekg(lineRecordSize, std::ios::cur);
|
||||
pos += lineRecordSize;
|
||||
|
||||
int pointCount = 0;
|
||||
if (!ReadBinary(inputFile, pointCount) ||
|
||||
pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) {
|
||||
return false;
|
||||
}
|
||||
pos += static_cast<std::streamoff>(sizeof(pointCount));
|
||||
const std::streamoff pointBytes =
|
||||
static_cast<std::streamoff>(pointCount) * featurePointSize;
|
||||
if (pointBytes < 0 || pos + pointBytes > fileSize) {
|
||||
return false;
|
||||
}
|
||||
inputFile.seekg(pointBytes, std::ios::cur);
|
||||
pos += pointBytes;
|
||||
}
|
||||
return pos == fileSize;
|
||||
};
|
||||
|
||||
int lineRecordSize = 40;
|
||||
if (!validateLayout(lineRecordSize)) {
|
||||
lineRecordSize = 32;
|
||||
if (!validateLayout(lineRecordSize)) {
|
||||
return fail(FILE_ERR_FORMAT, "Invalid dat layout: " + fileName);
|
||||
}
|
||||
}
|
||||
|
||||
lineNum = header.scanLineNum;
|
||||
scanSpeed = static_cast<float>(header.lineV);
|
||||
maxTimeStamp = static_cast<int>(header.maxTimeStamp);
|
||||
clockPerSecond = header.clock;
|
||||
inputFile.clear();
|
||||
inputFile.seekg(static_cast<std::streamoff>(sizeof(header)), std::ios::beg);
|
||||
|
||||
for (int i = 0; i < lineNum; ++i) {
|
||||
DatLineScan3DData datLine;
|
||||
std::memset(&datLine, 0, sizeof(datLine));
|
||||
if (!ReadDatLineRecord(inputFile, lineRecordSize, datLine)) {
|
||||
return fail(FILE_ERR_READ, "Cannot read dat line header: " + fileName);
|
||||
}
|
||||
|
||||
int pointCount = 0;
|
||||
if (!ReadBinary(inputFile, pointCount)) {
|
||||
return fail(FILE_ERR_READ, "Cannot read dat point count: " + fileName);
|
||||
}
|
||||
if (pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) {
|
||||
return fail(FILE_ERR_FORMAT, "Invalid dat point count: " + fileName);
|
||||
}
|
||||
|
||||
std::unique_ptr<SVzNL3DPosition[]> points3D;
|
||||
std::unique_ptr<SVzNL2DPosition[]> points2D;
|
||||
if (pointCount > 0) {
|
||||
points3D.reset(new (std::nothrow) SVzNL3DPosition[pointCount]);
|
||||
points2D.reset(new (std::nothrow) SVzNL2DPosition[pointCount]);
|
||||
if (!points3D || !points2D) {
|
||||
return fail(DATA_ERR_MEM, "Memory allocation failed while reading dat: " + fileName);
|
||||
}
|
||||
std::memset(points3D.get(), 0,
|
||||
sizeof(SVzNL3DPosition) * static_cast<size_t>(pointCount));
|
||||
std::memset(points2D.get(), 0,
|
||||
sizeof(SVzNL2DPosition) * static_cast<size_t>(pointCount));
|
||||
}
|
||||
|
||||
for (int j = 0; j < pointCount; ++j) {
|
||||
Dat3DFeaturePoint featurePoint;
|
||||
std::memset(&featurePoint, 0, sizeof(featurePoint));
|
||||
if (!ReadBinary(inputFile, featurePoint)) {
|
||||
return fail(FILE_ERR_READ, "Cannot read dat feature point: " + fileName);
|
||||
}
|
||||
points3D[j].nPointIdx = j;
|
||||
points3D[j].pt3D.x = featurePoint.eye.X;
|
||||
points3D[j].pt3D.y = featurePoint.eye.Y;
|
||||
points3D[j].pt3D.z = featurePoint.eye.Z;
|
||||
points2D[j].nPointIdx = j;
|
||||
points2D[j].ptLeft2D.x = static_cast<int>(featurePoint.stereo.x);
|
||||
points2D[j].ptLeft2D.y = static_cast<int>(featurePoint.stereo.y);
|
||||
points2D[j].ptRight2D.x = static_cast<int>(featurePoint.stereoRight.x);
|
||||
points2D[j].ptRight2D.y = static_cast<int>(featurePoint.stereoRight.y);
|
||||
}
|
||||
|
||||
SVzLaserLineData lineData;
|
||||
std::memset(&lineData, 0, sizeof(lineData));
|
||||
lineData.nPointCount = pointCount;
|
||||
lineData.dTotleOffset = datLine.lineStepping;
|
||||
lineData.dStep = datLine.lineStepping;
|
||||
lineData.llFrameIdx = static_cast<unsigned long long>(datLine.lineIndex);
|
||||
lineData.llTimeStamp = static_cast<unsigned long long>(datLine.timeStamp);
|
||||
lineData.p3DPoint = points3D.get();
|
||||
lineData.p2DPoint = points2D.get();
|
||||
if (lineCallback) {
|
||||
lineCallback(keResultDataType_Position, lineData);
|
||||
}
|
||||
}
|
||||
|
||||
LOG_INFO("Successfully loaded %d dat laser scan lines by callback from file: %s\n",
|
||||
lineNum, fileName.c_str());
|
||||
return finish(SUCCESS);
|
||||
}
|
||||
|
||||
int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPositionF& s2DData)
|
||||
{
|
||||
double X = 0.0, Y = 0.0, Z = 0.0;
|
||||
|
||||
@ -1241,7 +1241,7 @@ QWidget* CloudViewMainWindow::createViewerArea()
|
||||
QVBoxLayout* layout = new QVBoxLayout(widget);
|
||||
layout->setContentsMargins(0, 0, 0, 0);
|
||||
|
||||
m_glWidget = new PointCloudGLWidget(widget);
|
||||
m_glWidget = new PointCloudGLWidget(widget, false);
|
||||
m_glWidget->setMinimumSize(400, 300);
|
||||
m_glWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
|
||||
layout->addWidget(m_glWidget);
|
||||
|
||||
@ -7,12 +7,16 @@
|
||||
#include <QMatrix4x4>
|
||||
#include <QVector3D>
|
||||
#include <QQuaternion>
|
||||
#include <QMouseEvent>
|
||||
#include <QWheelEvent>
|
||||
#include <vector>
|
||||
#include <QMouseEvent>
|
||||
#include <QWheelEvent>
|
||||
#include <QResizeEvent>
|
||||
#include <deque>
|
||||
#include <vector>
|
||||
|
||||
#include "CloudView3DTypes.h"
|
||||
#include "VZNL_Types.h"
|
||||
|
||||
class QToolButton;
|
||||
|
||||
/**
|
||||
* @brief 点云显示颜色枚举
|
||||
@ -164,13 +168,21 @@ class PointCloudGLWidget : public QOpenGLWidget, protected QOpenGLFunctions
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
explicit PointCloudGLWidget(QWidget* parent = nullptr);
|
||||
~PointCloudGLWidget() override;
|
||||
|
||||
void addPointCloud(const PointCloudXYZ& cloud, const QString& name = "");
|
||||
void addPointCloud(const PointCloudXYZRGB& cloud, const QString& name = "");
|
||||
void clearPointClouds();
|
||||
public:
|
||||
// Controls are enabled by default for standalone CloudView3D users.
|
||||
// Embedders such as CloudView can disable them and provide their own UI.
|
||||
explicit PointCloudGLWidget(QWidget* parent = nullptr);
|
||||
PointCloudGLWidget(QWidget* parent, bool showControlButtons);
|
||||
~PointCloudGLWidget() override;
|
||||
|
||||
void setControlButtonsVisible(bool visible);
|
||||
bool controlButtonsVisible() const { return m_showControlButtons; }
|
||||
|
||||
void addPointCloud(const PointCloudXYZ& cloud, const QString& name = "", bool resetView = true);
|
||||
void addPointCloud(const PointCloudXYZRGB& cloud, const QString& name = "", bool resetView = true);
|
||||
/** 删除最早添加的一组点云，用于维护滚动显示窗口。 */
|
||||
bool removeOldestPointCloud();
|
||||
void clearPointClouds();
|
||||
void setPointCloudColor(PointCloudColor color);
|
||||
void setPointSize(float size);
|
||||
void resetView();
|
||||
@ -359,7 +371,8 @@ protected:
|
||||
void mouseReleaseEvent(QMouseEvent* event) override;
|
||||
void wheelEvent(QWheelEvent* event) override;
|
||||
void mouseDoubleClickEvent(QMouseEvent* event) override;
|
||||
void keyPressEvent(QKeyEvent* event) override;
|
||||
void keyPressEvent(QKeyEvent* event) override;
|
||||
void resizeEvent(QResizeEvent* event) override;
|
||||
|
||||
private:
|
||||
struct PointCloudData
|
||||
@ -468,11 +481,13 @@ private:
|
||||
void drawBasicShapePoints();
|
||||
void drawLineSegmentList(const QVector<LineSegment>& segments);
|
||||
void drawPosePoints(); // 绘制姿态点
|
||||
void drawAxisLabels(); // 绘制坐标轴 XYZ 标注
|
||||
void drawAxisLabels(); // 绘制坐标轴 XYZ 标注
|
||||
void createControlButtons();
|
||||
void updateControlButtonGeometry();
|
||||
void uploadToVBO(PointCloudData& data); // 上传数据到 VBO
|
||||
void releaseVBO(PointCloudData& data); // 释放 VBO 资源
|
||||
|
||||
std::vector<PointCloudData> m_pointClouds;
|
||||
std::deque<PointCloudData> m_pointClouds;
|
||||
|
||||
QMatrix4x4 m_projection;
|
||||
QMatrix4x4 m_view;
|
||||
@ -516,6 +531,11 @@ private:
|
||||
QVector<SurfaceQuad> m_basicShapeSurfaces;
|
||||
QVector<BasicShapePoint> m_basicShapePoints;
|
||||
QVector<PosePoint> m_posePoints;
|
||||
|
||||
bool m_showControlButtons;
|
||||
QWidget* m_controlPanel;
|
||||
QToolButton* m_zoomInButton;
|
||||
QToolButton* m_zoomOutButton;
|
||||
};
|
||||
|
||||
#endif // POINT_CLOUD_GL_WIDGET_H
|
||||
|
||||
@ -1,8 +1,11 @@
|
||||
#include "PointCloudGLWidget.h"
|
||||
#include <QDebug>
|
||||
#include <QtMath>
|
||||
#include <QPainter>
|
||||
#include <cfloat>
|
||||
#include <QPainter>
|
||||
#include <QToolButton>
|
||||
#include <QHBoxLayout>
|
||||
#include <QVBoxLayout>
|
||||
#include <cfloat>
|
||||
#include <map>
|
||||
#include "VrLog.h"
|
||||
|
||||
@ -16,8 +19,13 @@
|
||||
#include <GL/glu.h>
|
||||
#endif
|
||||
|
||||
PointCloudGLWidget::PointCloudGLWidget(QWidget* parent)
|
||||
: QOpenGLWidget(parent)
|
||||
PointCloudGLWidget::PointCloudGLWidget(QWidget* parent)
|
||||
: PointCloudGLWidget(parent, true)
|
||||
{
|
||||
}
|
||||
|
||||
PointCloudGLWidget::PointCloudGLWidget(QWidget* parent, bool showControlButtons)
|
||||
: QOpenGLWidget(parent)
|
||||
, m_distance(100.0f)
|
||||
, m_rotationX(0.0f)
|
||||
, m_rotationY(0.0f)
|
||||
@ -35,17 +43,23 @@ PointCloudGLWidget::PointCloudGLWidget(QWidget* parent)
|
||||
, m_lineSelectMode(LineSelectMode::Vertical)
|
||||
, m_eulerRotationOrder(EulerRotationOrder::ZYX)
|
||||
, m_measureDistanceEnabled(false)
|
||||
, m_hasListHighlightPoint(false)
|
||||
, m_colorIndex(0)
|
||||
{
|
||||
, m_hasListHighlightPoint(false)
|
||||
, m_colorIndex(0)
|
||||
, m_showControlButtons(showControlButtons)
|
||||
, m_controlPanel(nullptr)
|
||||
, m_zoomInButton(nullptr)
|
||||
, m_zoomOutButton(nullptr)
|
||||
{
|
||||
setFocusPolicy(Qt::StrongFocus);
|
||||
setMouseTracking(true);
|
||||
|
||||
// 确保 m_model 是单位矩阵
|
||||
m_model.setToIdentity();
|
||||
}
|
||||
// 确保 m_model 是单位矩阵
|
||||
m_model.setToIdentity();
|
||||
|
||||
createControlButtons();
|
||||
}
|
||||
|
||||
PointCloudGLWidget::~PointCloudGLWidget()
|
||||
PointCloudGLWidget::~PointCloudGLWidget()
|
||||
{
|
||||
// 在 GL 上下文中释放所有 VBO
|
||||
makeCurrent();
|
||||
@ -113,18 +127,24 @@ void PointCloudGLWidget::initializeGL()
|
||||
glHint(GL_POINT_SMOOTH_HINT, GL_NICEST);
|
||||
}
|
||||
|
||||
void PointCloudGLWidget::resizeGL(int w, int h)
|
||||
{
|
||||
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::paintGL()
|
||||
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);
|
||||
|
||||
@ -240,7 +260,7 @@ void PointCloudGLWidget::paintGL()
|
||||
drawAxisLabels();
|
||||
}
|
||||
|
||||
void PointCloudGLWidget::addPointCloud(const PointCloudXYZ& cloud, const QString& name)
|
||||
void PointCloudGLWidget::addPointCloud(const PointCloudXYZ& cloud, const QString& name, bool resetViewAfterAdd)
|
||||
{
|
||||
LOG_INFO("[CloudView] addPointCloud called, cloud size: %zu\n", cloud.size());
|
||||
|
||||
@ -326,11 +346,134 @@ void PointCloudGLWidget::addPointCloud(const PointCloudXYZ& cloud, const QString
|
||||
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 (resetViewAfterAdd) {
|
||||
resetView();
|
||||
}
|
||||
update();
|
||||
}
|
||||
|
||||
void PointCloudGLWidget::setControlButtonsVisible(bool visible)
|
||||
{
|
||||
m_showControlButtons = visible;
|
||||
if (m_controlPanel) {
|
||||
m_controlPanel->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;
|
||||
@ -436,12 +579,14 @@ void PointCloudGLWidget::addPointCloud(const PointCloudXYZRGB& cloud, const QStr
|
||||
doneCurrent();
|
||||
|
||||
computeBoundingBox();
|
||||
resetView();
|
||||
update();
|
||||
}
|
||||
if (resetViewAfterAdd) {
|
||||
resetView();
|
||||
}
|
||||
update();
|
||||
}
|
||||
|
||||
void PointCloudGLWidget::clearPointClouds()
|
||||
{
|
||||
void PointCloudGLWidget::clearPointClouds()
|
||||
{
|
||||
// 释放所有 VBO
|
||||
makeCurrent();
|
||||
for (auto& cloudData : m_pointClouds) {
|
||||
@ -462,10 +607,30 @@ void PointCloudGLWidget::clearPointClouds()
|
||||
m_center = QVector3D(0, 0, 0);
|
||||
m_colorIndex = 0; // 重置颜色索引
|
||||
resetView();
|
||||
update();
|
||||
}
|
||||
|
||||
void PointCloudGLWidget::setPointCloudColor(PointCloudColor color)
|
||||
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();
|
||||
update();
|
||||
return true;
|
||||
}
|
||||
|
||||
void PointCloudGLWidget::setPointCloudColor(PointCloudColor color)
|
||||
{
|
||||
m_currentColor = color;
|
||||
update();
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
enum ErrorCode
|
||||
{
|
||||
SUCCESS = 0,
|
||||
CLASS_OBJ_NULL = 1000,
|
||||
CLASS_OBJ_NULL = 5000,
|
||||
FUN_UNSUPPORT,
|
||||
|
||||
//功能执行失败
|
||||
@ -15,7 +15,7 @@ enum ErrorCode
|
||||
|
||||
//相关加密内容
|
||||
ENCRYPT_SUCCESS = 0,
|
||||
ENCRYPT_ERROR_BASE = 1050,
|
||||
ENCRYPT_ERROR_BASE = 5050,
|
||||
ENCRYPT_LOAD,
|
||||
ENCRYPT_NOT_LOAD,
|
||||
ENCRYPT_RELEASE,
|
||||
@ -23,7 +23,7 @@ enum ErrorCode
|
||||
ENCRYPT_ARG,
|
||||
|
||||
//文件相关错误
|
||||
FILE_ERR_EXIST = 1100,
|
||||
FILE_ERR_EXIST = 5100,
|
||||
FILE_ERR_NOEXIST,
|
||||
FILE_ERR_READ,
|
||||
FILE_ERR_WRITE,
|
||||
@ -31,7 +31,7 @@ enum ErrorCode
|
||||
FILE_ERR_DEL,
|
||||
|
||||
//共享内存错误
|
||||
SHAREMEM_ERR_BASE = 1200,
|
||||
SHAREMEM_ERR_BASE = 5200,
|
||||
SHAREMEM_ERR_PARAM, // 参数错误
|
||||
SHAREMEM_ERR_CREATE, // 创建失败
|
||||
SHAREMEM_ERR_ALREADY_EXIST, // 已存在
|
||||
@ -43,7 +43,7 @@ enum ErrorCode
|
||||
SHAREMEM_ERR_TIMEOUT, // 超时
|
||||
|
||||
//数据错误
|
||||
DATA_ERR_LEN = 1300,
|
||||
DATA_ERR_LEN = 5300,
|
||||
DATA_ERR_INVALID,
|
||||
DATA_ERR_MD5,
|
||||
DATA_ERR_KEY,
|
||||
@ -52,7 +52,7 @@ enum ErrorCode
|
||||
|
||||
//网络错误
|
||||
NET_SUCCESS = 0,
|
||||
NET_ERR_CREAT_INIT = 1400,
|
||||
NET_ERR_CREAT_INIT = 5400,
|
||||
NET_ERR_CREAT_BIND,
|
||||
NET_ERR_CREAT_LISTEN,
|
||||
NET_ERR_CONNECT,
|
||||
@ -75,7 +75,7 @@ enum ErrorCode
|
||||
NET_DEV_CLIENT_RECV, //1420
|
||||
|
||||
// 设备类型
|
||||
DEV_TYPE_ERR = 1500,
|
||||
DEV_TYPE_ERR = 5500,
|
||||
DEV_NOT_FIND,
|
||||
DEV_NO_OPEN,
|
||||
DEV_ID_ERR,
|
||||
@ -88,41 +88,41 @@ enum ErrorCode
|
||||
DEV_UNSUPPORT, //1510
|
||||
DEV_ARG_INVAILD,
|
||||
DEV_BUSY,
|
||||
DEV_CONFIG_ERR,
|
||||
DEV_DATA_INVALID,
|
||||
DEV_RESULT_EMPTY, //1515
|
||||
DEV_UNACTIVATE, //未激活
|
||||
|
||||
// DroneScrew 业务错误
|
||||
DRONESCREW_ERR_BASE = 1600,
|
||||
DRONESCREW_ERR_INVALID_JSON,
|
||||
DRONESCREW_ERR_UNKNOWN_CMD,
|
||||
DRONESCREW_ERR_SINGLE_HANDLER_NOT_SET,
|
||||
DRONESCREW_ERR_MACHINE_CODE_MISMATCH,
|
||||
DRONESCREW_ERR_CAMERA_NOT_CONNECTED,
|
||||
DRONESCREW_ERR_BINOCULAR_FRAME_TIMEOUT,
|
||||
DRONESCREW_ERR_ALGO_NOT_INIT,
|
||||
DRONESCREW_ERR_CACHED_FRAME_NOT_READY,
|
||||
DRONESCREW_ERR_LEFT_ACQ_NOT_STARTED,
|
||||
DRONESCREW_ERR_RIGHT_ACQ_NOT_STARTED,
|
||||
DRONESCREW_ERR_GPIO_TRIGGER,
|
||||
DRONESCREW_ERR_SINGLE_BUSY,
|
||||
DRONESCREW_ERR_MODE_CONFLICT,
|
||||
DRONESCREW_ERR_DETECT_CONFIG_LEFT,
|
||||
DRONESCREW_ERR_DETECT_CONFIG_RIGHT,
|
||||
DRONESCREW_ERR_DETECT_START_LEFT,
|
||||
DRONESCREW_ERR_DETECT_START_RIGHT,
|
||||
DRONESCREW_ERR_LIVE_CONFIG_LEFT,
|
||||
DRONESCREW_ERR_LIVE_START_LEFT,
|
||||
DRONESCREW_ERR_RTSP_INIT,
|
||||
DRONESCREW_ERR_RTSP_START,
|
||||
DRONESCREW_ERR_ZMQ_HANDLER_NOT_SET,
|
||||
DRONESCREW_ERR_UDP_DISCOVERY,
|
||||
DRONESCREW_ERR_RESPONSE_NOT_OK,
|
||||
DRONESCREW_ERR_GPIO_EXPORT,
|
||||
DRONESCREW_ERR_GPIO_DIRECTION,
|
||||
DRONESCREW_ERR_GPIO_VALUE,
|
||||
};
|
||||
DEV_CONFIG_ERR,
|
||||
DEV_DATA_INVALID,
|
||||
DEV_RESULT_EMPTY, //1515
|
||||
DEV_UNACTIVATE, //未激活
|
||||
|
||||
// DroneScrew 业务错误
|
||||
DRONESCREW_ERR_BASE = 5600,
|
||||
DRONESCREW_ERR_INVALID_JSON,
|
||||
DRONESCREW_ERR_UNKNOWN_CMD,
|
||||
DRONESCREW_ERR_SINGLE_HANDLER_NOT_SET,
|
||||
DRONESCREW_ERR_MACHINE_CODE_MISMATCH,
|
||||
DRONESCREW_ERR_CAMERA_NOT_CONNECTED,
|
||||
DRONESCREW_ERR_BINOCULAR_FRAME_TIMEOUT,
|
||||
DRONESCREW_ERR_ALGO_NOT_INIT,
|
||||
DRONESCREW_ERR_CACHED_FRAME_NOT_READY,
|
||||
DRONESCREW_ERR_LEFT_ACQ_NOT_STARTED,
|
||||
DRONESCREW_ERR_RIGHT_ACQ_NOT_STARTED,
|
||||
DRONESCREW_ERR_GPIO_TRIGGER,
|
||||
DRONESCREW_ERR_SINGLE_BUSY,
|
||||
DRONESCREW_ERR_MODE_CONFLICT,
|
||||
DRONESCREW_ERR_DETECT_CONFIG_LEFT,
|
||||
DRONESCREW_ERR_DETECT_CONFIG_RIGHT,
|
||||
DRONESCREW_ERR_DETECT_START_LEFT,
|
||||
DRONESCREW_ERR_DETECT_START_RIGHT,
|
||||
DRONESCREW_ERR_LIVE_CONFIG_LEFT,
|
||||
DRONESCREW_ERR_LIVE_START_LEFT,
|
||||
DRONESCREW_ERR_RTSP_INIT,
|
||||
DRONESCREW_ERR_RTSP_START,
|
||||
DRONESCREW_ERR_ZMQ_HANDLER_NOT_SET,
|
||||
DRONESCREW_ERR_UDP_DISCOVERY,
|
||||
DRONESCREW_ERR_RESPONSE_NOT_OK,
|
||||
DRONESCREW_ERR_GPIO_EXPORT,
|
||||
DRONESCREW_ERR_GPIO_DIRECTION,
|
||||
DRONESCREW_ERR_GPIO_VALUE,
|
||||
};
|
||||
|
||||
#define ERR_CODE(nCode) -std::abs((int)nCode)
|
||||
#define ERR_CODE_RETURN(nCode) do{ if(SUCCESS != (nCode)) return ERR_CODE(nCode); }while(0)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user