diff --git a/CloudUtils/Src/LaserDataLoader.cpp b/CloudUtils/Src/LaserDataLoader.cpp index 7ad74f7..f324c56 100644 --- a/CloudUtils/Src/LaserDataLoader.cpp +++ b/CloudUtils/Src/LaserDataLoader.cpp @@ -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(sLaserData.p3DPoint); SVzNL2DLRPoint* p2DPoints = static_cast(sLaserData.p2DPoint); diff --git a/CloudView/Inc/PointCloudGLWidget.h b/CloudView/Inc/PointCloudGLWidget.h index e418ec0..fb0b55e 100644 --- a/CloudView/Inc/PointCloudGLWidget.h +++ b/CloudView/Inc/PointCloudGLWidget.h @@ -287,6 +287,7 @@ private: std::vector colors; std::vector lineIndices; // 每个点所属的线索引 std::vector originalIndices; // 每个显示点在原始点云中的索引（用于计算原始index） + std::vector 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); diff --git a/CloudView/Src/PointCloudGLWidget.cpp b/CloudView/Src/PointCloudGLWidget.cpp index dee2ac8..60b1d2d 100644 --- a/CloudView/Src/PointCloudGLWidget.cpp +++ b/CloudView/Src/PointCloudGLWidget.cpp @@ -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(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> 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(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]; } }