521 lines
21 KiB
C++
521 lines
21 KiB
C++
#include "LaserDataLoader.h"
|
||
#include <fstream>
|
||
#include <sstream>
|
||
#include <cstring>
|
||
#include <cstdlib>
|
||
#include <stdexcept>
|
||
#include <regex>
|
||
#include "VrLog.h"
|
||
#include <iomanip>
|
||
|
||
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)
|
||
{
|
||
LOG_INFO("Loading laser scan data from file: %s\n", fileName.c_str());
|
||
|
||
// 清空输出参数
|
||
laserLines.clear();
|
||
lineNum = 0;
|
||
scanSpeed = 0.0f;
|
||
maxTimeStamp = 0;
|
||
clockPerSecond = 0;
|
||
|
||
// 判断文件类型
|
||
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;
|
||
|
||
while (std::getline(inputFile, line)) {
|
||
if (line.find("LineNum:") == 0) {
|
||
sscanf(line.c_str(), "LineNum:%d", &lineNum);
|
||
} else if (line.find("DataType:") == 0) {
|
||
|
||
} else if (line.find("Line_") == 0) {
|
||
|
||
if(false == bFindLineNum) {
|
||
laserLines.push_back(std::make_pair(eDataType, sLaserData));
|
||
}
|
||
|
||
int lineIndex;
|
||
unsigned int timeStamp;
|
||
int ptNum = 0;
|
||
sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum);
|
||
|
||
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 (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 ERR_CODE(DATA_ERR_INVALID);
|
||
}
|
||
|
||
if (eDataType == keResultDataType_PointXYZRGBA) {
|
||
SVzNLPointXYZRGBA* pRGBAPoints = static_cast<SVzNLPointXYZRGBA*>(sLaserData.p3DPoint);
|
||
SVzNL2DLRPoint* p2DPoints = static_cast<SVzNL2DLRPoint*>(sLaserData.p2DPoint);
|
||
_ParseLaserScanPoint(line, pRGBAPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]);
|
||
nLaserPointIdx++;
|
||
} else {
|
||
SVzNL3DPosition* p3DPoints = static_cast<SVzNL3DPosition*>(sLaserData.p3DPoint);
|
||
SVzNL2DPosition* p2DPoints = static_cast<SVzNL2DPosition*>(sLaserData.p2DPoint);
|
||
_ParseLaserScanPoint(line, p3DPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]);
|
||
p3DPoints[nLaserPointIdx].nPointIdx = nLaserPointIdx;
|
||
nLaserPointIdx++;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 添加最后一条扫描线数据
|
||
if (!bFindLineNum) {
|
||
laserLines.push_back(std::make_pair(eDataType, sLaserData));
|
||
}
|
||
|
||
inputFile.close();
|
||
LOG_INFO("Successfully loaded %d 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 unified laser scan data 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 << std::endl;
|
||
sw << "DataType: 0" << std::endl;
|
||
sw << "ScanSpeed:" << scanSpeed << std::endl;
|
||
sw << "PointAdjust: 1" << std::endl;
|
||
sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl;
|
||
|
||
// 写入每条扫描线数据
|
||
for (const auto& linePair : laserLines) {
|
||
|
||
EVzResultDataType dataType = linePair.first;
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
|
||
sw << "Line_" << lineData.llFrameIdx << "_" << lineData.llTimeStamp << "_" << lineData.nPointCount << std::endl;
|
||
|
||
// 根据数据类型写入点云数据
|
||
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);
|
||
for (int i = 0; i < lineData.nPointCount; 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);
|
||
sw << "{ " << std::fixed << std::setprecision(6) << x << "," << y << "," << z << " }-";
|
||
sw << "{ " << points2D[i].ptLeft2D.x << "," << points2D[i].ptLeft2D.y << " }-";
|
||
sw << "{ " << points2D[i].ptRight2D.x << "," << points2D[i].ptRight2D.y << " }" << std::endl;
|
||
}
|
||
} 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);
|
||
for (int i = 0; i < lineData.nPointCount; 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;
|
||
|
||
sw << "{" << std::fixed << std::setprecision(6) << x << ","
|
||
<< std::fixed << std::setprecision(6) << y << ","
|
||
<< std::fixed << std::setprecision(6) << z << ","
|
||
<< std::fixed << std::setprecision(6) << b * 1.0f / 255 << ","
|
||
<< std::fixed << std::setprecision(6) << g * 1.0f / 255 << ","
|
||
<< std::fixed << std::setprecision(6) << r * 1.0f / 255 << "}-";
|
||
sw << "{ " << points2D[i].sLeft.x << "," << points2D[i].sLeft.y << " }-";
|
||
sw << "{ " << points2D[i].sRight.x << "," << points2D[i].sRight.y << " }" << std::endl;
|
||
}
|
||
}
|
||
}
|
||
|
||
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)
|
||
{
|
||
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() << std::endl;
|
||
sw << "DataType: 0" << std::endl;
|
||
sw << "ScanSpeed:" << 0 << std::endl;
|
||
sw << "PointAdjust: 1" << std::endl;
|
||
sw << "MaxTimeStamp:" << 0 << "_" << 0 << std::endl;
|
||
|
||
int index = 0;
|
||
// 写入每条扫描线数据
|
||
for (const auto& linePair : xyzData) {
|
||
sw << "Line_" << index++ << "_" << 0 << "_" << linePair.size() << std::endl;
|
||
// 根据数据类型写入点云数据
|
||
for(const auto& point : linePair){
|
||
|
||
// 写入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);
|
||
sw << "{ "
|
||
<< std::fixed << std::setprecision(6) << x << ", "
|
||
<< std::fixed << std::setprecision(6) << y << ", "
|
||
<< std::fixed << std::setprecision(6) << z << " } - ";
|
||
sw << "{ 0, 0 } - { 0, 0 }" << std::endl;
|
||
}
|
||
}
|
||
|
||
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);
|
||
}
|
||
}
|
||
|
||
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) {
|
||
delete[] lineData.p3DPoint;
|
||
lineData.p3DPoint = nullptr;
|
||
}
|
||
|
||
if (lineData.p2DPoint) {
|
||
delete[] lineData.p2DPoint;
|
||
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)
|
||
{
|
||
LOG_DEBUG("Converting unified data to scan lines format\n");
|
||
|
||
scanLines.clear();
|
||
|
||
for (const auto& linePair : unifiedData) {
|
||
EVzResultDataType dataType = linePair.first;
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
|
||
// 只处理Position类型的数据
|
||
if (dataType == keResultDataType_Position && 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]);
|
||
}
|
||
|
||
scanLines.push_back(scanLine);
|
||
}
|
||
}
|
||
|
||
LOG_DEBUG("Converted %zu lines to 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;
|
||
|
||
// 只处理Position类型的数据
|
||
if (dataType == keResultDataType_Position && lineData.p3DPoint) {
|
||
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) {
|
||
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, Y, Z;
|
||
float leftX, leftY;
|
||
float rightX, rightY;
|
||
sscanf(data.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
||
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, Y, Z;
|
||
float r, g, b;
|
||
float leftX, leftY;
|
||
float rightX, rightY;
|
||
sscanf(data.c_str(), "{%f,%f,%f,%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY);
|
||
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)) {
|
||
if (linedata.find("{") == 0) {
|
||
// 修复正则表达式以匹配实际数据格式
|
||
// XYZ格式: {x,y,z}-{leftX,leftY}-{rightX,rightY}
|
||
// RGBD格式: {x,y,z,r,g,b}-{leftX,leftY}-{rightX,rightY}
|
||
// 更宽松的正则表达式，允许更多的空格变化
|
||
std::regex xyzPattern(R"(\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\})");
|
||
std::regex rgbdPattern(R"(\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\})");
|
||
|
||
// 先尝试匹配RGBD格式（6个数字）
|
||
if (std::regex_match(linedata, rgbdPattern)) {
|
||
eDataType = keResultDataType_PointXYZRGBA;
|
||
bFind = true;
|
||
}
|
||
// 再尝试匹配XYZ格式（3个数字）
|
||
else if (std::regex_match(linedata, xyzPattern)) {
|
||
eDataType = keResultDataType_Position;
|
||
bFind = true;
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
inputFile.close();
|
||
return bFind ? SUCCESS : ERR_CODE(FILE_ERR_FORMAT);
|
||
}
|