1071 lines
39 KiB
C++
1071 lines
39 KiB
C++
#include "PointCloudConverter.h"
|
||
#include "LaserDataLoader.h"
|
||
#include "VZNL_Types.h"
|
||
#include "VrLog.h"
|
||
|
||
#include <fstream>
|
||
#include <sstream>
|
||
#include <algorithm>
|
||
#include <cctype>
|
||
#include <cmath>
|
||
#include <cstring>
|
||
|
||
PointCloudConverter::PointCloudConverter()
|
||
: m_loadedPointCount(0)
|
||
, m_loadedLineCount(0)
|
||
, m_lastLoadHadColor(false)
|
||
{
|
||
}
|
||
|
||
PointCloudConverter::~PointCloudConverter()
|
||
{
|
||
}
|
||
|
||
std::string PointCloudConverter::getFileExtension(const std::string& fileName)
|
||
{
|
||
size_t pos = fileName.rfind('.');
|
||
if (pos == std::string::npos) {
|
||
return "";
|
||
}
|
||
std::string ext = fileName.substr(pos + 1);
|
||
std::transform(ext.begin(), ext.end(), ext.begin(),
|
||
[](unsigned char c) { return std::tolower(c); });
|
||
return ext;
|
||
}
|
||
|
||
int PointCloudConverter::loadFromTxt(const std::string& fileName, PointCloudXYZ& cloud)
|
||
{
|
||
LaserDataLoader loader;
|
||
|
||
// 使用 CloudUtils 加载数据
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> laserLines;
|
||
int lineNum = 0;
|
||
float scanSpeed = 0.0f;
|
||
int maxTimeStamp = 0;
|
||
int clockPerSecond = 0;
|
||
|
||
int result = loader.LoadLaserScanData(fileName, laserLines, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
|
||
if (result != 0) {
|
||
m_lastError = "加载文件失败: " + loader.GetLastError();
|
||
return result;
|
||
}
|
||
|
||
LOG_INFO("[CloudView] LoadLaserScanData success, laserLines size: %zu, lineNum: %d\n",
|
||
laserLines.size(), lineNum);
|
||
|
||
// 转换为 SVzNL3DPosition 格式
|
||
std::vector<std::vector<SVzNL3DPosition>> scanLines;
|
||
result = loader.ConvertToSVzNL3DPosition(laserLines, scanLines);
|
||
if (result != 0) {
|
||
m_lastError = "转换数据失败";
|
||
loader.FreeLaserScanData(laserLines);
|
||
return result;
|
||
}
|
||
|
||
LOG_INFO("[CloudView] ConvertToSVzNL3DPosition success, scanLines size: %zu\n", scanLines.size());
|
||
|
||
// 转换为自定义点云格式，保留线索引（保留所有点包括0,0,0用于旋转）
|
||
cloud.clear();
|
||
size_t totalCount = 0;
|
||
int lineIndex = 0;
|
||
|
||
for (const auto& line : scanLines) {
|
||
for (const auto& pos : line) {
|
||
totalCount++;
|
||
Point3D point;
|
||
point.x = static_cast<float>(pos.pt3D.x);
|
||
point.y = static_cast<float>(pos.pt3D.y);
|
||
point.z = static_cast<float>(pos.pt3D.z);
|
||
cloud.push_back(point, lineIndex);
|
||
}
|
||
lineIndex++;
|
||
}
|
||
|
||
LOG_INFO("[CloudView] Total points: %zu, Lines: %d\n", totalCount, lineIndex);
|
||
|
||
loader.FreeLaserScanData(laserLines);
|
||
m_loadedPointCount = totalCount;
|
||
m_loadedLineCount = lineIndex;
|
||
return 0;
|
||
}
|
||
|
||
int PointCloudConverter::loadFromTxt(const std::string& fileName, PointCloudXYZRGB& cloud)
|
||
{
|
||
LaserDataLoader loader;
|
||
|
||
// 使用 CloudUtils 加载数据
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> laserLines;
|
||
int lineNum = 0;
|
||
float scanSpeed = 0.0f;
|
||
int maxTimeStamp = 0;
|
||
int clockPerSecond = 0;
|
||
|
||
int result = loader.LoadLaserScanData(fileName, laserLines, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
|
||
if (result != 0) {
|
||
m_lastError = "加载文件失败，loasreuslt: " + std::to_string(result);
|
||
return result;
|
||
}
|
||
|
||
LOG_INFO("[CloudView] LoadLaserScanData(XYZRGB) success, laserLines size: %zu, lineNum: %d\n",
|
||
laserLines.size(), lineNum);
|
||
|
||
// 检查数据类型是否包含 RGBA
|
||
bool hasRGBA = false;
|
||
for (const auto& linePair : laserLines) {
|
||
if (linePair.first == keResultDataType_PointXYZRGBA) {
|
||
hasRGBA = true;
|
||
break;
|
||
}
|
||
}
|
||
|
||
cloud.clear();
|
||
size_t totalCount = 0;
|
||
int lineIndex = 0;
|
||
|
||
if (hasRGBA) {
|
||
// RGBA 路径：使用 ConvertToSVzNLXYZRGBDLaserLine 转换
|
||
std::vector<SVzNLXYZRGBDLaserLine> rgbdData;
|
||
result = loader.ConvertToSVzNLXYZRGBDLaserLine(laserLines, rgbdData);
|
||
if (result != 0) {
|
||
m_lastError = "转换RGBA数据失败";
|
||
loader.FreeLaserScanData(laserLines);
|
||
return result;
|
||
}
|
||
|
||
LOG_INFO("[CloudView] ConvertToSVzNLXYZRGBDLaserLine success, rgbdData size: %zu\n", rgbdData.size());
|
||
|
||
for (const auto& line : rgbdData) {
|
||
for (int i = 0; i < line.nPointCnt; ++i) {
|
||
const SVzNLPointXYZRGBA& pt = line.p3DPoint[i];
|
||
// 解包颜色：nRGB 格式为 (A << 24) | (B << 16) | (G << 8) | R
|
||
uint8_t r = static_cast<uint8_t>(pt.nRGB & 0xFF);
|
||
uint8_t g = static_cast<uint8_t>((pt.nRGB >> 8) & 0xFF);
|
||
uint8_t b = static_cast<uint8_t>((pt.nRGB >> 16) & 0xFF);
|
||
uint8_t a = static_cast<uint8_t>((pt.nRGB >> 24) & 0xFF);
|
||
// A > 1 时作为点大小使用
|
||
float pointSize = (a > 1) ? static_cast<float>(a) : 0.0f;
|
||
|
||
Point3DRGB point(pt.x, pt.y, pt.z, r, g, b, pointSize);
|
||
cloud.push_back(point, lineIndex);
|
||
totalCount++;
|
||
}
|
||
lineIndex++;
|
||
}
|
||
|
||
loader.FreeConvertedData(rgbdData);
|
||
m_lastLoadHadColor = true;
|
||
} else {
|
||
// 非 RGBA 路径：回退到 SVzNL3DPosition + 白色
|
||
std::vector<std::vector<SVzNL3DPosition>> scanLines;
|
||
result = loader.ConvertToSVzNL3DPosition(laserLines, scanLines);
|
||
if (result != 0) {
|
||
m_lastError = "转换数据失败";
|
||
loader.FreeLaserScanData(laserLines);
|
||
return result;
|
||
}
|
||
|
||
LOG_INFO("[CloudView] ConvertToSVzNL3DPosition success, scanLines size: %zu\n", scanLines.size());
|
||
|
||
for (const auto& line : scanLines) {
|
||
for (const auto& pos : line) {
|
||
Point3DRGB point(
|
||
static_cast<float>(pos.pt3D.x),
|
||
static_cast<float>(pos.pt3D.y),
|
||
static_cast<float>(pos.pt3D.z),
|
||
255, 255, 255);
|
||
cloud.push_back(point, lineIndex);
|
||
totalCount++;
|
||
}
|
||
lineIndex++;
|
||
}
|
||
|
||
m_lastLoadHadColor = false;
|
||
}
|
||
|
||
LOG_INFO("[CloudView] Total points(XYZRGB): %zu, Lines: %d, hasRGBA: %d\n", totalCount, lineIndex, hasRGBA);
|
||
|
||
loader.FreeLaserScanData(laserLines);
|
||
m_loadedPointCount = totalCount;
|
||
m_loadedLineCount = lineIndex;
|
||
return 0;
|
||
}
|
||
|
||
bool PointCloudConverter::parsePcdHeader(std::ifstream& file, PcdHeader& header)
|
||
{
|
||
std::string line;
|
||
while (std::getline(file, line)) {
|
||
if (line.empty() || line[0] == '#') {
|
||
continue;
|
||
}
|
||
|
||
std::istringstream iss(line);
|
||
std::string key;
|
||
iss >> key;
|
||
|
||
if (key == "VERSION") {
|
||
// 忽略版本
|
||
} else if (key == "FIELDS") {
|
||
std::string field;
|
||
while (iss >> field) {
|
||
header.fields.push_back(field);
|
||
if (field == "rgb" || field == "rgba") {
|
||
header.hasRgb = true;
|
||
}
|
||
}
|
||
} else if (key == "SIZE") {
|
||
int size;
|
||
while (iss >> size) {
|
||
header.fieldSizes.push_back(size);
|
||
header.pointSize += size;
|
||
}
|
||
} else if (key == "TYPE") {
|
||
char type;
|
||
while (iss >> type) {
|
||
header.fieldTypes.push_back(type);
|
||
}
|
||
} else if (key == "COUNT") {
|
||
// 忽略 COUNT
|
||
} else if (key == "WIDTH") {
|
||
iss >> header.width;
|
||
} else if (key == "HEIGHT") {
|
||
iss >> header.height;
|
||
} else if (key == "VIEWPOINT") {
|
||
// 忽略 VIEWPOINT
|
||
} else if (key == "POINTS") {
|
||
iss >> header.points;
|
||
} else if (key == "DATA") {
|
||
std::string dataType;
|
||
iss >> dataType;
|
||
header.isBinary = (dataType == "binary" || dataType == "binary_compressed");
|
||
return true; // 头部解析完成
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
int PointCloudConverter::loadFromPcd(const std::string& fileName, PointCloudXYZ& cloud)
|
||
{
|
||
std::ifstream file(fileName, std::ios::binary);
|
||
if (!file.is_open()) {
|
||
m_lastError = "无法打开文件: " + fileName;
|
||
return -1;
|
||
}
|
||
|
||
PcdHeader header;
|
||
if (!parsePcdHeader(file, header)) {
|
||
m_lastError = "无法解析 PCD 文件头";
|
||
return -1;
|
||
}
|
||
|
||
int numPoints = header.points > 0 ? header.points : header.width * header.height;
|
||
LOG_INFO("[CloudView] PCD header: points=%d, width=%d, height=%d, isBinary=%d\n",
|
||
numPoints, header.width, header.height, header.isBinary);
|
||
|
||
cloud.clear();
|
||
cloud.reserve(numPoints);
|
||
|
||
// 查找 x, y, z 字段的索引
|
||
int xIdx = -1, yIdx = -1, zIdx = -1;
|
||
for (size_t i = 0; i < header.fields.size(); ++i) {
|
||
if (header.fields[i] == "x") xIdx = static_cast<int>(i);
|
||
else if (header.fields[i] == "y") yIdx = static_cast<int>(i);
|
||
else if (header.fields[i] == "z") zIdx = static_cast<int>(i);
|
||
}
|
||
|
||
if (xIdx < 0 || yIdx < 0 || zIdx < 0) {
|
||
m_lastError = "PCD 文件缺少 x, y, z 字段";
|
||
return -1;
|
||
}
|
||
|
||
if (header.isBinary) {
|
||
// 二进制格式
|
||
std::vector<char> buffer(header.pointSize);
|
||
for (int i = 0; i < numPoints; ++i) {
|
||
file.read(buffer.data(), header.pointSize);
|
||
if (!file) break;
|
||
|
||
Point3D pt;
|
||
int offset = 0;
|
||
for (size_t j = 0; j < header.fields.size(); ++j) {
|
||
if (j == static_cast<size_t>(xIdx)) {
|
||
memcpy(&pt.x, buffer.data() + offset, sizeof(float));
|
||
} else if (j == static_cast<size_t>(yIdx)) {
|
||
memcpy(&pt.y, buffer.data() + offset, sizeof(float));
|
||
} else if (j == static_cast<size_t>(zIdx)) {
|
||
memcpy(&pt.z, buffer.data() + offset, sizeof(float));
|
||
}
|
||
offset += header.fieldSizes[j];
|
||
}
|
||
|
||
if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
|
||
cloud.push_back(pt);
|
||
}
|
||
}
|
||
} else {
|
||
// ASCII 格式
|
||
std::string line;
|
||
while (std::getline(file, line) && cloud.size() < static_cast<size_t>(numPoints)) {
|
||
std::istringstream iss(line);
|
||
std::vector<float> values;
|
||
float val;
|
||
while (iss >> val) {
|
||
values.push_back(val);
|
||
}
|
||
|
||
if (values.size() >= 3 &&
|
||
static_cast<size_t>(xIdx) < values.size() &&
|
||
static_cast<size_t>(yIdx) < values.size() &&
|
||
static_cast<size_t>(zIdx) < values.size()) {
|
||
Point3D pt;
|
||
pt.x = values[xIdx];
|
||
pt.y = values[yIdx];
|
||
pt.z = values[zIdx];
|
||
|
||
if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
|
||
cloud.push_back(pt);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
LOG_INFO("[CloudView] Loaded %zu points from PCD\n", cloud.size());
|
||
m_loadedPointCount = cloud.size();
|
||
m_loadedLineCount = 0; // PCD 文件没有线信息
|
||
return 0;
|
||
}
|
||
|
||
int PointCloudConverter::loadFromPcd(const std::string& fileName, PointCloudXYZRGB& cloud)
|
||
{
|
||
std::ifstream file(fileName, std::ios::binary);
|
||
if (!file.is_open()) {
|
||
m_lastError = "无法打开文件: " + fileName;
|
||
return -1;
|
||
}
|
||
|
||
PcdHeader header;
|
||
if (!parsePcdHeader(file, header)) {
|
||
m_lastError = "无法解析 PCD 文件头";
|
||
return -1;
|
||
}
|
||
|
||
int numPoints = header.points > 0 ? header.points : header.width * header.height;
|
||
cloud.clear();
|
||
cloud.reserve(numPoints);
|
||
|
||
// 查找字段索引
|
||
int xIdx = -1, yIdx = -1, zIdx = -1, rgbIdx = -1;
|
||
for (size_t i = 0; i < header.fields.size(); ++i) {
|
||
if (header.fields[i] == "x") xIdx = static_cast<int>(i);
|
||
else if (header.fields[i] == "y") yIdx = static_cast<int>(i);
|
||
else if (header.fields[i] == "z") zIdx = static_cast<int>(i);
|
||
else if (header.fields[i] == "rgb" || header.fields[i] == "rgba") rgbIdx = static_cast<int>(i);
|
||
}
|
||
|
||
if (xIdx < 0 || yIdx < 0 || zIdx < 0) {
|
||
m_lastError = "PCD 文件缺少 x, y, z 字段";
|
||
return -1;
|
||
}
|
||
|
||
if (header.isBinary) {
|
||
std::vector<char> buffer(header.pointSize);
|
||
for (int i = 0; i < numPoints; ++i) {
|
||
file.read(buffer.data(), header.pointSize);
|
||
if (!file) break;
|
||
|
||
Point3DRGB pt;
|
||
int offset = 0;
|
||
for (size_t j = 0; j < header.fields.size(); ++j) {
|
||
if (j == static_cast<size_t>(xIdx)) {
|
||
memcpy(&pt.x, buffer.data() + offset, sizeof(float));
|
||
} else if (j == static_cast<size_t>(yIdx)) {
|
||
memcpy(&pt.y, buffer.data() + offset, sizeof(float));
|
||
} else if (j == static_cast<size_t>(zIdx)) {
|
||
memcpy(&pt.z, buffer.data() + offset, sizeof(float));
|
||
} else if (j == static_cast<size_t>(rgbIdx)) {
|
||
// RGB 通常存储为 packed float
|
||
float rgbFloat;
|
||
memcpy(&rgbFloat, buffer.data() + offset, sizeof(float));
|
||
uint32_t rgb = *reinterpret_cast<uint32_t*>(&rgbFloat);
|
||
pt.r = (rgb >> 16) & 0xFF;
|
||
pt.g = (rgb >> 8) & 0xFF;
|
||
pt.b = rgb & 0xFF;
|
||
}
|
||
offset += header.fieldSizes[j];
|
||
}
|
||
|
||
if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
|
||
cloud.push_back(pt);
|
||
}
|
||
}
|
||
} else {
|
||
std::string line;
|
||
while (std::getline(file, line) && cloud.size() < static_cast<size_t>(numPoints)) {
|
||
std::istringstream iss(line);
|
||
std::vector<float> values;
|
||
float val;
|
||
while (iss >> val) {
|
||
values.push_back(val);
|
||
}
|
||
|
||
if (values.size() >= 3 &&
|
||
static_cast<size_t>(xIdx) < values.size() &&
|
||
static_cast<size_t>(yIdx) < values.size() &&
|
||
static_cast<size_t>(zIdx) < values.size()) {
|
||
Point3DRGB pt;
|
||
pt.x = values[xIdx];
|
||
pt.y = values[yIdx];
|
||
pt.z = values[zIdx];
|
||
|
||
if (rgbIdx >= 0 && static_cast<size_t>(rgbIdx) < values.size()) {
|
||
float rgbFloat = values[rgbIdx];
|
||
uint32_t rgb = *reinterpret_cast<uint32_t*>(&rgbFloat);
|
||
pt.r = (rgb >> 16) & 0xFF;
|
||
pt.g = (rgb >> 8) & 0xFF;
|
||
pt.b = rgb & 0xFF;
|
||
}
|
||
|
||
if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
|
||
cloud.push_back(pt);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
m_loadedPointCount = cloud.size();
|
||
m_loadedLineCount = 0; // PCD 文件没有线信息
|
||
m_lastLoadHadColor = (rgbIdx >= 0);
|
||
return 0;
|
||
}
|
||
|
||
int PointCloudConverter::loadFromFile(const std::string& fileName, PointCloudXYZ& cloud)
|
||
{
|
||
std::string ext = getFileExtension(fileName);
|
||
|
||
if (ext == "pcd") {
|
||
return loadFromPcd(fileName, cloud);
|
||
} else if (ext == "txt") {
|
||
return loadFromTxt(fileName, cloud);
|
||
} else if (ext == "ply") {
|
||
return loadFromPly(fileName, cloud);
|
||
} else {
|
||
m_lastError = "不支持的文件格式: " + ext;
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
int PointCloudConverter::loadFromFile(const std::string& fileName, PointCloudXYZRGB& cloud)
|
||
{
|
||
std::string ext = getFileExtension(fileName);
|
||
|
||
if (ext == "pcd") {
|
||
return loadFromPcd(fileName, cloud);
|
||
} else if (ext == "txt") {
|
||
return loadFromTxt(fileName, cloud);
|
||
} else if (ext == "ply") {
|
||
return loadFromPly(fileName, cloud);
|
||
} else {
|
||
m_lastError = "不支持的文件格式: " + ext;
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
int PointCloudConverter::saveToTxt(const std::string& fileName, const PointCloudXYZ& cloud, int lineNum, int linePtNum)
|
||
{
|
||
if (cloud.empty()) {
|
||
m_lastError = "点云数据为空";
|
||
return -1;
|
||
}
|
||
|
||
// 转换为 std::vector<std::vector<SVzNL3DPosition>> 格式
|
||
std::vector<std::vector<SVzNL3DPosition>> xyzData;
|
||
xyzData.resize(lineNum);
|
||
|
||
for (int line = 0; line < lineNum; ++line) {
|
||
xyzData[line].resize(linePtNum);
|
||
}
|
||
|
||
// 填充数据
|
||
size_t ptIdx = 0;
|
||
for (int line = 0; line < lineNum; ++line) {
|
||
for (int j = 0; j < linePtNum; ++j) {
|
||
if (ptIdx < cloud.points.size()) {
|
||
xyzData[line][j].pt3D.x = cloud.points[ptIdx].x;
|
||
xyzData[line][j].pt3D.y = cloud.points[ptIdx].y;
|
||
xyzData[line][j].pt3D.z = cloud.points[ptIdx].z;
|
||
xyzData[line][j].nPointIdx = j;
|
||
ptIdx++;
|
||
} else {
|
||
// 填充零点
|
||
xyzData[line][j].pt3D.x = 0;
|
||
xyzData[line][j].pt3D.y = 0;
|
||
xyzData[line][j].pt3D.z = 0;
|
||
xyzData[line][j].nPointIdx = j;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 使用 LaserDataLoader 保存
|
||
LaserDataLoader loader;
|
||
int result = loader.DebugSaveLaser(fileName, xyzData);
|
||
if (result != 0) {
|
||
m_lastError = "保存文件失败: " + loader.GetLastError();
|
||
return result;
|
||
}
|
||
|
||
LOG_INFO("[CloudView] Saved %zu points to %s (lineNum=%d, linePtNum=%d)\n",
|
||
cloud.points.size(), fileName.c_str(), lineNum, linePtNum);
|
||
return 0;
|
||
}
|
||
|
||
int PointCloudConverter::getPlyTypeByteSize(const std::string& type)
|
||
{
|
||
if (type == "char" || type == "int8") return 1;
|
||
if (type == "uchar" || type == "uint8") return 1;
|
||
if (type == "short" || type == "int16") return 2;
|
||
if (type == "ushort" || type == "uint16") return 2;
|
||
if (type == "int" || type == "int32") return 4;
|
||
if (type == "uint" || type == "uint32") return 4;
|
||
if (type == "float" || type == "float32") return 4;
|
||
if (type == "double" || type == "float64") return 8;
|
||
return 0;
|
||
}
|
||
|
||
int PointCloudConverter::calcPlyElementStride(const PlyElement& element)
|
||
{
|
||
int stride = 0;
|
||
for (const auto& prop : element.properties) {
|
||
if (prop.isList) return -1; // list 类型无法计算固定 stride
|
||
stride += prop.byteSize;
|
||
}
|
||
return stride;
|
||
}
|
||
|
||
bool PointCloudConverter::parsePlyHeader(std::ifstream& file, PlyHeader& header)
|
||
{
|
||
std::string line;
|
||
|
||
// 第一行必须是 "ply"
|
||
if (!std::getline(file, line)) return false;
|
||
// 去除行尾可能的 \r
|
||
if (!line.empty() && line.back() == '\r') line.pop_back();
|
||
if (line != "ply") return false;
|
||
|
||
PlyElement* currentElement = nullptr;
|
||
|
||
while (std::getline(file, line)) {
|
||
// 去除行尾 \r
|
||
if (!line.empty() && line.back() == '\r') line.pop_back();
|
||
if (line.empty()) continue;
|
||
|
||
std::istringstream iss(line);
|
||
std::string keyword;
|
||
iss >> keyword;
|
||
|
||
if (keyword == "format") {
|
||
std::string fmt;
|
||
iss >> fmt;
|
||
if (fmt == "ascii") header.format = PlyHeader::ASCII;
|
||
else if (fmt == "binary_little_endian") header.format = PlyHeader::BINARY_LE;
|
||
else if (fmt == "binary_big_endian") header.format = PlyHeader::BINARY_BE;
|
||
else return false;
|
||
} else if (keyword == "comment" || keyword == "obj_info") {
|
||
// 跳过注释
|
||
} else if (keyword == "element") {
|
||
PlyElement elem;
|
||
iss >> elem.name >> elem.count;
|
||
header.elements.push_back(elem);
|
||
currentElement = &header.elements.back();
|
||
if (elem.name == "vertex") {
|
||
header.vertexElementIndex = static_cast<int>(header.elements.size()) - 1;
|
||
}
|
||
} else if (keyword == "property") {
|
||
if (!currentElement) return false;
|
||
std::string secondToken;
|
||
iss >> secondToken;
|
||
|
||
PlyProperty prop;
|
||
if (secondToken == "list") {
|
||
// property list <count_type> <value_type> <name>
|
||
prop.isList = true;
|
||
iss >> prop.listCountType >> prop.listValueType >> prop.name;
|
||
prop.byteSize = 0; // list 类型没有固定大小
|
||
} else {
|
||
// property <type> <name>
|
||
prop.type = secondToken;
|
||
iss >> prop.name;
|
||
prop.byteSize = getPlyTypeByteSize(prop.type);
|
||
prop.isList = false;
|
||
}
|
||
currentElement->properties.push_back(prop);
|
||
} else if (keyword == "end_header") {
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/**
|
||
* @brief 从二进制缓冲区读取指定 PLY 类型的值并转为 float
|
||
*/
|
||
static float readPlyValueAsFloat(const char* data, const std::string& type, bool swapEndian)
|
||
{
|
||
if (type == "float" || type == "float32") {
|
||
float v;
|
||
memcpy(&v, data, sizeof(float));
|
||
if (swapEndian) {
|
||
char* p = reinterpret_cast<char*>(&v);
|
||
std::swap(p[0], p[3]);
|
||
std::swap(p[1], p[2]);
|
||
}
|
||
return v;
|
||
} else if (type == "double" || type == "float64") {
|
||
double v;
|
||
memcpy(&v, data, sizeof(double));
|
||
if (swapEndian) {
|
||
char* p = reinterpret_cast<char*>(&v);
|
||
std::swap(p[0], p[7]);
|
||
std::swap(p[1], p[6]);
|
||
std::swap(p[2], p[5]);
|
||
std::swap(p[3], p[4]);
|
||
}
|
||
return static_cast<float>(v);
|
||
} else if (type == "int" || type == "int32") {
|
||
int32_t v;
|
||
memcpy(&v, data, sizeof(int32_t));
|
||
if (swapEndian) {
|
||
char* p = reinterpret_cast<char*>(&v);
|
||
std::swap(p[0], p[3]);
|
||
std::swap(p[1], p[2]);
|
||
}
|
||
return static_cast<float>(v);
|
||
} else if (type == "uint" || type == "uint32") {
|
||
uint32_t v;
|
||
memcpy(&v, data, sizeof(uint32_t));
|
||
if (swapEndian) {
|
||
char* p = reinterpret_cast<char*>(&v);
|
||
std::swap(p[0], p[3]);
|
||
std::swap(p[1], p[2]);
|
||
}
|
||
return static_cast<float>(v);
|
||
} else if (type == "short" || type == "int16") {
|
||
int16_t v;
|
||
memcpy(&v, data, sizeof(int16_t));
|
||
if (swapEndian) {
|
||
char* p = reinterpret_cast<char*>(&v);
|
||
std::swap(p[0], p[1]);
|
||
}
|
||
return static_cast<float>(v);
|
||
} else if (type == "ushort" || type == "uint16") {
|
||
uint16_t v;
|
||
memcpy(&v, data, sizeof(uint16_t));
|
||
if (swapEndian) {
|
||
char* p = reinterpret_cast<char*>(&v);
|
||
std::swap(p[0], p[1]);
|
||
}
|
||
return static_cast<float>(v);
|
||
} else if (type == "uchar" || type == "uint8") {
|
||
uint8_t v;
|
||
memcpy(&v, data, sizeof(uint8_t));
|
||
return static_cast<float>(v);
|
||
} else if (type == "char" || type == "int8") {
|
||
int8_t v;
|
||
memcpy(&v, data, sizeof(int8_t));
|
||
return static_cast<float>(v);
|
||
}
|
||
return 0.0f;
|
||
}
|
||
|
||
/**
|
||
* @brief 从二进制缓冲区读取指定 PLY 类型的值并转为 uint8_t（用于颜色）
|
||
*/
|
||
static uint8_t readPlyValueAsUint8(const char* data, const std::string& type, bool swapEndian)
|
||
{
|
||
float v = readPlyValueAsFloat(data, type, swapEndian);
|
||
// 如果是 float/double 类型的颜色（0~1范围），需要映射到 0~255
|
||
if (type == "float" || type == "float32" || type == "double" || type == "float64") {
|
||
if (v >= 0.0f && v <= 1.0f) {
|
||
return static_cast<uint8_t>(v * 255.0f + 0.5f);
|
||
}
|
||
}
|
||
// 截断到 0~255
|
||
if (v < 0.0f) return 0;
|
||
if (v > 255.0f) return 255;
|
||
return static_cast<uint8_t>(v);
|
||
}
|
||
|
||
int PointCloudConverter::loadFromPly(const std::string& fileName, PointCloudXYZ& cloud)
|
||
{
|
||
std::ifstream file(fileName, std::ios::binary);
|
||
if (!file.is_open()) {
|
||
m_lastError = "无法打开文件: " + fileName;
|
||
return -1;
|
||
}
|
||
|
||
PlyHeader header;
|
||
if (!parsePlyHeader(file, header)) {
|
||
m_lastError = "无法解析 PLY 文件头";
|
||
return -1;
|
||
}
|
||
|
||
if (header.vertexElementIndex < 0) {
|
||
m_lastError = "PLY 文件中未找到 vertex 元素";
|
||
return -1;
|
||
}
|
||
|
||
const PlyElement& vertexElem = header.elements[header.vertexElementIndex];
|
||
int numPoints = vertexElem.count;
|
||
|
||
// 查找 x, y, z 属性索引
|
||
int xIdx = -1, yIdx = -1, zIdx = -1;
|
||
for (size_t i = 0; i < vertexElem.properties.size(); ++i) {
|
||
const std::string& name = vertexElem.properties[i].name;
|
||
if (name == "x") xIdx = static_cast<int>(i);
|
||
else if (name == "y") yIdx = static_cast<int>(i);
|
||
else if (name == "z") zIdx = static_cast<int>(i);
|
||
}
|
||
|
||
if (xIdx < 0 || yIdx < 0 || zIdx < 0) {
|
||
m_lastError = "PLY 文件缺少 x, y, z 属性";
|
||
return -1;
|
||
}
|
||
|
||
LOG_INFO("[CloudView] PLY header: vertex count=%d, format=%d\n", numPoints, header.format);
|
||
|
||
cloud.clear();
|
||
cloud.reserve(numPoints);
|
||
|
||
bool swapEndian = (header.format == PlyHeader::BINARY_BE);
|
||
|
||
if (header.format == PlyHeader::ASCII) {
|
||
// 跳过 vertex 之前的元素
|
||
for (int ei = 0; ei < header.vertexElementIndex; ++ei) {
|
||
const PlyElement& elem = header.elements[ei];
|
||
for (int j = 0; j < elem.count; ++j) {
|
||
std::string skipLine;
|
||
if (!std::getline(file, skipLine)) {
|
||
m_lastError = "PLY 文件数据不完整（跳过元素时）";
|
||
return -1;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 读取 vertex 数据
|
||
std::string line;
|
||
for (int i = 0; i < numPoints && std::getline(file, line); ++i) {
|
||
// 去除 \r
|
||
if (!line.empty() && line.back() == '\r') line.pop_back();
|
||
std::istringstream iss(line);
|
||
std::vector<float> values;
|
||
float val;
|
||
while (iss >> val) {
|
||
values.push_back(val);
|
||
}
|
||
|
||
if (static_cast<size_t>(xIdx) < values.size() &&
|
||
static_cast<size_t>(yIdx) < values.size() &&
|
||
static_cast<size_t>(zIdx) < values.size()) {
|
||
Point3D pt(values[xIdx], values[yIdx], values[zIdx]);
|
||
if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
|
||
cloud.push_back(pt);
|
||
}
|
||
}
|
||
}
|
||
} else {
|
||
// Binary 格式（Little-Endian 或 Big-Endian）
|
||
// 跳过 vertex 之前的元素
|
||
for (int ei = 0; ei < header.vertexElementIndex; ++ei) {
|
||
const PlyElement& elem = header.elements[ei];
|
||
int stride = calcPlyElementStride(elem);
|
||
if (stride > 0) {
|
||
// 固定大小元素，直接跳过
|
||
file.seekg(static_cast<std::streamoff>(stride) * elem.count, std::ios::cur);
|
||
} else {
|
||
// 含 list 类型，需要逐条跳过
|
||
for (int j = 0; j < elem.count; ++j) {
|
||
for (const auto& prop : elem.properties) {
|
||
if (prop.isList) {
|
||
int countSize = getPlyTypeByteSize(prop.listCountType);
|
||
char countBuf[8] = {};
|
||
file.read(countBuf, countSize);
|
||
// 读取 list 计数值
|
||
uint32_t listCount = 0;
|
||
if (countSize == 1) listCount = static_cast<uint8_t>(countBuf[0]);
|
||
else if (countSize == 2) { uint16_t v; memcpy(&v, countBuf, 2); listCount = v; }
|
||
else if (countSize == 4) { uint32_t v; memcpy(&v, countBuf, 4); listCount = v; }
|
||
int valueSize = getPlyTypeByteSize(prop.listValueType);
|
||
file.seekg(static_cast<std::streamoff>(valueSize) * listCount, std::ios::cur);
|
||
} else {
|
||
file.seekg(prop.byteSize, std::ios::cur);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 计算 vertex 的 stride
|
||
int vertexStride = calcPlyElementStride(vertexElem);
|
||
if (vertexStride <= 0) {
|
||
m_lastError = "PLY vertex 元素包含 list 属性，不支持";
|
||
return -1;
|
||
}
|
||
|
||
// 计算各属性在 vertex 记录中的偏移
|
||
std::vector<int> offsets(vertexElem.properties.size(), 0);
|
||
int offset = 0;
|
||
for (size_t i = 0; i < vertexElem.properties.size(); ++i) {
|
||
offsets[i] = offset;
|
||
offset += vertexElem.properties[i].byteSize;
|
||
}
|
||
|
||
// 读取所有 vertex 数据
|
||
std::vector<char> buffer(static_cast<size_t>(vertexStride) * numPoints);
|
||
file.read(buffer.data(), buffer.size());
|
||
if (!file) {
|
||
m_lastError = "PLY 文件数据不完整";
|
||
return -1;
|
||
}
|
||
|
||
for (int i = 0; i < numPoints; ++i) {
|
||
const char* record = buffer.data() + static_cast<size_t>(i) * vertexStride;
|
||
float x = readPlyValueAsFloat(record + offsets[xIdx], vertexElem.properties[xIdx].type, swapEndian);
|
||
float y = readPlyValueAsFloat(record + offsets[yIdx], vertexElem.properties[yIdx].type, swapEndian);
|
||
float z = readPlyValueAsFloat(record + offsets[zIdx], vertexElem.properties[zIdx].type, swapEndian);
|
||
|
||
if (std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) {
|
||
cloud.push_back(Point3D(x, y, z));
|
||
}
|
||
}
|
||
}
|
||
|
||
LOG_INFO("[CloudView] Loaded %zu points from PLY\n", cloud.size());
|
||
m_loadedPointCount = cloud.size();
|
||
m_loadedLineCount = 0; // PLY 文件没有线信息
|
||
return 0;
|
||
}
|
||
|
||
int PointCloudConverter::loadFromPly(const std::string& fileName, PointCloudXYZRGB& cloud)
|
||
{
|
||
std::ifstream file(fileName, std::ios::binary);
|
||
if (!file.is_open()) {
|
||
m_lastError = "无法打开文件: " + fileName;
|
||
return -1;
|
||
}
|
||
|
||
PlyHeader header;
|
||
if (!parsePlyHeader(file, header)) {
|
||
m_lastError = "无法解析 PLY 文件头";
|
||
return -1;
|
||
}
|
||
|
||
if (header.vertexElementIndex < 0) {
|
||
m_lastError = "PLY 文件中未找到 vertex 元素";
|
||
return -1;
|
||
}
|
||
|
||
const PlyElement& vertexElem = header.elements[header.vertexElementIndex];
|
||
int numPoints = vertexElem.count;
|
||
|
||
// 查找属性索引
|
||
int xIdx = -1, yIdx = -1, zIdx = -1;
|
||
int rIdx = -1, gIdx = -1, bIdx = -1;
|
||
for (size_t i = 0; i < vertexElem.properties.size(); ++i) {
|
||
const std::string& name = vertexElem.properties[i].name;
|
||
if (name == "x") xIdx = static_cast<int>(i);
|
||
else if (name == "y") yIdx = static_cast<int>(i);
|
||
else if (name == "z") zIdx = static_cast<int>(i);
|
||
else if (name == "red" || name == "r") rIdx = static_cast<int>(i);
|
||
else if (name == "green" || name == "g") gIdx = static_cast<int>(i);
|
||
else if (name == "blue" || name == "b") bIdx = static_cast<int>(i);
|
||
}
|
||
|
||
if (xIdx < 0 || yIdx < 0 || zIdx < 0) {
|
||
m_lastError = "PLY 文件缺少 x, y, z 属性";
|
||
return -1;
|
||
}
|
||
|
||
bool hasColor = (rIdx >= 0 && gIdx >= 0 && bIdx >= 0);
|
||
|
||
LOG_INFO("[CloudView] PLY header(XYZRGB): vertex count=%d, format=%d, hasColor=%d\n",
|
||
numPoints, header.format, hasColor);
|
||
|
||
cloud.clear();
|
||
cloud.reserve(numPoints);
|
||
|
||
bool swapEndian = (header.format == PlyHeader::BINARY_BE);
|
||
|
||
if (header.format == PlyHeader::ASCII) {
|
||
// 跳过 vertex 之前的元素
|
||
for (int ei = 0; ei < header.vertexElementIndex; ++ei) {
|
||
const PlyElement& elem = header.elements[ei];
|
||
for (int j = 0; j < elem.count; ++j) {
|
||
std::string skipLine;
|
||
if (!std::getline(file, skipLine)) {
|
||
m_lastError = "PLY 文件数据不完整（跳过元素时）";
|
||
return -1;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 读取 vertex 数据
|
||
std::string line;
|
||
for (int i = 0; i < numPoints && std::getline(file, line); ++i) {
|
||
if (!line.empty() && line.back() == '\r') line.pop_back();
|
||
std::istringstream iss(line);
|
||
std::vector<float> values;
|
||
float val;
|
||
while (iss >> val) {
|
||
values.push_back(val);
|
||
}
|
||
|
||
if (static_cast<size_t>(xIdx) < values.size() &&
|
||
static_cast<size_t>(yIdx) < values.size() &&
|
||
static_cast<size_t>(zIdx) < values.size()) {
|
||
Point3DRGB pt;
|
||
pt.x = values[xIdx];
|
||
pt.y = values[yIdx];
|
||
pt.z = values[zIdx];
|
||
|
||
if (hasColor &&
|
||
static_cast<size_t>(rIdx) < values.size() &&
|
||
static_cast<size_t>(gIdx) < values.size() &&
|
||
static_cast<size_t>(bIdx) < values.size()) {
|
||
pt.r = static_cast<uint8_t>(std::min(255.0f, std::max(0.0f, values[rIdx])));
|
||
pt.g = static_cast<uint8_t>(std::min(255.0f, std::max(0.0f, values[gIdx])));
|
||
pt.b = static_cast<uint8_t>(std::min(255.0f, std::max(0.0f, values[bIdx])));
|
||
}
|
||
|
||
if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
|
||
cloud.push_back(pt);
|
||
}
|
||
}
|
||
}
|
||
} else {
|
||
// Binary 格式
|
||
// 跳过 vertex 之前的元素
|
||
for (int ei = 0; ei < header.vertexElementIndex; ++ei) {
|
||
const PlyElement& elem = header.elements[ei];
|
||
int stride = calcPlyElementStride(elem);
|
||
if (stride > 0) {
|
||
file.seekg(static_cast<std::streamoff>(stride) * elem.count, std::ios::cur);
|
||
} else {
|
||
for (int j = 0; j < elem.count; ++j) {
|
||
for (const auto& prop : elem.properties) {
|
||
if (prop.isList) {
|
||
int countSize = getPlyTypeByteSize(prop.listCountType);
|
||
char countBuf[8] = {};
|
||
file.read(countBuf, countSize);
|
||
uint32_t listCount = 0;
|
||
if (countSize == 1) listCount = static_cast<uint8_t>(countBuf[0]);
|
||
else if (countSize == 2) { uint16_t v; memcpy(&v, countBuf, 2); listCount = v; }
|
||
else if (countSize == 4) { uint32_t v; memcpy(&v, countBuf, 4); listCount = v; }
|
||
int valueSize = getPlyTypeByteSize(prop.listValueType);
|
||
file.seekg(static_cast<std::streamoff>(valueSize) * listCount, std::ios::cur);
|
||
} else {
|
||
file.seekg(prop.byteSize, std::ios::cur);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 计算 vertex 的 stride
|
||
int vertexStride = calcPlyElementStride(vertexElem);
|
||
if (vertexStride <= 0) {
|
||
m_lastError = "PLY vertex 元素包含 list 属性，不支持";
|
||
return -1;
|
||
}
|
||
|
||
// 计算各属性偏移
|
||
std::vector<int> offsets(vertexElem.properties.size(), 0);
|
||
int offset = 0;
|
||
for (size_t i = 0; i < vertexElem.properties.size(); ++i) {
|
||
offsets[i] = offset;
|
||
offset += vertexElem.properties[i].byteSize;
|
||
}
|
||
|
||
// 读取所有 vertex 数据
|
||
std::vector<char> buffer(static_cast<size_t>(vertexStride) * numPoints);
|
||
file.read(buffer.data(), buffer.size());
|
||
if (!file) {
|
||
m_lastError = "PLY 文件数据不完整";
|
||
return -1;
|
||
}
|
||
|
||
for (int i = 0; i < numPoints; ++i) {
|
||
const char* record = buffer.data() + static_cast<size_t>(i) * vertexStride;
|
||
|
||
Point3DRGB pt;
|
||
pt.x = readPlyValueAsFloat(record + offsets[xIdx], vertexElem.properties[xIdx].type, swapEndian);
|
||
pt.y = readPlyValueAsFloat(record + offsets[yIdx], vertexElem.properties[yIdx].type, swapEndian);
|
||
pt.z = readPlyValueAsFloat(record + offsets[zIdx], vertexElem.properties[zIdx].type, swapEndian);
|
||
|
||
if (hasColor) {
|
||
pt.r = readPlyValueAsUint8(record + offsets[rIdx], vertexElem.properties[rIdx].type, swapEndian);
|
||
pt.g = readPlyValueAsUint8(record + offsets[gIdx], vertexElem.properties[gIdx].type, swapEndian);
|
||
pt.b = readPlyValueAsUint8(record + offsets[bIdx], vertexElem.properties[bIdx].type, swapEndian);
|
||
}
|
||
|
||
if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
|
||
cloud.push_back(pt);
|
||
}
|
||
}
|
||
}
|
||
|
||
LOG_INFO("[CloudView] Loaded %zu points from PLY (hasColor=%d)\n", cloud.size(), hasColor);
|
||
m_loadedPointCount = cloud.size();
|
||
m_loadedLineCount = 0; // PLY 文件没有线信息
|
||
m_lastLoadHadColor = hasColor;
|
||
return 0;
|
||
}
|
||
|
||
int PointCloudConverter::rotateCloud(const PointCloudXYZ& cloud, PointCloudXYZ& rotatedCloud,
|
||
int lineNum, int linePtNum, int& newLineNum, int& newLinePtNum)
|
||
{
|
||
if (cloud.empty()) {
|
||
m_lastError = "无效的点云数据";
|
||
return -1;
|
||
}
|
||
|
||
if (lineNum <= 0 || linePtNum <= 0) {
|
||
m_lastError = "无效的线信息";
|
||
return -1;
|
||
}
|
||
|
||
// 检查点数是否匹配
|
||
size_t expectedPoints = static_cast<size_t>(lineNum) * static_cast<size_t>(linePtNum);
|
||
if (cloud.points.size() != expectedPoints) {
|
||
m_lastError = "点云数据与线信息不匹配，无法旋转";
|
||
return -1;
|
||
}
|
||
|
||
// 矩阵转置：原来的行列互换
|
||
// 原来: lineNum 条线，每条线 linePtNum 个点
|
||
// 转置后: linePtNum 条线，每条线 lineNum 个点
|
||
newLineNum = linePtNum;
|
||
newLinePtNum = lineNum;
|
||
|
||
rotatedCloud.clear();
|
||
rotatedCloud.reserve(cloud.points.size());
|
||
|
||
// 转置操作：
|
||
// 原来第 line 条线的第 col 个点 -> 新的第 col 条线的第 line 个点
|
||
// 原索引: line * linePtNum + col
|
||
// 新索引: col * lineNum + line
|
||
for (int newLine = 0; newLine < newLineNum; ++newLine) {
|
||
for (int newCol = 0; newCol < newLinePtNum; ++newCol) {
|
||
// newLine 对应原来的 col（点在线内的位置）
|
||
// newCol 对应原来的 line（线号）
|
||
int oldLine = newCol;
|
||
int oldCol = newLine;
|
||
size_t oldIdx = static_cast<size_t>(oldLine) * static_cast<size_t>(linePtNum) + static_cast<size_t>(oldCol);
|
||
|
||
const Point3D& pt = cloud.points[oldIdx];
|
||
rotatedCloud.push_back(pt, newLine);
|
||
}
|
||
}
|
||
|
||
LOG_INFO("[CloudView] Rotated cloud: %zu points, %d lines -> %d lines\n",
|
||
rotatedCloud.points.size(), lineNum, newLineNum);
|
||
return 0;
|
||
}
|