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

修正RGBD数据的加载及转换

Browse Source
This commit is contained in:
杰仔 2026-08-12 14:46:40 +08:00
commit 5d78e082df
4 changed files with 178 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
CloudUtils/Inc/LaserDataLoader.h
View File

@ -67,6 +67,10 @@ public:
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
size_t* validPointCount = nullptr);
// 转换统一格式数据为std::vector<std::vector<SVzNLPositionD>>格式
int ConvertToSVzNLPositionD(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
std::vector<std::vector<SVzNLPositionD>>& scanLines);
// 从文件加载Poly折线数据（带颜色）
// polyLines: 每条折线的点坐标列表，polyLines[i] 是第i条折线的所有点（含RGB颜色）
int LoadPolySegments(const std::string& fileName,
@ -78,6 +82,7 @@ public:
private:
// 读取XYZ格式的激光数据
int _ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPosition& s2DData);
int _ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPositionF& s2DData);
// 读取RGBD格式的激光数据 - 新增RGBD支持
int _ParseLaserScanPoint(const std::string& data, SVzNLPointXYZRGBA& sData, SVzNL2DLRPoint& s2DData);

175
CloudUtils/Src/LaserDataLoader.cpp
View File

@ -535,19 +535,6 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName,
LOG_ERROR("Invalid parameters for saving unified laser data\n");
return ERR_CODE(DEV_ARG_INVAILD);
}
const EVzResultDataType fileDataType = laserLines.front().first;
if (fileDataType != keResultDataType_Position &&
fileDataType != keResultDataType_PointXYZI &&
fileDataType != keResultDataType_PointXYZRGBA) {
m_lastError = "Unsupported laser data type for saving";
return ERR_CODE(DEV_ARG_INVAILD);
}
if (lineNum != static_cast<int>(laserLines.size()) ||
std::any_of(laserLines.begin(), laserLines.end(),
[fileDataType](const auto& line) { return line.first != fileDataType; })) {
m_lastError = "Line count mismatch or mixed laser data types";
return ERR_CODE(DEV_ARG_INVAILD);
}
try {
std::ofstream sw(fileName);
@ -587,13 +574,30 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName,
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);
const int leftX = points2D ? points2D[i].ptLeft2D.x : 0;
const int leftY = points2D ? points2D[i].ptLeft2D.y : 0;
const int rightX = points2D ? points2D[i].ptRight2D.x : 0;
const int rightY = points2D ? points2D[i].ptRight2D.y : 0;
AppendFormattedLine(buffer, "{ %.6f,%.6f,%.6f }-{ %d,%d }-{ %d,%d }\n",
x, y, z,
leftX, leftY, rightX, rightY);
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()));
}
@ -633,15 +637,12 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName,
int r = (points[i].nRGB >> 16) & 0xFF;
int g = (points[i].nRGB >> 8) & 0xFF;
int b = points[i].nRGB & 0xFF;
const int leftX = points2D ? points2D[i].sLeft.x : 0;
const int leftY = points2D ? points2D[i].sLeft.y : 0;
const int rightX = points2D ? points2D[i].sRight.x : 0;
const int rightY = points2D ? points2D[i].sRight.y : 0;
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,
leftX, leftY, rightX, rightY);
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()));
}
@ -917,7 +918,7 @@ void LaserDataLoader::FreeLaserScanData(std::vector<std::pair<EVzResultDataType,
delete[] static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
break;
case keResultDataType_PositionF:
delete[] static_cast<SVzNL3DPositionF*>(lineData.p3DPoint);
delete[] static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
break;
case keResultDataType_PointF:
delete[] static_cast<SVzNL3DPointF*>(lineData.p3DPoint);
@ -1012,6 +1013,22 @@ int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector<std::pair<EVzRes
}
}
scanLines.push_back(scanLine);
} else if (dataType == keResultDataType_PositionF && lineData.p3DPoint) {
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++) {
scanLine.push_back(points[i]);
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);
}
}
@ -1025,6 +1042,82 @@ int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector<std::pair<EVzRes
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)
@ -1035,8 +1128,9 @@ int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector<std::pair<EVzRe
EVzResultDataType dataType = linePair.first;
const SVzLaserLineData& lineData = linePair.second;
// 只处理Position类型的数据
if (dataType == keResultDataType_Position && lineData.p3DPoint) {
// PositionF 的 3D buffer 也是 SVzNL3DPosition；仅 2D UV 为浮点。
if ((dataType == keResultDataType_Position ||
dataType == keResultDataType_PositionF) && lineData.p3DPoint) {
SVzNL3DLaserLine xyzLine;
xyzLine.nTimeStamp = lineData.llTimeStamp;
xyzLine.nPositionCnt = lineData.nPointCount;
@ -1143,8 +1237,31 @@ int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPositi
float X = 0.0f, Y = 0.0f, Z = 0.0f;
float leftX = 0.0f, leftY = 0.0f;
float rightX = 0.0f, rightY = 0.0f;
if (7 != sscanf(data.c_str(), " { %f , %f , %f } - { %f , %f } - { %f , %f }",
&X, &Y, &Z, &leftX, &leftY, &rightX, &rightY)) {
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::_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;
@ -1253,7 +1370,7 @@ int LaserDataLoader::_GetLaserType(const std::string& fileName, EVzResultDataTyp
eDataType = keResultDataType_PointXYZRGBA;
bFind = true;
} else if (commaCount >= 2) {
// XYZ格式: {x,y,z}-{lx,ly}-{rx,ry}
// XYZUV文本格式: {x,y,z}-{lx,ly}-{rx,ry}
eDataType = keResultDataType_Position;
bFind = true;
}

6
CloudUtils/Src/PointCloudImageUtils.cpp
View File

@ -1188,7 +1188,8 @@ int PointCloudImageUtils::GenerateDepthImage(
if (!lineData.p3DPoint || lineData.nPointCount <= 0) continue;
// 根据数据类型处理点云数据
if (dataType == keResultDataType_Position) {
if (dataType == keResultDataType_Position ||
dataType == keResultDataType_PositionF) {
SVzNL3DPosition* positions = static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
for (int i = 0; i < lineData.nPointCount; i++) {
const SVzNL3DPosition& point = positions[i];
@ -1250,7 +1251,8 @@ int PointCloudImageUtils::GenerateDepthImage(
if (!lineData.p3DPoint || lineData.nPointCount <= 0) continue;
if (dataType == keResultDataType_Position) {
if (dataType == keResultDataType_Position ||
dataType == keResultDataType_PositionF) {
SVzNL3DPosition* positions = static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
for (int i = 0; i < lineData.nPointCount; i++) {
const SVzNL3DPosition& point = positions[i];

5
Utils.pro
View File

@ -12,11 +12,6 @@ CloudView.file = CloudView/CloudView.pro
SUBDIRS += \
VrCommon \
VrUtils \
CloudUtils \
DataUtils
win32-msvc {
SUBDIRS += \
CloudView3D \
CloudView
}