feat(点云处理): 添加基于Z阈值的点云旋转和保存功能
This commit is contained in:
杰仔
parent
598cd176f5
commit
c088ad39c4
@ -6,6 +6,17 @@
|
||||
#include "VZNL_Types.h"
|
||||
#include "VrError.h"
|
||||
|
||||
// 保存进度回调接口
|
||||
class ISaveProgressCallback
|
||||
{
|
||||
public:
|
||||
virtual ~ISaveProgressCallback() = default;
|
||||
// 进度回调，progress 范围 0.0~1.0
|
||||
virtual void onSaveProgress(float progress) = 0;
|
||||
// 返回 true 取消保存
|
||||
virtual bool isSaveCancelled() const { return false; }
|
||||
};
|
||||
|
||||
// 激光数据加载器类
|
||||
class LaserDataLoader
|
||||
{
|
||||
@ -30,6 +41,8 @@ public:
|
||||
int clockPerSecond);
|
||||
|
||||
int DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition>> xyzData);
|
||||
int DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition>> xyzData,
|
||||
ISaveProgressCallback* callback);
|
||||
|
||||
// 释放统一格式数据内存
|
||||
void FreeLaserScanData(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines);
|
||||
|
||||
@ -246,6 +246,12 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName,
|
||||
}
|
||||
|
||||
int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition> > xyzData)
|
||||
{
|
||||
return DebugSaveLaser(std::move(fileName), std::move(xyzData), nullptr);
|
||||
}
|
||||
|
||||
int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition>> xyzData,
|
||||
ISaveProgressCallback* callback)
|
||||
{
|
||||
LOG_INFO("Saving unified laser scan data to file: %s\n", fileName.c_str());
|
||||
|
||||
@ -270,10 +276,18 @@ int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vecto
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp:" << 0 << "_" << 0 << std::endl;
|
||||
|
||||
const size_t totalLines = xyzData.size();
|
||||
int index = 0;
|
||||
// 写入每条扫描线数据
|
||||
for (const auto& linePair : xyzData) {
|
||||
sw << "Line_" << index++ << "_" << 0 << "_" << linePair.size() << std::endl;
|
||||
if (callback && callback->isSaveCancelled()) {
|
||||
sw.close();
|
||||
m_lastError = "Save cancelled by user";
|
||||
LOG_INFO("Save cancelled by user at line %d/%zu\n", index, totalLines);
|
||||
return ERR_CODE(DEV_ARG_INVAILD);
|
||||
}
|
||||
|
||||
sw << "Line_" << index << "_" << 0 << "_" << linePair.size() << std::endl;
|
||||
// 根据数据类型写入点云数据
|
||||
for(const auto& point : linePair){
|
||||
|
||||
@ -287,6 +301,11 @@ int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vecto
|
||||
<< std::fixed << std::setprecision(6) << z << " } - ";
|
||||
sw << "{ 0, 0 } - { 0, 0 }" << std::endl;
|
||||
}
|
||||
|
||||
++index;
|
||||
if (callback) {
|
||||
callback->onSaveProgress(static_cast<float>(index) / static_cast<float>(totalLines));
|
||||
}
|
||||
}
|
||||
|
||||
sw.close();
|
||||
|
||||
@ -162,11 +162,16 @@ private slots:
|
||||
* @brief 应用矩阵变换到所有点云
|
||||
*/
|
||||
void onApplyMatrix();
|
||||
void onApplyEyeInHandTransform();
|
||||
|
||||
/**
|
||||
* @brief 重置矩阵为单位矩阵
|
||||
*/
|
||||
void onResetMatrix();
|
||||
void onGeneratePointCloud();
|
||||
void onRotateCloudByZ();
|
||||
void onSavePointCloud();
|
||||
void onConvertEulerMatrix();
|
||||
|
||||
private:
|
||||
/**
|
||||
@ -256,6 +261,8 @@ private:
|
||||
* @return 是否成功加载到包围盒数据
|
||||
*/
|
||||
bool loadBoundingBoxFile(const QString& fileName);
|
||||
void applyTransformToAllClouds(const QMatrix4x4& matrix);
|
||||
void addGeneratedCloud(const PointCloudXYZ& cloud, const QString& name);
|
||||
|
||||
// 点云显示控件
|
||||
PointCloudGLWidget* m_glWidget;
|
||||
@ -351,6 +358,13 @@ private:
|
||||
QTextEdit* m_matrixEdit;
|
||||
QPushButton* m_btnLoadMatrix;
|
||||
QPushButton* m_btnApplyMatrix;
|
||||
QLineEdit* m_editRobotX;
|
||||
QLineEdit* m_editRobotY;
|
||||
QLineEdit* m_editRobotZ;
|
||||
QLineEdit* m_editRobotRx;
|
||||
QLineEdit* m_editRobotRy;
|
||||
QLineEdit* m_editRobotRz;
|
||||
QPushButton* m_btnApplyEyeInHand;
|
||||
QPushButton* m_btnResetMatrix;
|
||||
|
||||
// 悬浮缩放按钮
|
||||
|
||||
@ -12,6 +12,7 @@
|
||||
#include <vector>
|
||||
|
||||
#include "PointCloudConverter.h"
|
||||
#include "VZNL_Types.h"
|
||||
|
||||
/**
|
||||
* @brief 点云显示颜色枚举
|
||||
@ -243,6 +244,21 @@ public:
|
||||
*/
|
||||
void transformAllClouds(const QMatrix4x4& matrix);
|
||||
|
||||
/**
|
||||
* @brief 对所有点云中 Z 值小于阈值的点应用旋转矩阵
|
||||
* @param rotMatrix 旋转矩阵
|
||||
* @param zThreshold Z 轴阈值
|
||||
* @return 被旋转的点数
|
||||
*/
|
||||
size_t rotateCloudsByZThreshold(const QMatrix4x4& rotMatrix, float zThreshold);
|
||||
|
||||
/**
|
||||
* @brief 获取所有点云数据（按线分组），用于保存
|
||||
* @param scanLines 输出：每条扫描线的点列表
|
||||
* @return 总点数
|
||||
*/
|
||||
size_t getAllCloudsByLines(std::vector<std::vector<SVzNL3DPosition>>& scanLines) const;
|
||||
|
||||
/**
|
||||
* @brief 添加线段
|
||||
*/
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -750,6 +750,98 @@ void PointCloudGLWidget::transformAllClouds(const QMatrix4x4& matrix)
|
||||
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();
|
||||
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()
|
||||
{
|
||||
if (m_pointClouds.empty()) {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user