支持新格式数据的读取

This commit is contained in:
杰仔 2026-07-03 12:46:19 +08:00
parent 9fbc9e7edf
commit c1cfe7e560
8 changed files with 580 additions and 57 deletions

View File

@ -85,6 +85,14 @@ private:
// 获取激光数据类型
int _GetLaserType(const std::string& fileName, EVzResultDataType& eDataType);
// Read binary .dat laser scan data.
int _LoadLaserScanDataFromDatFile(const std::string& fileName,
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
int& lineNum,
float& scanSpeed,
int& maxTimeStamp,
int& clockPerSecond);
std::string m_lastError;
static const int VZ_LASER_LINE_PT_MAX_NUM = 4096;
};

View File

@ -1,10 +1,15 @@
#include "LaserDataLoader.h"
#include <algorithm>
#include <cctype>
#include <cstdio>
#include <cstdint>
#include <cstring>
#include <cstdlib>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <memory>
#include <new>
#include <sstream>
#include <stdexcept>
#include <regex>
@ -19,6 +24,112 @@ static inline void TrimCarriageReturn(std::string& str)
}
}
namespace
{
struct DatValue2DFloat
{
double x;
double y;
double value;
};
struct Dat3DPoint
{
double X;
double Y;
double Z;
};
struct Dat3DFeaturePoint
{
int index;
int flag;
DatValue2DFloat stereo;
DatValue2DFloat stereoRight;
Dat3DPoint eye;
Dat3DPoint world;
std::uint64_t next;
};
struct DatLaserScan3DLinesHeader
{
char dataCalibrated;
char adjusted;
int scanLineNum;
int vr1;
int vr2;
double lineV;
int clock;
std::uint32_t maxTimeStamp;
std::uint64_t scanLineList;
};
struct DatLineScan3DData
{
int lineIndex;
std::uint32_t timeStamp;
double lineStepping;
};
static_assert(sizeof(DatLaserScan3DLinesHeader) == 40, "Unexpected dat header layout");
static_assert(sizeof(Dat3DFeaturePoint) == 112, "Unexpected dat feature point layout");
template <typename T>
bool ReadBinary(std::ifstream& inputFile, T& value)
{
inputFile.read(reinterpret_cast<char*>(&value), sizeof(T));
return static_cast<bool>(inputFile);
}
bool HasFileExtension(const std::string& fileName, const char* extension)
{
const size_t dotPos = fileName.find_last_of('.');
if (dotPos == std::string::npos) {
return false;
}
const size_t slashPos = fileName.find_last_of("/\\");
if (slashPos != std::string::npos && dotPos < slashPos) {
return false;
}
std::string fileExt = fileName.substr(dotPos);
std::transform(fileExt.begin(), fileExt.end(), fileExt.begin(),
[](unsigned char c) { return static_cast<char>(std::tolower(c)); });
return fileExt == extension;
}
bool LooksLikeTextLaserFile(const std::string& fileName)
{
std::ifstream inputFile(fileName, std::ios::binary);
if (!inputFile.is_open()) {
return false;
}
char header[8] = {};
inputFile.read(header, sizeof(header));
return inputFile.gcount() == sizeof(header) && std::memcmp(header, "LineNum:", sizeof(header)) == 0;
}
bool ReadDatLineRecord(std::ifstream& inputFile, int recordSize, DatLineScan3DData& lineData)
{
char buffer[40] = {};
if (recordSize <= 0 || recordSize > static_cast<int>(sizeof(buffer))) {
return false;
}
inputFile.read(buffer, recordSize);
if (!inputFile) {
return false;
}
std::memcpy(&lineData.lineIndex, buffer, sizeof(lineData.lineIndex));
std::memcpy(&lineData.timeStamp, buffer + 4, sizeof(lineData.timeStamp));
std::memcpy(&lineData.lineStepping, buffer + 8, sizeof(lineData.lineStepping));
return true;
}
}
LaserDataLoader::LaserDataLoader()
{
m_lastError.clear();
@ -44,6 +155,10 @@ int LaserDataLoader::LoadLaserScanData(const std::string& fileName,
maxTimeStamp = 0;
clockPerSecond = 0;
if (HasFileExtension(fileName, ".dat") && !LooksLikeTextLaserFile(fileName)) {
return _LoadLaserScanDataFromDatFile(fileName, laserLines, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
}
// 判断文件类型
std::ifstream inputFile(fileName);
if (!inputFile.is_open()) {
@ -161,6 +276,170 @@ int LaserDataLoader::LoadLaserScanData(const std::string& fileName,
return SUCCESS;
}
// Read binary .dat laser scan data.
int LaserDataLoader::_LoadLaserScanDataFromDatFile(const std::string& fileName,
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
int& lineNum,
float& scanSpeed,
int& maxTimeStamp,
int& clockPerSecond)
{
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 ERR_CODE(FILE_ERR_NOEXIST);
}
auto fail = [&](int code, const std::string& message) -> int {
m_lastError = message;
LOG_ERROR("%s\n", message.c_str());
FreeLaserScanData(laserLines);
lineNum = 0;
scanSpeed = 0.0f;
maxTimeStamp = 0;
clockPerSecond = 0;
return 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 kFeaturePointSize = 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)) {
return false;
}
pos += static_cast<std::streamoff>(sizeof(pointCount));
if (pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) {
return false;
}
const std::streamoff pointBytes =
static_cast<std::streamoff>(pointCount) * static_cast<std::streamoff>(kFeaturePointSize);
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;
laserLines.clear();
laserLines.reserve(static_cast<size_t>(lineNum));
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);
}
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);
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);
}
lineData.p3DPoint = points3D.get();
lineData.p2DPoint = points2D.get();
}
laserLines.push_back(std::make_pair(keResultDataType_Position, lineData));
points3D.release();
points2D.release();
}
LOG_INFO("Successfully loaded %d dat laser scan lines from file: %s\n", lineNum, fileName.c_str());
return SUCCESS;
}
// 保存激光扫描数据到文件 - 统一接口,支持两种类型的数据
int LaserDataLoader::SaveLaserScanData(const std::string& fileName,
const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
@ -487,12 +766,58 @@ void LaserDataLoader::FreeLaserScanData(std::vector<std::pair<EVzResultDataType,
SVzLaserLineData& lineData = linePair.second;
if (lineData.p3DPoint) {
delete[] lineData.p3DPoint;
switch (dataType) {
case keResultDataType_Position:
delete[] static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
break;
case keResultDataType_PositionF:
delete[] static_cast<SVzNL3DPositionF*>(lineData.p3DPoint);
break;
case keResultDataType_PointF:
delete[] static_cast<SVzNL3DPointF*>(lineData.p3DPoint);
break;
case keResultDataType_PointXYZ:
delete[] static_cast<SVzNLPointXYZ*>(lineData.p3DPoint);
break;
case keResultDataType_PointXYZRGBA:
delete[] static_cast<SVzNLPointXYZRGBA*>(lineData.p3DPoint);
break;
case keResultDataType_PointGray:
delete[] static_cast<SVzNLPointXYZGray*>(lineData.p3DPoint);
break;
case keResultDataType_PointXYZI:
delete[] static_cast<SVzNLPointXYZI*>(lineData.p3DPoint);
break;
case keResultDataType_PointRGBA_D:
delete[] static_cast<SVzNLPointRGBA_D*>(lineData.p3DPoint);
break;
default:
LOG_WARN("Unknown laser data type when freeing p3DPoint: %d\n", dataType);
break;
}
lineData.p3DPoint = nullptr;
}
if (lineData.p2DPoint) {
delete[] lineData.p2DPoint;
switch (dataType) {
case keResultDataType_Position:
delete[] static_cast<SVzNL2DPosition*>(lineData.p2DPoint);
break;
case keResultDataType_PositionF:
delete[] static_cast<SVzNL2DPositionF*>(lineData.p2DPoint);
break;
case keResultDataType_PointF:
case keResultDataType_PointXYZ:
case keResultDataType_PointXYZRGBA:
case keResultDataType_PointGray:
case keResultDataType_PointXYZI:
case keResultDataType_PointRGBA_D:
delete[] static_cast<SVzNL2DLRPoint*>(lineData.p2DPoint);
break;
default:
LOG_WARN("Unknown laser data type when freeing p2DPoint: %d\n", dataType);
break;
}
lineData.p2DPoint = nullptr;
}
}

View File

@ -167,6 +167,7 @@ private slots:
void onRotateCloudByZ();
void onSavePointCloud(); // 保存 txt 点云
void onSavePlyPcdCloud(); // 保存 ply/pcd 点云
void onBatchConvertTxtToPly(); // 批量转换 txt -> ply
void onConvertEulerMatrix();
/**

View File

@ -54,7 +54,7 @@ using PointCloudXYZRGB = SimplePointCloud<Point3DRGB>;
/**
* @brief
* txt pcd
* txt/datpcdply
*/
class PointCloudConverter
{
@ -63,7 +63,7 @@ public:
~PointCloudConverter();
/**
* @brief txt 使 CloudUtils
* @brief txt/dat 使 CloudUtils
*/
int loadFromTxt(const std::string& fileName, PointCloudXYZ& cloud);
int loadFromTxt(const std::string& fileName, PointCloudXYZRGB& cloud);

View File

@ -27,6 +27,8 @@
#include <QSettings>
#include <QProgressDialog>
#include <QCoreApplication>
#include <QDir>
#include <QDirIterator>
#include <cmath>
#include <cctype>
#include <algorithm>
@ -421,6 +423,9 @@ void CloudViewMainWindow::setupUI()
QAction* actSavePlyPcd = fileMenu->addAction("保存ply/pcd点云...\tCtrl+Shift+S");
actSavePlyPcd->setShortcut(QKeySequence("Ctrl+Shift+S"));
connect(actSavePlyPcd, &QAction::triggered, this, &CloudViewMainWindow::onSavePlyPcdCloud);
fileMenu->addSeparator();
QAction* actBatchConvert = fileMenu->addAction("批量转换txt->ply...");
connect(actBatchConvert, &QAction::triggered, this, &CloudViewMainWindow::onBatchConvertTxtToPly);
// ── 点云菜单 ──
QMenu* cloudMenu = menuBar()->addMenu("点云");
@ -1136,7 +1141,7 @@ void CloudViewMainWindow::onOpenFile()
this,
"打开文件",
QString(),
"所有支持格式 (*.pcd *.ply *.txt);;PCD 文件 (*.pcd);;PLY 文件 (*.ply);;TXT 文件 (*.txt);;所有文件 (*.*)"
"所有支持格式 (*.pcd *.ply *.txt *.dat);;PCD 文件 (*.pcd);;PLY 文件 (*.ply);;TXT 文件 (*.txt);;DAT 文件 (*.dat);;所有文件 (*.*)"
);
if (fileName.isEmpty()) {
@ -2655,6 +2660,189 @@ void CloudViewMainWindow::onSavePlyPcdCloud()
cloud.points.size(), fileName.toStdString().c_str());
}
void CloudViewMainWindow::onBatchConvertTxtToPly()
{
// 选择源文件夹
const QString srcDir = QFileDialog::getExistingDirectory(
this, "选择源文件夹(包含 txt 点云文件)", QString(),
QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
if (srcDir.isEmpty()) return;
// 选择目标文件夹
const QString dstDir = QFileDialog::getExistingDirectory(
this, "选择目标文件夹PLY 文件输出位置)", srcDir,
QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
if (dstDir.isEmpty()) return;
// 递归收集所有 .txt 文件及其相对路径
struct FileEntry
{
QString absolutePath;
QString relativePath; // 相对于源文件夹的路径,用于构建目标路径
};
QVector<FileEntry> fileEntries;
{
QDirIterator it(srcDir, QStringList("*.txt"), QDir::Files,
QDirIterator::Subdirectories | QDirIterator::FollowSymlinks);
while (it.hasNext()) {
it.next();
FileEntry entry;
entry.absolutePath = it.filePath();
// 计算相对路径(文件路径去掉源目录前缀)
entry.relativePath = QDir(srcDir).relativeFilePath(it.filePath());
fileEntries.append(entry);
}
}
if (fileEntries.isEmpty()) {
QMessageBox::information(this, "提示",
QString("源文件夹及其子目录中没有找到 .txt 文件:\n%1").arg(srcDir));
return;
}
// 按相对路径排序
std::sort(fileEntries.begin(), fileEntries.end(),
[](const FileEntry& a, const FileEntry& b) {
return a.relativePath < b.relativePath;
});
// 进度对话框
QProgressDialog progress("正在批量转换 txt -> ply...", "取消", 0, fileEntries.size(), this);
progress.setWindowTitle("批量转换");
progress.setWindowModality(Qt::WindowModal);
progress.setMinimumDuration(0);
progress.setAutoClose(true);
// 使用独立的转换器,避免干扰主窗口状态
PointCloudConverter converter;
int successCount = 0;
int failCount = 0;
int skipCount = 0;
QStringList failedFiles;
const float kZeroEps = 1e-6f;
for (int i = 0; i < fileEntries.size(); ++i) {
if (progress.wasCanceled()) {
statusBar()->showMessage("批量转换已取消");
break;
}
const FileEntry& entry = fileEntries[i];
const QString& srcPath = entry.absolutePath;
const QString& relPath = entry.relativePath;
// 构建目标路径:目标文件夹 + 相对路径,扩展名改为 .ply
const QString baseName = QFileInfo(relPath).completeBaseName();
const QString relDir = QFileInfo(relPath).path(); // 相对路径中的目录部分
const QString dstRelPath = (relDir == "." ? baseName + ".ply"
: relDir + "/" + baseName + ".ply");
const QString dstPath = dstDir + "/" + dstRelPath;
// 确保目标子目录存在
const QString dstParentDir = QFileInfo(dstPath).absolutePath();
if (!QDir().mkpath(dstParentDir)) {
failCount++;
failedFiles.append(QString("%1 (无法创建目标目录)").arg(relPath));
LOG_WARN("[CloudView] Batch convert: failed to create dir %s\n",
dstParentDir.toStdString().c_str());
continue;
}
// 显示简短的相对路径便于阅读
const QString displayPath = relPath.length() > 60
? ("..." + relPath.right(57)) : relPath;
progress.setLabelText(QString("正在转换: %1 (%2/%3)")
.arg(displayPath).arg(i + 1).arg(fileEntries.size()));
progress.setValue(i);
QCoreApplication::processEvents();
// 加载 txt 点云
PointCloudXYZ cloud;
int loadRet = converter.loadFromTxt(srcPath.toStdString(), cloud);
if (loadRet != 0) {
failCount++;
failedFiles.append(QString("%1 (加载失败: %2)")
.arg(relPath)
.arg(QString::fromStdString(converter.getLastError())));
LOG_WARN("[CloudView] Batch convert: failed to load %s\n",
srcPath.toStdString().c_str());
continue;
}
// 过滤掉零点,构建用于保存的点云
PointCloudXYZ filteredCloud;
filteredCloud.reserve(cloud.size());
for (const auto& pt : cloud.points) {
if (std::fabs(pt.x) < kZeroEps &&
std::fabs(pt.y) < kZeroEps &&
std::fabs(pt.z) < kZeroEps)
continue;
filteredCloud.push_back(pt);
}
if (filteredCloud.empty()) {
failCount++;
failedFiles.append(QString("%1 (无有效点)").arg(relPath));
LOG_WARN("[CloudView] Batch convert: no valid points in %s\n",
srcPath.toStdString().c_str());
continue;
}
// 保存为 PLY
int saveRet = converter.saveToPly(dstPath.toStdString(), filteredCloud);
if (saveRet != 0) {
failCount++;
failedFiles.append(QString("%1 (保存失败: %2)")
.arg(relPath)
.arg(QString::fromStdString(converter.getLastError())));
LOG_WARN("[CloudView] Batch convert: failed to save %s\n",
dstPath.toStdString().c_str());
continue;
}
successCount++;
LOG_INFO("[CloudView] Batch convert: %s -> %s (%zu points)\n",
srcPath.toStdString().c_str(),
dstPath.toStdString().c_str(),
filteredCloud.size());
}
progress.setValue(fileEntries.size());
// 汇总结果
const int totalCount = fileEntries.size();
QString summary = QString("批量转换完成!\n\n"
"总文件数: %1\n"
"成功: %2\n"
"失败: %3")
.arg(totalCount)
.arg(successCount)
.arg(failCount);
if (skipCount > 0) {
summary += QString("\n跳过: %1").arg(skipCount);
}
if (!failedFiles.isEmpty()) {
summary += "\n\n失败文件:\n";
// 最多显示前 10 个失败文件
const int maxShow = std::min(failedFiles.size(), 10);
for (int i = 0; i < maxShow; ++i) {
summary += QString(" • %1\n").arg(failedFiles[i]);
}
if (failedFiles.size() > maxShow) {
summary += QString(" ... 还有 %1 个\n")
.arg(failedFiles.size() - maxShow);
}
}
QMessageBox::information(this, "转换结果", summary);
statusBar()->showMessage(
QString("批量转换完成: %1 成功, %2 失败").arg(successCount).arg(failCount));
}
void CloudViewMainWindow::onRotateCloudByZ()
{
if (m_glWidget->getCloudCount() == 0) {

View File

@ -444,7 +444,7 @@ int PointCloudConverter::loadFromFile(const std::string& fileName, PointCloudXYZ
if (ext == "pcd") {
return loadFromPcd(fileName, cloud);
} else if (ext == "txt") {
} else if (ext == "txt" || ext == "dat") {
return loadFromTxt(fileName, cloud);
} else if (ext == "ply") {
return loadFromPly(fileName, cloud);
@ -460,7 +460,7 @@ int PointCloudConverter::loadFromFile(const std::string& fileName, PointCloudXYZ
if (ext == "pcd") {
return loadFromPcd(fileName, cloud);
} else if (ext == "txt") {
} else if (ext == "txt" || ext == "dat") {
return loadFromTxt(fileName, cloud);
} else if (ext == "ply") {
return loadFromPly(fileName, cloud);

View File

@ -3,7 +3,7 @@
#include <QIcon>
#include "CloudViewMainWindow.h"
#define APP_VERSION "1.1.5"
#define APP_VERSION "1.1.6"
int main(int argc, char* argv[])
{

View File

@ -2,6 +2,7 @@
#include <iostream>
#include <cstring>
#include <cstdio>
#include <stdarg.h>
#include <clocale>
#include <mutex>
@ -311,9 +312,9 @@ void VrLogUtils::EchoLog(VrLogLevel eLogLevel, const char* sFilePath, const int
va_start(args, sFormat);
char szLogInfo[1024] = { 0 };
#ifdef _WIN32
vsprintf_s(szLogInfo, sFormat, args);
vsnprintf_s(szLogInfo, sizeof(szLogInfo), _TRUNCATE, sFormat, args);
#else
vsprintf(szLogInfo, sFormat, args);
vsnprintf(szLogInfo, sizeof(szLogInfo), sFormat, args);
#endif
va_end(args);
log4cpp::Category& root = log4cpp::Category::getRoot();