2008 lines
75 KiB
C++
2008 lines
75 KiB
C++
#include "LaserDataLoader.h"
|
||
#include <algorithm>
|
||
#include <cctype>
|
||
#include <cstdarg>
|
||
#include <cstdio>
|
||
#include <cstdint>
|
||
#include <cstring>
|
||
#include <cstdlib>
|
||
#include <cmath>
|
||
#include <fstream>
|
||
#include <iomanip>
|
||
#include <memory>
|
||
#include <new>
|
||
#include <sstream>
|
||
#include <stdexcept>
|
||
#include <regex>
|
||
|
||
#include "VrLog.h"
|
||
|
||
// 辅助函数：去除字符串末尾的 \r 字符（处理 Windows 格式的 CR LF 换行符）
|
||
static inline void TrimCarriageReturn(std::string& str)
|
||
{
|
||
if (!str.empty() && str.back() == '\r') {
|
||
str.pop_back();
|
||
}
|
||
}
|
||
|
||
namespace
|
||
{
|
||
bool IsShapeBlockHeader(const std::string& line)
|
||
{
|
||
static const char* prefixes[] = {
|
||
"Segment_", "Segments_",
|
||
"Rect_", "Rectangle_", "Surface_", "Face_", "Circle_", "Poly_", "Shape_"
|
||
};
|
||
for (const char* prefix : prefixes) {
|
||
if (line.find(prefix) == 0) {
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
int ShapeBlockPointCount(const std::string& line)
|
||
{
|
||
const size_t lastUnderscore = line.rfind('_');
|
||
if (lastUnderscore == std::string::npos || lastUnderscore + 1 >= line.size()) {
|
||
return 0;
|
||
}
|
||
const int count = std::atoi(line.c_str() + lastUnderscore + 1);
|
||
return count > 0 ? count : 0;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
void AppendFormattedLine(std::string& output, const char* format, ...)
|
||
{
|
||
char stackBuffer[256];
|
||
|
||
va_list args;
|
||
va_start(args, format);
|
||
va_list argsCopy;
|
||
va_copy(argsCopy, args);
|
||
int written = vsnprintf(stackBuffer, sizeof(stackBuffer), format, args);
|
||
va_end(args);
|
||
|
||
if (written < 0) {
|
||
va_end(argsCopy);
|
||
return;
|
||
}
|
||
|
||
if (static_cast<size_t>(written) < sizeof(stackBuffer)) {
|
||
output.append(stackBuffer, static_cast<size_t>(written));
|
||
va_end(argsCopy);
|
||
return;
|
||
}
|
||
|
||
std::vector<char> dynamicBuffer(static_cast<size_t>(written) + 1);
|
||
vsnprintf(dynamicBuffer.data(), dynamicBuffer.size(), format, argsCopy);
|
||
va_end(argsCopy);
|
||
output.append(dynamicBuffer.data(), static_cast<size_t>(written));
|
||
}
|
||
|
||
void FilterPositionLineInPlace(SVzLaserLineData& lineData, int sourcePointCount, const PointCloudXYZRange& range,
|
||
EVzResultDataType dataType, std::vector<int>* sourcePointIndices)
|
||
{
|
||
if (sourcePointIndices)
|
||
{
|
||
sourcePointIndices->clear();
|
||
}
|
||
if (!lineData.p3DPoint)
|
||
{
|
||
return;
|
||
}
|
||
|
||
SVzNL3DPosition* points = static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
|
||
const int count = std::max(0, std::min(sourcePointCount, lineData.nPointCount));
|
||
if (sourcePointIndices)
|
||
{
|
||
sourcePointIndices->reserve(static_cast<size_t>(count));
|
||
}
|
||
if (!range.enabled)
|
||
{
|
||
for (int index = 0; index < count; ++index)
|
||
{
|
||
if (sourcePointIndices)
|
||
{
|
||
sourcePointIndices->push_back(points[index].nPointIdx);
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
// 默认保留扫描线槽位，仅将范围外点的 XYZ 坐标清零。这样可以
|
||
// 保持原始扫描线长度及点索引，供网格/横向选线等调用方使用。
|
||
if (range.filterMode != PointCloudXYZRangeFilterMode::Remove)
|
||
{
|
||
for (int index = 0; index < count; ++index)
|
||
{
|
||
SVzNL3DPosition& point = points[index];
|
||
if (!range.contains(point.pt3D.x, point.pt3D.y, point.pt3D.z))
|
||
{
|
||
point.pt3D.x = 0.0;
|
||
point.pt3D.y = 0.0;
|
||
point.pt3D.z = 0.0;
|
||
}
|
||
if (sourcePointIndices)
|
||
{
|
||
sourcePointIndices->push_back(point.nPointIdx);
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
int keptCount = 0;
|
||
for (int index = 0; index < count; ++index)
|
||
{
|
||
const SVzNL3DPosition& point = points[index];
|
||
if (!range.contains(point.pt3D.x, point.pt3D.y, point.pt3D.z))
|
||
{
|
||
continue;
|
||
}
|
||
|
||
if (keptCount != index)
|
||
{
|
||
points[keptCount] = point;
|
||
if (lineData.p2DPoint && dataType == keResultDataType_PositionF)
|
||
{
|
||
SVzNL2DPositionF* points2D =
|
||
static_cast<SVzNL2DPositionF*>(lineData.p2DPoint);
|
||
points2D[keptCount] = points2D[index];
|
||
}
|
||
else if (lineData.p2DPoint)
|
||
{
|
||
SVzNL2DPosition* points2D =
|
||
static_cast<SVzNL2DPosition*>(lineData.p2DPoint);
|
||
points2D[keptCount] = points2D[index];
|
||
}
|
||
}
|
||
if (sourcePointIndices)
|
||
{
|
||
sourcePointIndices->push_back(point.nPointIdx);
|
||
}
|
||
++keptCount;
|
||
}
|
||
lineData.nPointCount = keptCount;
|
||
}
|
||
|
||
void FilterRgbaLineInPlace(SVzLaserLineData& lineData, int sourcePointCount, const PointCloudXYZRange& range,
|
||
std::vector<int>* sourcePointIndices)
|
||
{
|
||
if (sourcePointIndices)
|
||
{
|
||
sourcePointIndices->clear();
|
||
}
|
||
if (!lineData.p3DPoint)
|
||
{
|
||
return;
|
||
}
|
||
|
||
SVzNLPointXYZRGBA* points = static_cast<SVzNLPointXYZRGBA*>(lineData.p3DPoint);
|
||
SVzNL2DLRPoint* points2D = static_cast<SVzNL2DLRPoint*>(lineData.p2DPoint);
|
||
const int count = std::max(0, std::min(sourcePointCount, lineData.nPointCount));
|
||
if (sourcePointIndices)
|
||
{
|
||
sourcePointIndices->reserve(static_cast<size_t>(count));
|
||
}
|
||
if (!range.enabled)
|
||
{
|
||
for (int index = 0; index < count; ++index)
|
||
{
|
||
if (sourcePointIndices)
|
||
{
|
||
sourcePointIndices->push_back(index);
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
// 默认保留扫描线槽位，仅将范围外点的 XYZ 坐标清零；颜色和 2D
|
||
// 坐标保持不变，以免破坏原始点与图像坐标的对应关系。
|
||
if (range.filterMode != PointCloudXYZRangeFilterMode::Remove)
|
||
{
|
||
for (int index = 0; index < count; ++index)
|
||
{
|
||
SVzNLPointXYZRGBA& point = points[index];
|
||
if (!range.contains(point.x, point.y, point.z))
|
||
{
|
||
point.x = 0.0f;
|
||
point.y = 0.0f;
|
||
point.z = 0.0f;
|
||
}
|
||
if (sourcePointIndices)
|
||
{
|
||
sourcePointIndices->push_back(index);
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
int keptCount = 0;
|
||
for (int index = 0; index < count; ++index)
|
||
{
|
||
const SVzNLPointXYZRGBA& point = points[index];
|
||
if (!range.contains(point.x, point.y, point.z))
|
||
{
|
||
continue;
|
||
}
|
||
|
||
if (keptCount != index)
|
||
{
|
||
points[keptCount] = point;
|
||
if (points2D)
|
||
{
|
||
points2D[keptCount] = points2D[index];
|
||
}
|
||
}
|
||
if (sourcePointIndices)
|
||
{
|
||
sourcePointIndices->push_back(index);
|
||
}
|
||
++keptCount;
|
||
}
|
||
lineData.nPointCount = keptCount;
|
||
}
|
||
|
||
void FilterLaserLineInPlace(SVzLaserLineData& lineData, EVzResultDataType dataType, int sourcePointCount,
|
||
const PointCloudXYZRange& range, std::vector<int>* sourcePointIndices)
|
||
{
|
||
if (sourcePointIndices)
|
||
{
|
||
sourcePointIndices->clear();
|
||
}
|
||
if (dataType == keResultDataType_PointXYZRGBA)
|
||
{
|
||
FilterRgbaLineInPlace(lineData, sourcePointCount, range, sourcePointIndices);
|
||
}
|
||
else if (dataType == keResultDataType_Position || dataType == keResultDataType_PositionF)
|
||
{
|
||
FilterPositionLineInPlace(lineData, sourcePointCount, range, dataType, sourcePointIndices);
|
||
}
|
||
}
|
||
}
|
||
|
||
LaserDataLoader::LaserDataLoader()
|
||
{
|
||
m_lastError.clear();
|
||
}
|
||
|
||
LaserDataLoader::~LaserDataLoader()
|
||
{
|
||
}
|
||
|
||
int LaserDataLoader::LoadLaserScanData(const std::string& fileName,
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
||
int& lineNum,
|
||
float& scanSpeed,
|
||
int& maxTimeStamp,
|
||
int& clockPerSecond)
|
||
{
|
||
const PointCloudXYZRange range;
|
||
return LoadLaserScanData(fileName, laserLines, lineNum, scanSpeed,
|
||
maxTimeStamp, clockPerSecond, range);
|
||
}
|
||
|
||
int LaserDataLoader::LoadLaserScanData(const std::string& fileName,
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
||
int& lineNum,
|
||
float& scanSpeed,
|
||
int& maxTimeStamp,
|
||
int& clockPerSecond,
|
||
const PointCloudXYZRange& range)
|
||
{
|
||
LOG_INFO("Loading from file: %s\n", fileName.c_str());
|
||
|
||
// 输出容器拥有上次加载的缓冲区；即使本次范围参数无效，也先释放旧数据。
|
||
FreeLaserScanData(laserLines);
|
||
|
||
if (!range.isValid()) {
|
||
m_lastError = "Invalid XYZ point-cloud range";
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
lineNum = 0;
|
||
scanSpeed = 0.0f;
|
||
maxTimeStamp = 0;
|
||
clockPerSecond = 0;
|
||
return ERR_CODE(FILE_ERR_FORMAT);
|
||
}
|
||
|
||
// 清空输出参数
|
||
laserLines.clear();
|
||
lineNum = 0;
|
||
scanSpeed = 0.0f;
|
||
maxTimeStamp = 0;
|
||
clockPerSecond = 0;
|
||
|
||
if (HasFileExtension(fileName, ".dat") && !LooksLikeTextLaserFile(fileName)) {
|
||
return _LoadLaserScanDataFromDatFile(fileName, laserLines, lineNum, scanSpeed,
|
||
maxTimeStamp, clockPerSecond, range);
|
||
}
|
||
|
||
// 判断文件类型
|
||
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;
|
||
int result = SUCCESS;
|
||
|
||
// 包含DataType字段，检查数据类型
|
||
EVzResultDataType eDataType = keResultDataType_Invalid;
|
||
result = _GetLaserType(fileName, eDataType);
|
||
ERR_CODE_RETURN(result);
|
||
|
||
LOG_INFO("Laser data type: %d \n", eDataType);
|
||
|
||
SVzLaserLineData sLaserData;
|
||
memset(&sLaserData, 0, sizeof(SVzLaserLineData));
|
||
|
||
bool bFindLineNum = true;
|
||
int nLaserPointIdx = 0;
|
||
|
||
auto failTextLoad = [&](int code, const std::string& message) -> int {
|
||
m_lastError = message;
|
||
LOG_ERROR("%s\n", message.c_str());
|
||
if (!bFindLineNum) {
|
||
laserLines.push_back(std::make_pair(eDataType, sLaserData));
|
||
bFindLineNum = true;
|
||
}
|
||
FreeLaserScanData(laserLines);
|
||
lineNum = 0;
|
||
return ERR_CODE(code);
|
||
};
|
||
|
||
auto commitCurrentLine = [&]() -> int {
|
||
if (bFindLineNum) {
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 先校验原始记录数，再压缩范围外的点，避免把损坏文件当成成功。
|
||
if (nLaserPointIdx != sLaserData.nPointCount) {
|
||
std::ostringstream message;
|
||
message << "Incomplete laser line: expected " << sLaserData.nPointCount
|
||
<< " points, got " << nLaserPointIdx;
|
||
return failTextLoad(FILE_ERR_FORMAT, message.str());
|
||
}
|
||
FilterLaserLineInPlace(sLaserData, eDataType, nLaserPointIdx, range, nullptr);
|
||
laserLines.push_back(std::make_pair(eDataType, sLaserData));
|
||
bFindLineNum = true;
|
||
return SUCCESS;
|
||
};
|
||
|
||
while (std::getline(inputFile, line)) {
|
||
// 去除行末的 \r 字符（处理 Windows 格式的 CR LF 换行符）
|
||
TrimCarriageReturn(line);
|
||
|
||
if (line.find("LineNum:") == 0) {
|
||
if (1 != sscanf(line.c_str(), "LineNum:%d", &lineNum) || lineNum <= 0) {
|
||
return failTextLoad(FILE_ERR_FORMAT, "Invalid LineNum header: " + line);
|
||
}
|
||
} else if (line.find("DataType:") == 0) {
|
||
|
||
} else if (line.find("Line_") == 0) {
|
||
|
||
if(false == bFindLineNum) {
|
||
result = commitCurrentLine();
|
||
if (SUCCESS != result) return result;
|
||
}
|
||
|
||
int lineIndex = 0;
|
||
unsigned int timeStamp = 0;
|
||
int ptNum = 0;
|
||
if (3 != sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum) ||
|
||
ptNum < 0 || ptNum > VZ_LASER_LINE_PT_MAX_NUM) {
|
||
return failTextLoad(FILE_ERR_FORMAT, "Invalid laser line header: " + line);
|
||
}
|
||
|
||
sLaserData.llFrameIdx = lineIndex;
|
||
sLaserData.llTimeStamp = timeStamp;
|
||
sLaserData.nPointCount = ptNum;
|
||
if (eDataType == keResultDataType_PointXYZRGBA) {
|
||
sLaserData.p3DPoint = new SVzNLPointXYZRGBA[ptNum];
|
||
sLaserData.p2DPoint = new SVzNL2DLRPoint[ptNum];
|
||
memset(sLaserData.p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * ptNum);
|
||
memset(sLaserData.p2DPoint, 0, sizeof(SVzNL2DLRPoint) * ptNum);
|
||
} else if(eDataType == keResultDataType_Position) {
|
||
sLaserData.p3DPoint = new SVzNL3DPosition[ptNum];
|
||
sLaserData.p2DPoint = new SVzNL2DPosition[ptNum];
|
||
memset(sLaserData.p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum);
|
||
memset(sLaserData.p2DPoint, 0, sizeof(SVzNL2DPosition) * ptNum);
|
||
}
|
||
nLaserPointIdx = 0;
|
||
bFindLineNum = false;
|
||
|
||
} else if (IsShapeBlockHeader(line)) {
|
||
// 跳过图形块头及后续点坐标数据。
|
||
const int shapePointCount = ShapeBlockPointCount(line);
|
||
for (int skip = 0; skip < shapePointCount; ) {
|
||
if (!std::getline(inputFile, line)) {
|
||
return failTextLoad(inputFile.bad() ? FILE_ERR_READ : FILE_ERR_FORMAT,
|
||
"Incomplete shape block in laser file: " + fileName);
|
||
}
|
||
TrimCarriageReturn(line);
|
||
if (!line.empty()) {
|
||
++skip;
|
||
}
|
||
}
|
||
|
||
} else if (line.find("{") == 0) {
|
||
// 使用正则表达式判断是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 failTextLoad(DATA_ERR_INVALID,
|
||
"Point data does not match laser line header: " + line);
|
||
}
|
||
|
||
// 防止数组越界：跳过超出声明点数的数据行
|
||
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);
|
||
result = _ParseLaserScanPoint(line, pRGBAPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]);
|
||
if (SUCCESS != result) {
|
||
return failTextLoad(FILE_ERR_FORMAT, "Invalid RGBD point: " + line);
|
||
}
|
||
nLaserPointIdx++;
|
||
} else {
|
||
SVzNL3DPosition* p3DPoints = static_cast<SVzNL3DPosition*>(sLaserData.p3DPoint);
|
||
SVzNL2DPosition* p2DPoints = static_cast<SVzNL2DPosition*>(sLaserData.p2DPoint);
|
||
result = _ParseLaserScanPoint(line, p3DPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]);
|
||
if (SUCCESS != result) {
|
||
return failTextLoad(FILE_ERR_FORMAT, "Invalid XYZ point: " + line);
|
||
}
|
||
p3DPoints[nLaserPointIdx].nPointIdx = nLaserPointIdx;
|
||
nLaserPointIdx++;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 添加最后一条扫描线数据
|
||
if (!bFindLineNum) {
|
||
result = commitCurrentLine();
|
||
if (result != SUCCESS) {
|
||
return result;
|
||
}
|
||
}
|
||
|
||
inputFile.close();
|
||
LOG_INFO("Successfully loaded %d laser scan lines from file: %s\n", lineNum, fileName.c_str());
|
||
return SUCCESS;
|
||
}
|
||
|
||
int LaserDataLoader::LoadLaserScanDataByCallback(
|
||
const std::string& fileName,
|
||
const LaserLineLoadCallback& lineCallback,
|
||
const LaserLoadFinishedCallback& finishedCallback)
|
||
{
|
||
const PointCloudXYZRange range;
|
||
return LoadLaserScanDataByCallback(fileName, lineCallback, finishedCallback, range);
|
||
}
|
||
|
||
int LaserDataLoader::LoadLaserScanDataByCallback(
|
||
const std::string& fileName,
|
||
const LaserLineLoadCallback& lineCallback,
|
||
const LaserLoadFinishedCallback& finishedCallback,
|
||
const PointCloudXYZRange& range)
|
||
{
|
||
const LaserLineLoadIndexedCallback indexedCallback =
|
||
[lineCallback](EVzResultDataType dataType, SVzLaserLineData& lineData,
|
||
const LaserLineLoadInfo&)
|
||
{
|
||
if (lineCallback)
|
||
{
|
||
lineCallback(dataType, lineData);
|
||
}
|
||
};
|
||
return LoadLaserScanDataByIndexedCallback(fileName, indexedCallback,
|
||
finishedCallback, range);
|
||
}
|
||
|
||
int LaserDataLoader::LoadLaserScanDataByIndexedCallback(
|
||
const std::string& fileName,
|
||
const LaserLineLoadIndexedCallback& lineCallback,
|
||
const LaserLoadFinishedCallback& finishedCallback,
|
||
const PointCloudXYZRange& range)
|
||
{
|
||
LOG_INFO("Loading from file by callback: %s\n", fileName.c_str());
|
||
m_lastError.clear();
|
||
|
||
if (!range.isValid()) {
|
||
m_lastError = "Invalid XYZ point-cloud range";
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
if (finishedCallback) {
|
||
finishedCallback(ERR_CODE(FILE_ERR_FORMAT), 0, 0.0f, 0, 0);
|
||
}
|
||
return ERR_CODE(FILE_ERR_FORMAT);
|
||
}
|
||
|
||
int lineNum = 0;
|
||
float scanSpeed = 0.0f;
|
||
int maxTimeStamp = 0;
|
||
int clockPerSecond = 0;
|
||
|
||
auto finish = [&](int result) -> int {
|
||
if (finishedCallback) {
|
||
finishedCallback(result, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
|
||
}
|
||
return result;
|
||
};
|
||
|
||
if (HasFileExtension(fileName, ".dat") && !LooksLikeTextLaserFile(fileName)) {
|
||
return _LoadLaserScanDataFromDatFileByIndexedCallback(
|
||
fileName, lineCallback, finishedCallback, range);
|
||
}
|
||
|
||
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 finish(ERR_CODE(FILE_ERR_NOEXIST));
|
||
}
|
||
|
||
EVzResultDataType dataType = keResultDataType_Invalid;
|
||
int result = _GetLaserType(fileName, dataType);
|
||
if (result != SUCCESS) {
|
||
m_lastError = "Cannot determine laser data type: " + fileName;
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
return finish(result);
|
||
}
|
||
|
||
SVzLaserLineData lineData;
|
||
std::memset(&lineData, 0, sizeof(lineData));
|
||
std::unique_ptr<SVzNL3DPosition[]> positionPoints;
|
||
std::unique_ptr<SVzNL2DPosition[]> position2DPoints;
|
||
std::unique_ptr<SVzNLPointXYZRGBA[]> rgbaPoints;
|
||
std::unique_ptr<SVzNL2DLRPoint[]> rgba2DPoints;
|
||
bool hasLine = false;
|
||
int pointIndex = 0;
|
||
|
||
auto emitLine = [&]() -> int {
|
||
if (!hasLine) {
|
||
return SUCCESS;
|
||
}
|
||
if (pointIndex != lineData.nPointCount) {
|
||
std::ostringstream message;
|
||
message << "Laser line " << lineData.llFrameIdx << " declares "
|
||
<< lineData.nPointCount << " points but contains " << pointIndex;
|
||
m_lastError = message.str();
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
return ERR_CODE(FILE_ERR_FORMAT);
|
||
}
|
||
LaserLineLoadInfo loadInfo;
|
||
loadInfo.sourcePointCount = lineData.nPointCount;
|
||
FilterLaserLineInPlace(lineData, dataType, loadInfo.sourcePointCount, range,
|
||
&loadInfo.sourcePointIndices);
|
||
if (lineCallback) {
|
||
lineCallback(dataType, lineData, loadInfo);
|
||
}
|
||
positionPoints.reset();
|
||
position2DPoints.reset();
|
||
rgbaPoints.reset();
|
||
rgba2DPoints.reset();
|
||
std::memset(&lineData, 0, sizeof(lineData));
|
||
hasLine = false;
|
||
pointIndex = 0;
|
||
return SUCCESS;
|
||
};
|
||
|
||
std::string line;
|
||
while (std::getline(inputFile, line)) {
|
||
TrimCarriageReturn(line);
|
||
|
||
if (line.find("LineNum:") == 0) {
|
||
sscanf(line.c_str(), "LineNum:%d", &lineNum);
|
||
} else if (line.find("ScanSpeed:") == 0) {
|
||
sscanf(line.c_str(), "ScanSpeed:%f", &scanSpeed);
|
||
} else if (line.find("MaxTimeStamp:") == 0) {
|
||
sscanf(line.c_str(), "MaxTimeStamp:%d_%d", &maxTimeStamp, &clockPerSecond);
|
||
} else if (line.find("Line_") == 0) {
|
||
result = emitLine();
|
||
if (result != SUCCESS) {
|
||
return finish(result);
|
||
}
|
||
|
||
int lineIndex = 0;
|
||
unsigned int timeStamp = 0;
|
||
int pointCount = 0;
|
||
if (sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &pointCount) != 3 ||
|
||
pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) {
|
||
m_lastError = "Invalid laser line header: " + line;
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
return finish(ERR_CODE(FILE_ERR_FORMAT));
|
||
}
|
||
|
||
lineData.llFrameIdx = lineIndex;
|
||
lineData.llTimeStamp = timeStamp;
|
||
lineData.nPointCount = pointCount;
|
||
|
||
if (pointCount > 0 && dataType == keResultDataType_PointXYZRGBA) {
|
||
rgbaPoints.reset(new (std::nothrow) SVzNLPointXYZRGBA[pointCount]);
|
||
rgba2DPoints.reset(new (std::nothrow) SVzNL2DLRPoint[pointCount]);
|
||
if (!rgbaPoints || !rgba2DPoints) {
|
||
m_lastError = "Memory allocation failed while reading: " + fileName;
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
return finish(ERR_CODE(DATA_ERR_MEM));
|
||
}
|
||
std::memset(rgbaPoints.get(), 0,
|
||
sizeof(SVzNLPointXYZRGBA) * static_cast<size_t>(pointCount));
|
||
std::memset(rgba2DPoints.get(), 0,
|
||
sizeof(SVzNL2DLRPoint) * static_cast<size_t>(pointCount));
|
||
lineData.p3DPoint = rgbaPoints.get();
|
||
lineData.p2DPoint = rgba2DPoints.get();
|
||
} else if (pointCount > 0 && dataType == keResultDataType_Position) {
|
||
positionPoints.reset(new (std::nothrow) SVzNL3DPosition[pointCount]);
|
||
position2DPoints.reset(new (std::nothrow) SVzNL2DPosition[pointCount]);
|
||
if (!positionPoints || !position2DPoints) {
|
||
m_lastError = "Memory allocation failed while reading: " + fileName;
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
return finish(ERR_CODE(DATA_ERR_MEM));
|
||
}
|
||
std::memset(positionPoints.get(), 0,
|
||
sizeof(SVzNL3DPosition) * static_cast<size_t>(pointCount));
|
||
std::memset(position2DPoints.get(), 0,
|
||
sizeof(SVzNL2DPosition) * static_cast<size_t>(pointCount));
|
||
lineData.p3DPoint = positionPoints.get();
|
||
lineData.p2DPoint = position2DPoints.get();
|
||
}
|
||
hasLine = true;
|
||
if (pointCount == 0) {
|
||
result = emitLine();
|
||
if (result != SUCCESS) {
|
||
return finish(result);
|
||
}
|
||
}
|
||
} else if (IsShapeBlockHeader(line)) {
|
||
const int shapePointCount = ShapeBlockPointCount(line);
|
||
for (int skipped = 0; skipped < shapePointCount;) {
|
||
if (!std::getline(inputFile, line)) {
|
||
break;
|
||
}
|
||
TrimCarriageReturn(line);
|
||
if (!line.empty()) {
|
||
++skipped;
|
||
}
|
||
}
|
||
} else if (line.find("{") == 0) {
|
||
if (!hasLine || pointIndex >= lineData.nPointCount ||
|
||
!lineData.p3DPoint || !lineData.p2DPoint) {
|
||
m_lastError = "Point data does not match laser line header: " + line;
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
return finish(ERR_CODE(FILE_ERR_FORMAT));
|
||
}
|
||
|
||
if (dataType == keResultDataType_PointXYZRGBA) {
|
||
result = _ParseLaserScanPoint(line, rgbaPoints[pointIndex], rgba2DPoints[pointIndex]);
|
||
} else {
|
||
result = _ParseLaserScanPoint(line, positionPoints[pointIndex], position2DPoints[pointIndex]);
|
||
positionPoints[pointIndex].nPointIdx = pointIndex;
|
||
}
|
||
if (result != SUCCESS) {
|
||
m_lastError = "Invalid point line: " + line;
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
return finish(result);
|
||
}
|
||
++pointIndex;
|
||
if (pointIndex == lineData.nPointCount) {
|
||
result = emitLine();
|
||
if (result != SUCCESS) {
|
||
return finish(result);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
result = emitLine();
|
||
if (result != SUCCESS) {
|
||
return finish(result);
|
||
}
|
||
LOG_INFO("Successfully loaded %d laser scan lines by callback from file: %s\n",
|
||
lineNum, fileName.c_str());
|
||
return finish(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,
|
||
const PointCloudXYZRange& range)
|
||
{
|
||
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();
|
||
}
|
||
|
||
FilterLaserLineInPlace(lineData, keResultDataType_Position, pointCount, range, nullptr);
|
||
|
||
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,
|
||
int lineNum,
|
||
float scanSpeed,
|
||
int maxTimeStamp,
|
||
int clockPerSecond)
|
||
{
|
||
LOG_INFO("Saving to file: %s\n", fileName.c_str());
|
||
|
||
if (laserLines.empty() || lineNum <= 0) {
|
||
m_lastError = "Invalid input parameters for saving unified data";
|
||
LOG_ERROR("Invalid parameters for saving unified laser data\n");
|
||
return ERR_CODE(DEV_ARG_INVAILD);
|
||
}
|
||
|
||
try {
|
||
std::ofstream sw(fileName);
|
||
if (!sw.is_open()) {
|
||
m_lastError = "Cannot open file for writing: " + fileName;
|
||
LOG_ERROR("Cannot open file for writing: %s\n", fileName.c_str());
|
||
return ERR_CODE(FILE_ERR_WRITE);
|
||
}
|
||
|
||
// 写入文件头
|
||
sw << "LineNum:" << lineNum << '\n';
|
||
sw << "DataType:" << 0 << '\n';
|
||
sw << "ScanSpeed:" << scanSpeed << '\n';
|
||
sw << "PointAdjust: 1" << '\n';
|
||
sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << '\n';
|
||
|
||
// 写入每条扫描线数据
|
||
for (const auto& linePair : laserLines) {
|
||
|
||
EVzResultDataType dataType = linePair.first;
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
|
||
sw << "Line_" << lineData.llFrameIdx << "_" << lineData.llTimeStamp << "_" << lineData.nPointCount << '\n';
|
||
|
||
// 根据数据类型写入点云数据
|
||
if (dataType == keResultDataType_Position && lineData.p3DPoint) {
|
||
// 写入XYZ格式数据
|
||
const SVzNL3DPosition* points = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
|
||
const SVzNL2DPosition* points2D = static_cast<const SVzNL2DPosition*>(lineData.p2DPoint);
|
||
constexpr int BATCH = 256;
|
||
constexpr int PER_POINT = 128;
|
||
for (int base = 0; base < lineData.nPointCount; base += BATCH) {
|
||
int end = (base + BATCH < lineData.nPointCount) ? (base + BATCH) : lineData.nPointCount;
|
||
std::string buffer;
|
||
buffer.reserve(BATCH * PER_POINT);
|
||
for (int i = base; i < end; ++i) {
|
||
float x = static_cast<float>(points[i].pt3D.x);
|
||
float y = static_cast<float>(points[i].pt3D.y);
|
||
float z = static_cast<float>(points[i].pt3D.z);
|
||
AppendFormattedLine(buffer, "{ %.6f,%.6f,%.6f }-{ %d,%d }-{ %d,%d }\n",
|
||
x, y, z,
|
||
points2D[i].ptLeft2D.x, points2D[i].ptLeft2D.y,
|
||
points2D[i].ptRight2D.x, points2D[i].ptRight2D.y);
|
||
}
|
||
sw.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
|
||
}
|
||
} else if (dataType == keResultDataType_PositionF && lineData.p3DPoint) {
|
||
const SVzNL3DPosition* points = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
|
||
const SVzNL2DPositionF* points2D = static_cast<const SVzNL2DPositionF*>(lineData.p2DPoint);
|
||
constexpr int BATCH = 256;
|
||
constexpr int PER_POINT = 160;
|
||
for (int base = 0; base < lineData.nPointCount; base += BATCH) {
|
||
int end = (base + BATCH < lineData.nPointCount) ? (base + BATCH) : lineData.nPointCount;
|
||
std::string buffer;
|
||
buffer.reserve(BATCH * PER_POINT);
|
||
for (int i = base; i < end; ++i) {
|
||
AppendFormattedLine(buffer,
|
||
"{ %.9f, %.9f, %.9f } - { %.6f, %.6f } - { %.6f, %.6f }\n",
|
||
points[i].pt3D.x, points[i].pt3D.y, points[i].pt3D.z,
|
||
points2D ? points2D[i].ptLeft2D.x : 0.0f,
|
||
points2D ? points2D[i].ptLeft2D.y : 0.0f,
|
||
points2D ? points2D[i].ptRight2D.x : 0.0f,
|
||
points2D ? points2D[i].ptRight2D.y : 0.0f);
|
||
}
|
||
sw.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
|
||
}
|
||
} else if (dataType == keResultDataType_PointXYZI && lineData.p3DPoint) {
|
||
// LiDAR 点云：snprintf 批量写入，每 256 点 flush
|
||
// 坐标 mm 范围可达 ±200000 → "%.6f" 最长 14 字符 → 整行 ≤73 字符
|
||
const SVzNLPointXYZI* points = static_cast<const SVzNLPointXYZI*>(lineData.p3DPoint);
|
||
const int n = lineData.nPointCount;
|
||
constexpr int BATCH = 256;
|
||
constexpr int PER_POINT = 80;
|
||
char buf[BATCH * PER_POINT];
|
||
for (int base = 0; base < n; base += BATCH)
|
||
{
|
||
int end = (base + BATCH < n) ? (base + BATCH) : n;
|
||
char* p = buf;
|
||
for (int i = base; i < end; ++i)
|
||
{
|
||
p += snprintf(p, PER_POINT, "{ %.6f, %.6f, %.6f } - { 0, 0 } - { 0, 0 }\n",
|
||
points[i].fData[0], points[i].fData[1], points[i].fData[2]);
|
||
}
|
||
sw.write(buf, p - buf);
|
||
}
|
||
} else if (dataType == keResultDataType_PointXYZRGBA && lineData.p3DPoint) {
|
||
// 写入RGBD格式数据
|
||
const SVzNLPointXYZRGBA* points = static_cast<const SVzNLPointXYZRGBA*>(lineData.p3DPoint);
|
||
const SVzNL2DLRPoint* points2D = static_cast<const SVzNL2DLRPoint*>(lineData.p2DPoint);
|
||
constexpr int BATCH = 256;
|
||
constexpr int PER_POINT = 160;
|
||
for (int base = 0; base < lineData.nPointCount; base += BATCH) {
|
||
int end = (base + BATCH < lineData.nPointCount) ? (base + BATCH) : lineData.nPointCount;
|
||
std::string buffer;
|
||
buffer.reserve(BATCH * PER_POINT);
|
||
for (int i = base; i < end; ++i) {
|
||
float x = static_cast<float>(points[i].x);
|
||
float y = static_cast<float>(points[i].y);
|
||
float z = static_cast<float>(points[i].z);
|
||
int r = (points[i].nRGB >> 16) & 0xFF;
|
||
int g = (points[i].nRGB >> 8) & 0xFF;
|
||
int b = points[i].nRGB & 0xFF;
|
||
|
||
AppendFormattedLine(buffer, "{%.6f,%.6f,%.6f,%.6f,%.6f,%.6f}-{ %d,%d }-{ %d,%d }\n",
|
||
x, y, z,
|
||
b * 1.0f / 255, g * 1.0f / 255, r * 1.0f / 255,
|
||
points2D[i].sLeft.x, points2D[i].sLeft.y,
|
||
points2D[i].sRight.x, points2D[i].sRight.y);
|
||
}
|
||
sw.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
|
||
}
|
||
}
|
||
}
|
||
|
||
sw.flush();
|
||
if (!sw.good()) {
|
||
m_lastError = "Failed while writing file: " + fileName;
|
||
LOG_ERROR("Failed while writing file: %s\n", fileName.c_str());
|
||
return ERR_CODE(FILE_ERR_WRITE);
|
||
}
|
||
sw.close();
|
||
return SUCCESS;
|
||
|
||
} catch (const std::exception& e) {
|
||
m_lastError = "Error saving unified file: " + std::string(e.what());
|
||
LOG_ERROR("Error saving unified laser data to file: %s\n", e.what());
|
||
return ERR_CODE(FILE_ERR_WRITE);
|
||
}
|
||
}
|
||
|
||
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());
|
||
|
||
if (xyzData.empty()) {
|
||
m_lastError = "Invalid input parameters for saving unified data";
|
||
LOG_ERROR("Invalid parameters for saving unified laser data\n");
|
||
return ERR_CODE(DEV_ARG_INVAILD);
|
||
}
|
||
|
||
try {
|
||
std::ofstream sw(fileName);
|
||
if (!sw.is_open()) {
|
||
m_lastError = "Cannot open file for writing: " + fileName;
|
||
LOG_ERROR("Cannot open file for writing: %s\n", fileName.c_str());
|
||
return ERR_CODE(FILE_ERR_WRITE);
|
||
}
|
||
|
||
// 写入文件头
|
||
sw << "LineNum:" << xyzData.size() << '\n';
|
||
sw << "DataType: 0" << '\n';
|
||
sw << "ScanSpeed:" << 0 << '\n';
|
||
sw << "PointAdjust: 1" << '\n';
|
||
sw << "MaxTimeStamp:" << 0 << "_" << 0 << '\n';
|
||
|
||
const size_t totalLines = xyzData.size();
|
||
int index = 0;
|
||
// 写入每条扫描线数据
|
||
for (const auto& linePair : xyzData) {
|
||
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() << '\n';
|
||
// 根据数据类型写入点云数据
|
||
constexpr int BATCH = 256;
|
||
constexpr int PER_POINT = 96;
|
||
for (size_t base = 0; base < linePair.size(); base += BATCH) {
|
||
size_t end = std::min(base + static_cast<size_t>(BATCH), linePair.size());
|
||
std::string buffer;
|
||
buffer.reserve(BATCH * PER_POINT);
|
||
for (size_t i = base; i < end; ++i) {
|
||
const auto& point = linePair[i];
|
||
|
||
// 写入XYZ格式数据
|
||
float x = static_cast<float>(point.pt3D.x);
|
||
float y = static_cast<float>(point.pt3D.y);
|
||
float z = static_cast<float>(point.pt3D.z);
|
||
AppendFormattedLine(buffer, "{ %.6f, %.6f, %.6f } - { 0, 0 } - { 0, 0 }\n",
|
||
x, y, z);
|
||
}
|
||
sw.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
|
||
}
|
||
|
||
++index;
|
||
if (callback) {
|
||
callback->onSaveProgress(static_cast<float>(index) / static_cast<float>(totalLines));
|
||
}
|
||
}
|
||
|
||
sw.flush();
|
||
if (!sw.good()) {
|
||
m_lastError = "Failed while writing file: " + fileName;
|
||
LOG_ERROR("Failed while writing file: %s\n", fileName.c_str());
|
||
return ERR_CODE(FILE_ERR_WRITE);
|
||
}
|
||
sw.close();
|
||
return SUCCESS;
|
||
|
||
} catch (const std::exception& e) {
|
||
m_lastError = "Error saving unified file: " + std::string(e.what());
|
||
LOG_ERROR("Error saving unified laser data to file: %s\n", e.what());
|
||
return ERR_CODE(FILE_ERR_WRITE);
|
||
}
|
||
}
|
||
|
||
int LaserDataLoader::LoadShapeBlocks(const std::string& fileName,
|
||
std::vector<std::vector<SVzNLPointXYZRGBA>>& shapeBlocks)
|
||
{
|
||
LOG_INFO("Loading shape blocks from file: %s\n", fileName.c_str());
|
||
|
||
shapeBlocks.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;
|
||
}
|
||
|
||
// 查找图形块头。
|
||
if (!IsShapeBlockHeader(line)) {
|
||
continue;
|
||
}
|
||
|
||
// 解析点数量（最后一个下划线后的数字）
|
||
const int num = ShapeBlockPointCount(line);
|
||
if (num < 2) {
|
||
LOG_WARN("[LaserDataLoader] Invalid shape header: %s\n", line.c_str());
|
||
continue;
|
||
}
|
||
|
||
// 读取 num 个点坐标
|
||
std::vector<SVzNLPointXYZRGBA> points;
|
||
bool incompleteBlock = false;
|
||
for (int i = 0; i < num; ) {
|
||
if (!std::getline(inputFile, line)) {
|
||
incompleteBlock = true;
|
||
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），Alpha 存储在高 8 位
|
||
unsigned int nRGB = static_cast<unsigned int>(A) << 24;
|
||
nRGB |= static_cast<unsigned int>(B) << 16;
|
||
nRGB |= static_cast<unsigned int>(G) << 8;
|
||
nRGB |= static_cast<unsigned int>(R);
|
||
pt.nRGB = nRGB;
|
||
}
|
||
parsed = true;
|
||
}
|
||
|
||
// Compact coloured shape format: {x,y,z} - {R,G,B,A}.
|
||
if (!parsed) {
|
||
int cr = 255, cg = 255, cb = 255, ca = 255;
|
||
if (sscanf(line.c_str(),
|
||
" { %f , %f , %f } - { %d , %d , %d , %d }",
|
||
&fx, &fy, &fz, &cr, &cg, &cb, &ca) == 7) {
|
||
pt.x = fx;
|
||
pt.y = fy;
|
||
pt.z = fz;
|
||
pt.nRGB = (static_cast<unsigned int>(ca) << 24) |
|
||
(static_cast<unsigned int>(cb) << 16) |
|
||
(static_cast<unsigned int>(cg) << 8) |
|
||
static_cast<unsigned int>(cr);
|
||
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 (incompleteBlock || points.size() != static_cast<size_t>(num)) {
|
||
m_lastError = "Incomplete or invalid shape block in file: " + fileName;
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
shapeBlocks.clear();
|
||
return ERR_CODE(inputFile.bad() ? FILE_ERR_READ : FILE_ERR_FORMAT);
|
||
}
|
||
shapeBlocks.push_back(std::move(points));
|
||
}
|
||
|
||
if (inputFile.bad()) {
|
||
m_lastError = "Failed while reading shape file: " + fileName;
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
shapeBlocks.clear();
|
||
return ERR_CODE(FILE_ERR_READ);
|
||
}
|
||
|
||
inputFile.close();
|
||
LOG_INFO("Loaded %zu shape blocks from file: %s\n", shapeBlocks.size(), fileName.c_str());
|
||
return SUCCESS;
|
||
}
|
||
|
||
int LaserDataLoader::LoadPolySegments(
|
||
const std::string& fileName,
|
||
std::vector<std::vector<SVzNLPointXYZRGBA>>& polyLines)
|
||
{
|
||
return LoadShapeBlocks(fileName, polyLines);
|
||
}
|
||
|
||
void LaserDataLoader::FreeLaserScanData(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines)
|
||
{
|
||
LOG_DEBUG("Freeing unified laser scan data, line count: %zu\n", laserLines.size());
|
||
|
||
if (!laserLines.empty()) {
|
||
for (auto& linePair : laserLines) {
|
||
EVzResultDataType dataType = linePair.first;
|
||
SVzLaserLineData& lineData = linePair.second;
|
||
|
||
if (lineData.p3DPoint) {
|
||
switch (dataType) {
|
||
case keResultDataType_Position:
|
||
delete[] static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
|
||
break;
|
||
case keResultDataType_PositionF:
|
||
delete[] static_cast<SVzNL3DPosition*>(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) {
|
||
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;
|
||
}
|
||
}
|
||
laserLines.clear();
|
||
}
|
||
|
||
LOG_DEBUG("Unified laser scan data freed successfully\n");
|
||
}
|
||
|
||
// 转换统一格式数据为std::vector<std::vector<SVzNL3DPosition>>格式
|
||
int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
|
||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||
size_t* validPointCount)
|
||
{
|
||
scanLines.clear();
|
||
|
||
// 初始化有效点计数
|
||
size_t validCount = 0;
|
||
|
||
// 浮点数零值判断阈值
|
||
const double EPSILON = 1e-6;
|
||
|
||
for (const auto& linePair : unifiedData) {
|
||
EVzResultDataType dataType = linePair.first;
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
|
||
if (lineData.nPointCount < 0) {
|
||
continue;
|
||
}
|
||
|
||
// 只处理Position类型的数据；过滤后为空的扫描线也要保留位置。
|
||
if ((dataType == keResultDataType_Position ||
|
||
dataType == keResultDataType_PositionF) &&
|
||
(lineData.p3DPoint || lineData.nPointCount == 0)) {
|
||
std::vector<SVzNL3DPosition> scanLine;
|
||
scanLine.reserve(lineData.nPointCount);
|
||
|
||
const SVzNL3DPosition* points = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
|
||
for (int i = 0; i < lineData.nPointCount; ++i) {
|
||
if (points == nullptr) {
|
||
break;
|
||
}
|
||
scanLine.push_back(points[i]);
|
||
|
||
// 统计有效点（xyz全非0，使用epsilon阈值判断）
|
||
if (std::fabs(points[i].pt3D.x) > EPSILON &&
|
||
std::fabs(points[i].pt3D.y) > EPSILON &&
|
||
std::fabs(points[i].pt3D.z) > EPSILON) {
|
||
validCount++;
|
||
}
|
||
}
|
||
|
||
scanLines.push_back(scanLine);
|
||
}
|
||
}
|
||
|
||
// 返回有效点计数（如果调用者需要）
|
||
if (validPointCount) {
|
||
*validPointCount = validCount;
|
||
}
|
||
return SUCCESS;
|
||
}
|
||
|
||
int LaserDataLoader::ConvertToSVzNLPositionD(
|
||
const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
|
||
std::vector<std::vector<SVzNLPositionD>>& scanLines)
|
||
{
|
||
LOG_DEBUG("Converting unified data to SVzNLPositionD scan lines format\n");
|
||
|
||
scanLines.clear();
|
||
|
||
for (const auto& linePair : unifiedData) {
|
||
if (linePair.first != keResultDataType_Position &&
|
||
linePair.first != keResultDataType_PositionF) {
|
||
continue;
|
||
}
|
||
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
if (lineData.nPointCount < 0) {
|
||
continue;
|
||
}
|
||
|
||
if (lineData.nPointCount > 0 && (!lineData.p3DPoint || !lineData.p2DPoint)) {
|
||
m_lastError = "Missing 3D or 2D point buffer in Position laser data";
|
||
LOG_ERROR("%s, pointCount=%d, p3D=%p, p2D=%p\n",
|
||
m_lastError.c_str(),
|
||
lineData.nPointCount,
|
||
lineData.p3DPoint,
|
||
lineData.p2DPoint);
|
||
continue;
|
||
}
|
||
|
||
std::vector<SVzNLPositionD> scanLine;
|
||
scanLine.reserve(static_cast<size_t>(lineData.nPointCount));
|
||
if (linePair.first == keResultDataType_Position) {
|
||
const SVzNL3DPosition* points3D = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
|
||
const SVzNL2DPosition* points2D = static_cast<const SVzNL2DPosition*>(lineData.p2DPoint);
|
||
for (int i = 0; i < lineData.nPointCount; ++i) {
|
||
SVzNLPositionD point;
|
||
std::memset(&point, 0, sizeof(point));
|
||
point.nPointIdx = points3D[i].nPointIdx;
|
||
point.pt3D = points3D[i].pt3D;
|
||
point.ptLeft2D.x = points2D[i].ptLeft2D.x;
|
||
point.ptLeft2D.y = points2D[i].ptLeft2D.y;
|
||
point.ptRight2D.x = points2D[i].ptRight2D.x;
|
||
point.ptRight2D.y = points2D[i].ptRight2D.y;
|
||
scanLine.push_back(point);
|
||
}
|
||
} else {
|
||
const SVzNL3DPosition* points3D = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
|
||
const SVzNL2DPositionF* points2D = static_cast<const SVzNL2DPositionF*>(lineData.p2DPoint);
|
||
for (int i = 0; i < lineData.nPointCount; ++i) {
|
||
SVzNLPositionD point;
|
||
std::memset(&point, 0, sizeof(point));
|
||
point.nPointIdx = points3D[i].nPointIdx;
|
||
point.pt3D = points3D[i].pt3D;
|
||
point.ptLeft2D.x = points2D[i].ptLeft2D.x;
|
||
point.ptLeft2D.y = points2D[i].ptLeft2D.y;
|
||
point.ptRight2D.x = points2D[i].ptRight2D.x;
|
||
point.ptRight2D.y = points2D[i].ptRight2D.y;
|
||
scanLine.push_back(point);
|
||
}
|
||
}
|
||
|
||
if (!scanLine.empty()) {
|
||
scanLines.push_back(scanLine);
|
||
}
|
||
}
|
||
|
||
if (scanLines.empty()) {
|
||
m_lastError = "No Position laser data to convert";
|
||
LOG_ERROR("%s\n", m_lastError.c_str());
|
||
return ERR_CODE(DATA_ERR_INVALID);
|
||
}
|
||
|
||
LOG_DEBUG("Converted %zu lines to SVzNLPositionD scan lines format\n", scanLines.size());
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 转换统一格式数据为SVzNL3DLaserLine格式
|
||
int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
|
||
std::vector<SVzNL3DLaserLine>& xyzData)
|
||
{
|
||
xyzData.clear();
|
||
|
||
for (const auto& linePair : unifiedData) {
|
||
EVzResultDataType dataType = linePair.first;
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
|
||
// PositionF 的 3D buffer 也是 SVzNL3DPosition；仅 2D UV 为浮点。
|
||
if ((dataType == keResultDataType_Position ||
|
||
dataType == keResultDataType_PositionF) &&
|
||
(lineData.p3DPoint || lineData.nPointCount == 0)) {
|
||
SVzNL3DLaserLine xyzLine;
|
||
xyzLine.nTimeStamp = lineData.llTimeStamp;
|
||
xyzLine.nPositionCnt = lineData.nPointCount;
|
||
|
||
if (lineData.nPointCount > 0) {
|
||
xyzLine.p3DPosition = new SVzNL3DPosition[lineData.nPointCount];
|
||
if (xyzLine.p3DPosition) {
|
||
memcpy(xyzLine.p3DPosition, lineData.p3DPoint, sizeof(SVzNL3DPosition) * lineData.nPointCount);
|
||
} else {
|
||
m_lastError = "Memory allocation failed for SVzNL3DPosition";
|
||
LOG_ERROR("Memory allocation failed for SVzNL3DPosition\n");
|
||
return ERR_CODE(DATA_ERR_INVALID);
|
||
}
|
||
} else {
|
||
xyzLine.p3DPosition = nullptr;
|
||
}
|
||
|
||
xyzData.push_back(xyzLine);
|
||
}
|
||
}
|
||
|
||
LOG_DEBUG("Converted %zu lines to SVzNL3DLaserLine format\n", xyzData.size());
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 转换统一格式数据为SVzNLXYZRGBDLaserLine格式
|
||
int LaserDataLoader::ConvertToSVzNLXYZRGBDLaserLine(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
|
||
std::vector<SVzNLXYZRGBDLaserLine>& rgbdData)
|
||
{
|
||
LOG_DEBUG("Converting unified data to SVzNLXYZRGBDLaserLine format\n");
|
||
|
||
rgbdData.clear();
|
||
|
||
for (const auto& linePair : unifiedData) {
|
||
EVzResultDataType dataType = linePair.first;
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
|
||
// 只处理PointXYZRGBA类型的数据
|
||
if (dataType == keResultDataType_PointXYZRGBA &&
|
||
(lineData.p3DPoint || lineData.nPointCount == 0)) {
|
||
SVzNLXYZRGBDLaserLine rgbdLine;
|
||
rgbdLine.nTimeStamp = lineData.llTimeStamp;
|
||
rgbdLine.nPointCnt = lineData.nPointCount;
|
||
|
||
if (lineData.nPointCount > 0) {
|
||
rgbdLine.p3DPoint = new SVzNLPointXYZRGBA[lineData.nPointCount];
|
||
if (rgbdLine.p3DPoint) {
|
||
memcpy(rgbdLine.p3DPoint, lineData.p3DPoint, sizeof(SVzNLPointXYZRGBA) * lineData.nPointCount);
|
||
} else {
|
||
m_lastError = "Memory allocation failed for SVzNLPointXYZRGBA";
|
||
LOG_ERROR("Memory allocation failed for SVzNLPointXYZRGBA\n");
|
||
return ERR_CODE(DATA_ERR_INVALID);
|
||
}
|
||
} else {
|
||
rgbdLine.p3DPoint = nullptr;
|
||
}
|
||
|
||
rgbdData.push_back(rgbdLine);
|
||
}
|
||
}
|
||
|
||
LOG_DEBUG("Converted %zu lines to SVzNLXYZRGBDLaserLine format\n", rgbdData.size());
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 释放转换后的SVzNL3DLaserLine数据内存
|
||
void LaserDataLoader::FreeConvertedData(std::vector<SVzNL3DLaserLine>& xyzData)
|
||
{
|
||
LOG_DEBUG("Freeing converted SVzNL3DLaserLine data, line count: %zu\n", xyzData.size());
|
||
|
||
if (!xyzData.empty()) {
|
||
for (auto& scanLine : xyzData) {
|
||
if (scanLine.p3DPosition) {
|
||
delete[] scanLine.p3DPosition;
|
||
scanLine.p3DPosition = nullptr;
|
||
}
|
||
}
|
||
xyzData.clear();
|
||
}
|
||
|
||
LOG_DEBUG("Converted SVzNL3DLaserLine data freed successfully\n");
|
||
}
|
||
|
||
// 释放转换后的SVzNLXYZRGBDLaserLine数据内存
|
||
void LaserDataLoader::FreeConvertedData(std::vector<SVzNLXYZRGBDLaserLine>& rgbdData)
|
||
{
|
||
LOG_DEBUG("Freeing converted SVzNLXYZRGBDLaserLine data, line count: %zu\n", rgbdData.size());
|
||
|
||
if (!rgbdData.empty()) {
|
||
for (auto& scanLine : rgbdData) {
|
||
if (scanLine.p3DPoint) {
|
||
delete[] scanLine.p3DPoint;
|
||
scanLine.p3DPoint = nullptr;
|
||
}
|
||
}
|
||
rgbdData.clear();
|
||
}
|
||
|
||
LOG_DEBUG("Converted SVzNLXYZRGBDLaserLine data freed successfully\n");
|
||
}
|
||
|
||
|
||
int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPosition& s2DData)
|
||
{
|
||
float X = 0.0f, Y = 0.0f, Z = 0.0f;
|
||
float leftX = 0.0f, leftY = 0.0f;
|
||
float rightX = 0.0f, rightY = 0.0f;
|
||
const int parsed = sscanf(data.c_str(),
|
||
" { %f , %f , %f } - { %f , %f } - { %f , %f }",
|
||
&X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
||
if (parsed != 7) {
|
||
return ERR_CODE(FILE_ERR_FORMAT);
|
||
}
|
||
sData.pt3D.x = X;
|
||
sData.pt3D.y = Y;
|
||
sData.pt3D.z = Z;
|
||
s2DData.ptLeft2D.x = leftX;
|
||
s2DData.ptLeft2D.y = leftY;
|
||
s2DData.ptRight2D.x = rightX;
|
||
s2DData.ptRight2D.y = rightY;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int LaserDataLoader::_LoadLaserScanDataFromDatFileByIndexedCallback(
|
||
const std::string& fileName,
|
||
const LaserLineLoadIndexedCallback& lineCallback,
|
||
const LaserLoadFinishedCallback& finishedCallback,
|
||
const PointCloudXYZRange& range)
|
||
{
|
||
int lineNum = 0;
|
||
float scanSpeed = 0.0f;
|
||
int maxTimeStamp = 0;
|
||
int clockPerSecond = 0;
|
||
|
||
auto finish = [&](int result) -> int {
|
||
if (finishedCallback) {
|
||
finishedCallback(result, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
|
||
}
|
||
return result;
|
||
};
|
||
|
||
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 finish(ERR_CODE(FILE_ERR_NOEXIST));
|
||
}
|
||
|
||
auto fail = [&](int code, const std::string& message) -> int {
|
||
m_lastError = message;
|
||
LOG_ERROR("%s\n", message.c_str());
|
||
return finish(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 featurePointSize = 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) ||
|
||
pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) {
|
||
return false;
|
||
}
|
||
pos += static_cast<std::streamoff>(sizeof(pointCount));
|
||
const std::streamoff pointBytes =
|
||
static_cast<std::streamoff>(pointCount) * featurePointSize;
|
||
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;
|
||
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);
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
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);
|
||
lineData.p3DPoint = points3D.get();
|
||
lineData.p2DPoint = points2D.get();
|
||
LaserLineLoadInfo loadInfo;
|
||
loadInfo.sourcePointCount = pointCount;
|
||
FilterLaserLineInPlace(lineData, keResultDataType_Position, pointCount, range,
|
||
&loadInfo.sourcePointIndices);
|
||
if (lineCallback) {
|
||
lineCallback(keResultDataType_Position, lineData, loadInfo);
|
||
}
|
||
}
|
||
|
||
LOG_INFO("Successfully loaded %d dat laser scan lines by callback from file: %s\n",
|
||
lineNum, fileName.c_str());
|
||
return finish(SUCCESS);
|
||
}
|
||
|
||
int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPositionF& s2DData)
|
||
{
|
||
double X = 0.0, Y = 0.0, Z = 0.0;
|
||
float leftX = 0.0f, leftY = 0.0f;
|
||
float rightX = 0.0f, rightY = 0.0f;
|
||
const int parsed = sscanf(data.c_str(),
|
||
" { %lf , %lf , %lf } - { %f , %f } - { %f , %f }",
|
||
&X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
||
if (parsed != 7) {
|
||
return ERR_CODE(FILE_ERR_FORMAT);
|
||
}
|
||
sData.pt3D.x = X;
|
||
sData.pt3D.y = Y;
|
||
sData.pt3D.z = Z;
|
||
s2DData.ptLeft2D.x = leftX;
|
||
s2DData.ptLeft2D.y = leftY;
|
||
s2DData.ptRight2D.x = rightX;
|
||
s2DData.ptRight2D.y = rightY;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNLPointXYZRGBA& sData, SVzNL2DLRPoint& s2DData)
|
||
{
|
||
float X = 0.0f, Y = 0.0f, Z = 0.0f;
|
||
float leftX = 0.0f, leftY = 0.0f;
|
||
float rightX = 0.0f, rightY = 0.0f;
|
||
|
||
// 尝试新格式: {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,A 是 0-255 整数值，Alpha 存储在高 8 位
|
||
unsigned int nRGB = static_cast<unsigned int>(A) << 24;
|
||
nRGB |= static_cast<unsigned int>(B) << 16;
|
||
nRGB |= static_cast<unsigned int>(G) << 8;
|
||
nRGB |= static_cast<unsigned int>(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 = 0.0f, g = 0.0f, b = 0.0f;
|
||
if (10 != sscanf(data.c_str(),
|
||
" { %f , %f , %f , %f , %f , %f } - { %f , %f } - { %f , %f }",
|
||
&X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY)) {
|
||
return ERR_CODE(FILE_ERR_FORMAT);
|
||
}
|
||
sData.x = X;
|
||
sData.y = Y;
|
||
sData.z = Z;
|
||
|
||
int nr = (int)(r * 255);
|
||
int ng = (int)(g * 255);
|
||
int nb = (int)(b * 255);
|
||
nb <<= 8;
|
||
nb += ng;
|
||
nb <<= 8;
|
||
nb += nr;
|
||
sData.nRGB = nb;
|
||
|
||
s2DData.sLeft.x = leftX;
|
||
s2DData.sLeft.y = leftY;
|
||
s2DData.sRight.x = rightX;
|
||
s2DData.sRight.y = rightY;
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 获取激光数据类型
|
||
int LaserDataLoader::_GetLaserType(const std::string& fileName, EVzResultDataType& eDataType)
|
||
{
|
||
std::ifstream inputFile(fileName);
|
||
std::string linedata;
|
||
|
||
if (!inputFile.is_open()) {
|
||
m_lastError = "Cannot open file: " + fileName;
|
||
return ERR_CODE(FILE_ERR_NOEXIST);
|
||
}
|
||
|
||
bool bFind = false;
|
||
while (std::getline(inputFile, linedata)) {
|
||
TrimCarriageReturn(linedata);
|
||
|
||
if (linedata.find("{") == 0) {
|
||
// 统计 { 的数量判断格式
|
||
int braceCount = 0;
|
||
for (char c : linedata) {
|
||
if (c == '{') braceCount++;
|
||
}
|
||
|
||
if (braceCount == 4) {
|
||
// 新RGBD格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A}
|
||
eDataType = keResultDataType_PointXYZRGBA;
|
||
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) {
|
||
// XYZUV文本格式: {x,y,z}-{lx,ly}-{rx,ry}
|
||
eDataType = keResultDataType_Position;
|
||
bFind = true;
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
inputFile.close();
|
||
return bFind ? SUCCESS : ERR_CODE(FILE_ERR_FORMAT);
|
||
}
|