diff --git a/CloudUtils/Inc/LaserDataLoader.h b/CloudUtils/Inc/LaserDataLoader.h index 5f4e171..2ee550c 100644 --- a/CloudUtils/Inc/LaserDataLoader.h +++ b/CloudUtils/Inc/LaserDataLoader.h @@ -54,6 +54,11 @@ public: std::vector>& scanLines, size_t* validPointCount = nullptr); + // 从文件加载Poly折线数据（带颜色） + // polyLines: 每条折线的点坐标列表，polyLines[i] 是第i条折线的所有点（含RGB颜色） + int LoadPolySegments(const std::string& fileName, + std::vector>& polyLines); + // 获取最后的错误信息 std::string GetLastError() const { return m_lastError; } diff --git a/CloudUtils/Src/LaserDataLoader.cpp b/CloudUtils/Src/LaserDataLoader.cpp index 1acd5f5..3687837 100644 --- a/CloudUtils/Src/LaserDataLoader.cpp +++ b/CloudUtils/Src/LaserDataLoader.cpp @@ -102,6 +102,21 @@ int LaserDataLoader::LoadLaserScanData(const std::string& fileName, nLaserPointIdx = 0; bFindLineNum = false; + } else if (line.find("Poly_") == 0) { + // 跳过 Poly 折线头及后续点坐标数据 + size_t lastUnderscore = line.rfind('_'); + int polyNum = 0; + if (lastUnderscore != std::string::npos && lastUnderscore + 1 < line.size()) { + polyNum = std::atoi(line.c_str() + lastUnderscore + 1); + } + for (int skip = 0; skip < polyNum; ) { + if (!std::getline(inputFile, line)) break; + TrimCarriageReturn(line); + if (!line.empty()) { + ++skip; + } + } + } else if (line.find("{") == 0) { // 使用正则表达式判断是XYZ还是RGBD格式 // XYZ格式: {x,y,z}-{leftX,leftY}-{rightX,rightY} @@ -278,6 +293,141 @@ int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector>& polyLines) +{ + LOG_INFO("Loading poly segments from file: %s\n", fileName.c_str()); + + polyLines.clear(); + + 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 ERR_CODE(FILE_ERR_NOEXIST); + } + + std::string line; + + while (std::getline(inputFile, line)) { + TrimCarriageReturn(line); + + // 跳过空行和注释 + if (line.empty() || line[0] == '#') { + continue; + } + + // 查找 Poly_index_num 头 + if (line.find("Poly_") != 0) { + continue; + } + + // 解析点数量（最后一个下划线后的数字） + size_t lastUnderscore = line.rfind('_'); + if (lastUnderscore == std::string::npos || lastUnderscore + 1 >= line.size()) { + LOG_WARN("[LaserDataLoader] Invalid Poly header: %s\n", line.c_str()); + continue; + } + + int num = std::atoi(line.c_str() + lastUnderscore + 1); + if (num < 2) { + LOG_WARN("[LaserDataLoader] Invalid point count in Poly header: %s\n", line.c_str()); + continue; + } + + // 读取 num 个点坐标 + std::vector points; + for (int i = 0; i < num; ) { + if (!std::getline(inputFile, line)) break; + TrimCarriageReturn(line); + if (line.empty()) { + continue; // 跳过空行，不计数 + } + + SVzNLPointXYZRGBA pt; + memset(&pt, 0, sizeof(pt)); + pt.nRGB = 0x00FFFFFF; // 默认白色 + bool parsed = false; + + // 尝试花括号格式 + if (line.find('{') != std::string::npos) { + float fx, fy, fz; + int R = 255, G = 255, B = 255, A = 255; + + // 新RGBD格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A} + int cnt = sscanf(line.c_str(), + " { %f , %f , %f } - { %*f , %*f } - { %*f , %*f } - { %d , %d , %d , %d }", + &fx, &fy, &fz, &R, &G, &B, &A); + if (cnt >= 3) { + pt.x = fx; + pt.y = fy; + pt.z = fz; + if (cnt >= 7) { + // 解析到了颜色 R,G,B,A（0-255） + int nRGB = B; nRGB <<= 8; nRGB += G; nRGB <<= 8; nRGB += R; + pt.nRGB = nRGB; + } + parsed = true; + } + + // 旧RGBD格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry} + if (!parsed) { + float r, g, b; + if (sscanf(line.c_str(), " { %f , %f , %f , %f , %f , %f }", + &fx, &fy, &fz, &r, &g, &b) == 6) { + pt.x = fx; + pt.y = fy; + pt.z = fz; + int nRGB = (int)(b * 255); nRGB <<= 8; + nRGB += (int)(g * 255); nRGB <<= 8; + nRGB += (int)(r * 255); + pt.nRGB = nRGB; + parsed = true; + } + } + + // XYZ格式: {x,y,z}-{...}-{...} + if (!parsed) { + if (sscanf(line.c_str(), " { %f , %f , %f }", &fx, &fy, &fz) == 3) { + pt.x = fx; + pt.y = fy; + pt.z = fz; + parsed = true; + } + } + } + + // 尝试纯数字格式: x y z（空格/Tab/逗号分隔） + if (!parsed) { + float fx, fy, fz; + std::string lineCopy = line; + for (char& c : lineCopy) { + if (c == ',' || c == '\t') c = ' '; + } + if (sscanf(lineCopy.c_str(), "%f %f %f", &fx, &fy, &fz) == 3) { + pt.x = fx; + pt.y = fy; + pt.z = fz; + parsed = true; + } + } + + if (parsed) { + points.push_back(pt); + } + ++i; + } + + if (points.size() >= 2) { + polyLines.push_back(std::move(points)); + } + } + + inputFile.close(); + LOG_INFO("Loaded %zu poly lines from file: %s\n", polyLines.size(), fileName.c_str()); + return SUCCESS; +} + void LaserDataLoader::FreeLaserScanData(std::vector>& laserLines) { LOG_DEBUG("Freeing unified laser scan data, line count: %zu\n", laserLines.size()); @@ -487,10 +637,39 @@ int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPositi int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNLPointXYZRGBA& sData, SVzNL2DLRPoint& s2DData) { float X, Y, Z; - float r, g, b; float leftX, leftY; float rightX, rightY; - sscanf(data.c_str(), "{%f,%f,%f,%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); + + // 尝试新格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A} + int R, G, B, A; + int parsed = sscanf(data.c_str(), + " { %f , %f , %f } - { %f , %f } - { %f , %f } - { %d , %d , %d , %d }", + &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY, &R, &G, &B, &A); + if (parsed >= 11) { + sData.x = X; + sData.y = Y; + sData.z = Z; + + // R,G,B 是 0-255 整数值 + int nRGB = B; + nRGB <<= 8; + nRGB += G; + nRGB <<= 8; + nRGB += R; + sData.nRGB = nRGB; + + s2DData.sLeft.x = leftX; + s2DData.sLeft.y = leftY; + s2DData.sRight.x = rightX; + s2DData.sRight.y = rightY; + return SUCCESS; + } + + // 旧格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry} r,g,b 为 0~1 浮点值 + float r, g, b; + sscanf(data.c_str(), + " { %f , %f , %f , %f , %f , %f } - { %f , %f } - { %f , %f }", + &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); sData.x = X; sData.y = Y; sData.z = Z; @@ -524,26 +703,37 @@ int LaserDataLoader::_GetLaserType(const std::string& fileName, EVzResultDataTyp bool bFind = false; while (std::getline(inputFile, linedata)) { - // 去除行末的 \r 字符（处理 Windows 格式的 CR LF 换行符） TrimCarriageReturn(linedata); if (linedata.find("{") == 0) { - // 修复正则表达式以匹配实际数据格式 - // XYZ格式: {x,y,z}-{leftX,leftY}-{rightX,rightY} - // RGBD格式: {x,y,z,r,g,b}-{leftX,leftY}-{rightX,rightY} - // 更宽松的正则表达式，允许更多的空格变化 - std::regex xyzPattern(R"(\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\})"); - std::regex rgbdPattern(R"(\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\})"); + // 统计 { 的数量判断格式 + int braceCount = 0; + for (char c : linedata) { + if (c == '{') braceCount++; + } - // 先尝试匹配RGBD格式（6个数字） - if (std::regex_match(linedata, rgbdPattern)) { + if (braceCount == 4) { + // 新RGBD格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A} eDataType = keResultDataType_PointXYZRGBA; bFind = true; - } - // 再尝试匹配XYZ格式（3个数字） - else if (std::regex_match(linedata, xyzPattern)) { - eDataType = keResultDataType_Position; - bFind = true; + } else if (braceCount == 3) { + // 检查第一组的逗号数量区分XYZ和旧RGBD + size_t firstClose = linedata.find('}'); + if (firstClose != std::string::npos) { + int commaCount = 0; + for (size_t i = 0; i < firstClose; ++i) { + if (linedata[i] == ',') commaCount++; + } + if (commaCount >= 5) { + // 旧RGBD格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry} + eDataType = keResultDataType_PointXYZRGBA; + bFind = true; + } else if (commaCount >= 2) { + // XYZ格式: {x,y,z}-{lx,ly}-{rx,ry} + eDataType = keResultDataType_Position; + bFind = true; + } + } } break; } diff --git a/CloudView/Inc/CloudViewMainWindow.h b/CloudView/Inc/CloudViewMainWindow.h index f42418a..c524e63 100644 --- a/CloudView/Inc/CloudViewMainWindow.h +++ b/CloudView/Inc/CloudViewMainWindow.h @@ -44,11 +44,6 @@ private slots: */ void onOpenFile(); - /** - * @brief 打开线段文件 - */ - void onOpenSegmentFile(); - /** * @brief 打开姿态点文件 */ @@ -225,6 +220,18 @@ private: */ void updateLinePointsDialog(); + /** + * @brief 加载点云文件 + * @return 是否成功加载到点云数据 + */ + bool loadPointCloudFile(const QString& fileName); + + /** + * @brief 加载线段文件（Poly格式） + * @return 是否成功加载到线段数据 + */ + bool loadSegmentFile(const QString& fileName); + // 点云显示控件 PointCloudGLWidget* m_glWidget; @@ -233,7 +240,6 @@ private: // 文件操作控件 QPushButton* m_btnOpenFile; - QPushButton* m_btnOpenSegment; QPushButton* m_btnOpenPose; QPushButton* m_btnClearAll; diff --git a/CloudView/Src/CloudViewMainWindow.cpp b/CloudView/Src/CloudViewMainWindow.cpp index 2e09d63..862f693 100644 --- a/CloudView/Src/CloudViewMainWindow.cpp +++ b/CloudView/Src/CloudViewMainWindow.cpp @@ -13,6 +13,7 @@ #include #include #include "VrLog.h" +#include "LaserDataLoader.h" CloudViewMainWindow::CloudViewMainWindow(QWidget* parent) : QMainWindow(parent) @@ -125,18 +126,12 @@ QGroupBox* CloudViewMainWindow::createFileGroup() layout->setSpacing(3); layout->setContentsMargins(5, 5, 5, 5); - m_btnOpenFile = new QPushButton("打开点云", group); + m_btnOpenFile = new QPushButton("打开文件", group); m_btnOpenFile->setMinimumHeight(24); m_btnOpenFile->setMaximumHeight(24); connect(m_btnOpenFile, &QPushButton::clicked, this, &CloudViewMainWindow::onOpenFile); layout->addWidget(m_btnOpenFile); - m_btnOpenSegment = new QPushButton("打开线段 {x,y,z}-{x,y,z}", group); - m_btnOpenSegment->setMinimumHeight(24); - m_btnOpenSegment->setMaximumHeight(24); - connect(m_btnOpenSegment, &QPushButton::clicked, this, &CloudViewMainWindow::onOpenSegmentFile); - layout->addWidget(m_btnOpenSegment); - m_btnOpenPose = new QPushButton("打开姿态点 {x,y,z}-{r,p,y}", group); m_btnOpenPose->setMinimumHeight(24); m_btnOpenPose->setMaximumHeight(24); @@ -523,6 +518,7 @@ QGroupBox* CloudViewMainWindow::createLineGroup() m_btnSelectByNumber->setMaximumHeight(24); m_btnSelectByNumber->setMaximumWidth(50); connect(m_btnSelectByNumber, &QPushButton::clicked, this, &CloudViewMainWindow::onSelectLineByNumber); + connect(m_lineNumberInput, &QLineEdit::returnPressed, this, &CloudViewMainWindow::onSelectLineByNumber); inputLayout->addWidget(m_lineNumberInput, 1); inputLayout->addWidget(m_btnSelectByNumber); layout->addLayout(inputLayout); @@ -579,17 +575,42 @@ QGroupBox* CloudViewMainWindow::createCloudListGroup() void CloudViewMainWindow::onOpenFile() { + // 如果已有打开的文件，提示是否清除 + if (m_cloudList->count() > 0) { + auto ret = QMessageBox::question(this, "提示", + "当前已有打开的文件，是否清除？", + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); + if (ret == QMessageBox::Cancel) { + return; + } + if (ret == QMessageBox::Yes) { + onClearAll(); + } + } + QString fileName = QFileDialog::getOpenFileName( this, - "打开点云文件", + "打开文件", QString(), - "点云文件 (*.pcd *.txt);;PCD 文件 (*.pcd);;TXT 文件 (*.txt);;所有文件 (*.*)" + "所有支持格式 (*.pcd *.txt);;PCD 文件 (*.pcd);;TXT 文件 (*.txt);;所有文件 (*.*)" ); if (fileName.isEmpty()) { return; } + // 同一文件中可能同时包含点云和线段，两者都尝试加载 + bool cloudOk = loadPointCloudFile(fileName); + bool segmentOk = loadSegmentFile(fileName); + + if (!cloudOk && !segmentOk) { + QMessageBox::critical(this, "错误", "无法从文件中加载点云或线段数据"); + statusBar()->showMessage("加载失败"); + } +} + +bool CloudViewMainWindow::loadPointCloudFile(const QString& fileName) +{ statusBar()->showMessage("正在加载点云..."); QFileInfo fileInfo(fileName); @@ -601,9 +622,8 @@ void CloudViewMainWindow::onOpenFile() int result = m_converter->loadFromFile(fileName.toStdString(), rgbCloud); if (result != 0) { - QMessageBox::critical(this, "错误", QString("加载点云失败: %1").arg(QString::fromStdString(m_converter->getLastError()))); - statusBar()->showMessage("加载失败"); - return; + LOG_INFO("[CloudView] Load point cloud failed: %s\n", m_converter->getLastError().c_str()); + return false; } // 保存原始完整点云 XYZ（用于旋转/线上点等功能） @@ -632,11 +652,9 @@ void CloudViewMainWindow::onOpenFile() } if (hadColor) { - // 有颜色数据：使用 addPointCloud(PointCloudXYZRGB) 显示原始颜色 m_glWidget->addPointCloud(rgbCloud, cloudName); LOG_INFO("[CloudView] Loaded with original color, points: %zu\n", rgbCloud.size()); } else { - // 无颜色数据：使用 addPointCloud(PointCloudXYZ) 显示（颜色表轮换） m_glWidget->addPointCloud(m_originalCloud, cloudName); LOG_INFO("[CloudView] Loaded without color (color table), points: %zu\n", m_originalCloud.size()); } @@ -664,86 +682,60 @@ void CloudViewMainWindow::onOpenFile() m_cloudList->addItem(itemText); statusBar()->showMessage(QString("已加载 %1 个点%2").arg(m_converter->getLoadedPointCount()).arg(hadColor ? " (彩色)" : "")); + return true; } -void CloudViewMainWindow::onOpenSegmentFile() +bool CloudViewMainWindow::loadSegmentFile(const QString& fileName) { - QString fileName = QFileDialog::getOpenFileName( - this, - "打开线段文件", - QString(), - "文本文件 (*.txt);;所有文件 (*.*)" - ); - - if (fileName.isEmpty()) { - return; - } - statusBar()->showMessage("正在加载线段..."); - QFile file(fileName); - if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { - QMessageBox::critical(this, "错误", QString("无法打开文件: %1").arg(fileName)); - statusBar()->showMessage("加载失败"); - return; + LaserDataLoader loader; + std::vector> polyLines; + int result = loader.LoadPolySegments(fileName.toStdString(), polyLines); + + if (result != 0) { + LOG_INFO("[CloudView] Load segments failed: %s\n", loader.GetLastError().c_str()); + return false; } + if (polyLines.empty()) { + return false; + } + + // 将折线点转换为线段（使用点自带的颜色） QVector segments; - QTextStream in(&file); - int lineNum = 0; - int validCount = 0; - - while (!in.atEnd()) { - QString line = in.readLine().trimmed(); - lineNum++; - - // 跳过空行和注释 - if (line.isEmpty() || line.startsWith('#')) { - continue; + for (const auto& polyLine : polyLines) { + for (size_t i = 0; i + 1 < polyLine.size(); ++i) { + const auto& p1 = polyLine[i]; + const auto& p2 = polyLine[i + 1]; + // 取起点颜色作为线段颜色，nRGB 为 BGR 打包格式 + float r = ((p1.nRGB >> 0) & 0xFF) / 255.0f; + float g = ((p1.nRGB >> 8) & 0xFF) / 255.0f; + float b = ((p1.nRGB >> 16) & 0xFF) / 255.0f; + segments.append(LineSegment( + p1.x, p1.y, p1.z, + p2.x, p2.y, p2.z, + r, g, b)); } - - // 解析格式：{x,y,z}-{x,y,z} - QRegExp regex("\\{([^}]+)\\}-\\{([^}]+)\\}"); - if (regex.indexIn(line) == -1) { - LOG_WARN("[CloudView] Line %d: Invalid format, expected {x,y,z}-{x,y,z}\n", lineNum); - continue; - } - - QString point1Str = regex.cap(1); - QString point2Str = regex.cap(2); - - QStringList p1 = point1Str.split(','); - QStringList p2 = point2Str.split(','); - - if (p1.size() != 3 || p2.size() != 3) { - LOG_WARN("[CloudView] Line %d: Invalid point format\n", lineNum); - continue; - } - - bool ok = true; - float x1 = p1[0].toFloat(&ok); if (!ok) continue; - float y1 = p1[1].toFloat(&ok); if (!ok) continue; - float z1 = p1[2].toFloat(&ok); if (!ok) continue; - float x2 = p2[0].toFloat(&ok); if (!ok) continue; - float y2 = p2[1].toFloat(&ok); if (!ok) continue; - float z2 = p2[2].toFloat(&ok); if (!ok) continue; - - // 默认白色 - segments.append(LineSegment(x1, y1, z1, x2, y2, z2, 1.0f, 1.0f, 1.0f)); - validCount++; } - file.close(); - if (segments.isEmpty()) { - QMessageBox::warning(this, "警告", "文件中没有有效的线段数据"); - statusBar()->showMessage("加载失败"); - return; + return false; } m_glWidget->addLineSegments(segments); - statusBar()->showMessage(QString("已加载 %1 条线段").arg(validCount)); - LOG_INFO("[CloudView] Loaded %d line segments from %s\n", validCount, fileName.toStdString().c_str()); + + // 添加到列表 + QFileInfo fileInfo(fileName); + int totalPolyCount = static_cast(polyLines.size()); + QString itemName = QString("Segments (%1)").arg(fileInfo.fileName()); + m_cloudList->addItem(QString("%1 - %2 条折线, %3 条线段") + .arg(itemName).arg(totalPolyCount).arg(segments.size())); + + statusBar()->showMessage(QString("已加载 %1 条折线, %2 条线段").arg(totalPolyCount).arg(segments.size())); + LOG_INFO("[CloudView] Loaded %d polylines, %d segments from %s\n", + totalPolyCount, segments.size(), fileName.toStdString().c_str()); + return true; } void CloudViewMainWindow::onOpenPoseFile() @@ -831,6 +823,8 @@ void CloudViewMainWindow::onOpenPoseFile() void CloudViewMainWindow::onClearAll() { m_glWidget->clearPointClouds(); + m_glWidget->clearLineSegments(); + m_glWidget->clearPosePoints(); m_cloudList->clear(); m_cloudCount = 0; m_currentLineNum = 0; diff --git a/CloudView/main.cpp b/CloudView/main.cpp index a335f78..7a5a652 100644 --- a/CloudView/main.cpp +++ b/CloudView/main.cpp @@ -3,7 +3,7 @@ #include #include "CloudViewMainWindow.h" -#define APP_VERSION "1.1.0" +#define APP_VERSION "1.1.1" int main(int argc, char* argv[]) {