YangOne/Utils
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 4 Wiki Activity

修复崩溃，修复选点序号不对

Browse Source
This commit is contained in:
yiyi 2026-03-17 22:37:46 +08:00
parent d6049e7846
commit 36645c6ceb
3 changed files with 61 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
CloudUtils/Src/LaserDataLoader.cpp
View File

@ -121,13 +121,19 @@ int LaserDataLoader::LoadLaserScanData(const std::string& fileName,
// 使用正则表达式判断是XYZ还是RGBD格式
// XYZ格式: {x,y,z}-{leftX,leftY}-{rightX,rightY}
// RGBD格式: {x,y,z,r,g,b}-{leftX,leftY}-{rightX,rightY}
// 更精确的正则表达式，匹配完整的行格式
if(sLaserData.p3DPoint == nullptr || sLaserData.p2DPoint == nullptr) {
LOG_ERROR("sLaserData.p3DPoint == nullptr || sLaserData.p2DPoint == nullptr \n");
return ERR_CODE(DATA_ERR_INVALID);
}
// 防止数组越界：跳过超出声明点数的数据行
if (nLaserPointIdx >= sLaserData.nPointCount) {
LOG_WARN("nLaserPointIdx(%d) >= nPointCount(%d), skip\n", nLaserPointIdx, sLaserData.nPointCount);
continue;
}
if (eDataType == keResultDataType_PointXYZRGBA) {
SVzNLPointXYZRGBA* pRGBAPoints = static_cast<SVzNLPointXYZRGBA*>(sLaserData.p3DPoint);
SVzNL2DLRPoint* p2DPoints = static_cast<SVzNL2DLRPoint*>(sLaserData.p2DPoint);

3
CloudView/Inc/PointCloudGLWidget.h
View File

@ -287,6 +287,7 @@ private:
std::vector<float> colors;
std::vector<int> lineIndices; // 每个点所属的线索引
std::vector<int> originalIndices; // 每个显示点在原始点云中的索引（用于计算原始index）
std::vector<int> pointInLineIndices; // 每个显示点在所属线中的索引（预计算，支持不等长线）
bool hasColor;
bool hasLineInfo; // 是否有线信息
QString name;
@ -322,6 +323,7 @@ private:
, colors(std::move(other.colors))
, lineIndices(std::move(other.lineIndices))
, originalIndices(std::move(other.originalIndices))
, pointInLineIndices(std::move(other.pointInLineIndices))
, hasColor(other.hasColor)
, hasLineInfo(other.hasLineInfo)
, name(std::move(other.name))
@ -351,6 +353,7 @@ private:
colors = std::move(other.colors);
lineIndices = std::move(other.lineIndices);
originalIndices = std::move(other.originalIndices);
pointInLineIndices = std::move(other.pointInLineIndices);
hasColor = other.hasColor;
hasLineInfo = other.hasLineInfo;
name = std::move(other.name);

59
CloudView/Src/PointCloudGLWidget.cpp
View File

@ -255,13 +255,27 @@ void PointCloudGLWidget::addPointCloud(const PointCloudXYZ& cloud, const QString
data.pointsPerLine = 0;
const float EPSILON = 1e-6f;
int prevLineIdx = -1;
int ptInLineCounter = 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;
}
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);
@ -271,14 +285,18 @@ void PointCloudGLWidget::addPointCloud(const PointCloudXYZ& cloud, const QString
// 保存原始索引
data.originalIndices.push_back(static_cast<int>(i));
// 保存线内索引（点在所属线中的位置）
data.pointInLineIndices.push_back(ptInLineCounter);
// 保存线索引
if (data.hasLineInfo && i < cloud.lineIndices.size()) {
int lineIdx = cloud.lineIndices[i];
if (lineIdx >= 0) {
data.lineIndices.push_back(lineIdx);
if (lineIdx + 1 > data.totalLines) {
data.totalLines = lineIdx + 1;
}
}
ptInLineCounter++;
}
// 计算每线点数（假设网格化点云）
@ -328,9 +346,31 @@ void PointCloudGLWidget::addPointCloud(const PointCloudXYZRGB& cloud, const QStr
// 用于按点大小分组的临时 map
std::map<float, std::vector<size_t>> sizeGroupMap;
const float EPSILON = 1e-6f;
int prevLineIdx = -1;
int ptInLineCounter = 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;
}
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;
}
@ -347,9 +387,11 @@ void PointCloudGLWidget::addPointCloud(const PointCloudXYZRGB& cloud, const QStr
// 保存原始索引
data.originalIndices.push_back(static_cast<int>(i));
// 保存线内索引（点在所属线中的位置）
data.pointInLineIndices.push_back(ptInLineCounter);
// 保存线索引
if (data.hasLineInfo && i < cloud.lineIndices.size()) {
int lineIdx = cloud.lineIndices[i];
if (lineIdx >= 0) {
data.lineIndices.push_back(lineIdx);
if (lineIdx + 1 > data.totalLines) {
data.totalLines = lineIdx + 1;
@ -360,6 +402,8 @@ void PointCloudGLWidget::addPointCloud(const PointCloudXYZRGB& cloud, const QStr
if (pt.pointSize > 1.0f) {
sizeGroupMap[pt.pointSize].push_back(pointIndex);
}
ptInLineCounter++;
}
// 构建自定义点大小分组
@ -843,10 +887,9 @@ SelectedPointInfo PointCloudGLWidget::pickPoint(int screenX, int screenY)
// 计算点在线中的原始索引
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;
// 使用预计算的线内索引（支持不等长线）
if (bestIndex < cloudData.pointInLineIndices.size()) {
result.pointIndexInLine = cloudData.pointInLineIndices[bestIndex];
}
}