Utils/CloudUtils/Src/LaserDataLoader.cpp

#include "LaserDataLoader.h"
#include <fstream>
#include <sstream>
#include <cstring>
#include <cstdlib>
#include <stdexcept>
#include <regex>
#include <cmath>
#include "VrLog.h"
#include <iomanip>

// 辅助函数：去除字符串末尾的 \r 字符（处理 Windows 格式的 CR LF 换行符）
static inline void TrimCarriageReturn(std::string& str)
{
    if (!str.empty() && str.back() == '\r') {
        str.pop_back();
    }
}

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)) {
        // 去除行末的 \r 字符（处理 Windows 格式的 CR LF 换行符）
        TrimCarriageReturn(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("Poly_") == 0) {
            // 跳过 Poly 折线头及后续点坐标数据
            size_t lastUnderscore = line.rfind('_');
            int polyNum = 0;
            if (lastUnderscore != std::string::npos && lastUnderscore + 1 < line.size()) {
                polyNum = std::atoi(line.c_str() + lastUnderscore + 1);
            }
            for (int skip = 0; skip < polyNum; ) {
                if (!std::getline(inputFile, line)) break;
                TrimCarriageReturn(line);
                if (!line.empty()) {
                    ++skip;
                }
            }

        } else if (line.find("{") == 0) {
            // 使用正则表达式判断是XYZ还是RGBD格式
            // XYZ格式: {x,y,z}-{leftX,leftY}-{rightX,rightY}
            // 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);
    }
}

int LaserDataLoader::LoadPolySegments(const std::string& fileName,
                                      std::vector<std::vector<SVzNLPointXYZRGBA>>& polyLines)
{
    LOG_INFO("Loading poly segments from file: %s\n", fileName.c_str());

    polyLines.clear();

    std::ifstream inputFile(fileName);
    if (!inputFile.is_open()) {
        m_lastError = "Cannot open file: " + fileName;
        LOG_ERROR("Cannot open file: %s\n", fileName.c_str());
        return ERR_CODE(FILE_ERR_NOEXIST);
    }

    std::string line;

    while (std::getline(inputFile, line)) {
        TrimCarriageReturn(line);

        // 跳过空行和注释
        if (line.empty() || line[0] == '#') {
            continue;
        }

        // 查找 Poly_index_num 头
        if (line.find("Poly_") != 0) {
            continue;
        }

        // 解析点数量（最后一个下划线后的数字）
        size_t lastUnderscore = line.rfind('_');
        if (lastUnderscore == std::string::npos || lastUnderscore + 1 >= line.size()) {
            LOG_WARN("[LaserDataLoader] Invalid Poly header: %s\n", line.c_str());
            continue;
        }

        int num = std::atoi(line.c_str() + lastUnderscore + 1);
        if (num < 2) {
            LOG_WARN("[LaserDataLoader] Invalid point count in Poly header: %s\n", line.c_str());
            continue;
        }

        // 读取 num 个点坐标
        std::vector<SVzNLPointXYZRGBA> points;
        for (int i = 0; i < num; ) {
            if (!std::getline(inputFile, line)) break;
            TrimCarriageReturn(line);
            if (line.empty()) {
                continue;  // 跳过空行，不计数
            }

            SVzNLPointXYZRGBA pt;
            memset(&pt, 0, sizeof(pt));
            pt.nRGB = 0x00FFFFFF;  // 默认白色
            bool parsed = false;

            // 尝试花括号格式
            if (line.find('{') != std::string::npos) {
                float fx, fy, fz;
                int R = 255, G = 255, B = 255, A = 255;

                // 新RGBD格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A}
                int cnt = sscanf(line.c_str(),
                    " { %f , %f , %f } - { %*f , %*f } - { %*f , %*f } - { %d , %d , %d , %d }",
                    &fx, &fy, &fz, &R, &G, &B, &A);
                if (cnt >= 3) {
                    pt.x = fx;
                    pt.y = fy;
                    pt.z = fz;
                    if (cnt >= 7) {
                        // 解析到了颜色 R,G,B,A（0-255），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;
                }

                // 旧RGBD格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry}
                if (!parsed) {
                    float r, g, b;
                    if (sscanf(line.c_str(), " { %f , %f , %f , %f , %f , %f }",
                               &fx, &fy, &fz, &r, &g, &b) == 6) {
                        pt.x = fx;
                        pt.y = fy;
                        pt.z = fz;
                        int nRGB = (int)(b * 255); nRGB <<= 8;
                        nRGB += (int)(g * 255); nRGB <<= 8;
                        nRGB += (int)(r * 255);
                        pt.nRGB = nRGB;
                        parsed = true;
                    }
                }

                // XYZ格式: {x,y,z}-{...}-{...}
                if (!parsed) {
                    if (sscanf(line.c_str(), " { %f , %f , %f }", &fx, &fy, &fz) == 3) {
                        pt.x = fx;
                        pt.y = fy;
                        pt.z = fz;
                        parsed = true;
                    }
                }
            }

            // 尝试纯数字格式: x y z（空格/Tab/逗号分隔）
            if (!parsed) {
                float fx, fy, fz;
                std::string lineCopy = line;
                for (char& c : lineCopy) {
                    if (c == ',' || c == '\t') c = ' ';
                }
                if (sscanf(lineCopy.c_str(), "%f %f %f", &fx, &fy, &fz) == 3) {
                    pt.x = fx;
                    pt.y = fy;
                    pt.z = fz;
                    parsed = true;
                }
            }

            if (parsed) {
                points.push_back(pt);
            }
            ++i;
        }

        if (points.size() >= 2) {
            polyLines.push_back(std::move(points));
        }
    }

    inputFile.close();
    LOG_INFO("Loaded %zu poly lines from file: %s\n", polyLines.size(), fileName.c_str());
    return SUCCESS;
}

void LaserDataLoader::FreeLaserScanData(std::vector<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,
                                       size_t* validPointCount)
{
    LOG_DEBUG("Converting unified data to scan lines format\n");

    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;

        // 只处理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]);

                // 统计有效点（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;
    }

    LOG_DEBUG("Converted %zu lines to scan lines format, valid points: %zu\n", scanLines.size(), validCount);
    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 leftX, leftY;
    float rightX, rightY;

    // 尝试新格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A}
    int R, G, B, A;
    int parsed = sscanf(data.c_str(),
        " { %f , %f , %f } - { %f , %f } - { %f , %f } - { %d , %d , %d , %d }",
        &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY, &R, &G, &B, &A);
    if (parsed >= 11) {
        sData.x = X;
        sData.y = Y;
        sData.z = Z;

        // R,G,B,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, g, b;
    sscanf(data.c_str(),
        " { %f , %f , %f , %f , %f , %f } - { %f , %f } - { %f , %f }",
        &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY);
    sData.x = X;
    sData.y = Y;
    sData.z = Z;

    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) {
                        // XYZ格式: {x,y,z}-{lx,ly}-{rx,ry}
                        eDataType = keResultDataType_Position;
                        bFind = true;
                    }
                }
            }
            break;
        }
    }

    inputFile.close();
    return bFind ? SUCCESS : ERR_CODE(FILE_ERR_FORMAT);
}