1294 lines
46 KiB
C++
1294 lines
46 KiB
C++
#include "PointCloudConverter.h"
|
||
#include "LaserDataLoader.h"
|
||
#include "VZNL_Types.h"
|
||
#include "VrLog.h"
|
||
|
||
#include <fstream>
|
||
#include <iomanip>
|
||
#include <sstream>
|
||
#include <algorithm>
|
||
#include <cctype>
|
||
#include <cmath>
|
||
#include <cstring>
|
||
|
||
namespace
|
||
{
|
||
// 应用范围过滤；替换模式保留记录并将越界或非有限坐标归零。
|
||
bool ApplyPointCloudRange(float& x, float& y, float& z, const PointCloudXYZRange& range)
|
||
{
|
||
if (!range.enabled)
|
||
{
|
||
// 范围过滤关闭时保持读取结果原样，和 CloudUtils 的默认行为一致。
|
||
return true;
|
||
}
|
||
|
||
if (range.contains(x, y, z))
|
||
{
|
||
return true;
|
||
}
|
||
|
||
if (range.filterMode == PointCloudXYZRangeFilterMode::Remove)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
x = 0.0f;
|
||
y = 0.0f;
|
||
z = 0.0f;
|
||
return true;
|
||
}
|
||
}
|
||
|
||
PointCloudConverter::PointCloudConverter()
|
||
: m_loadedPointCount(0)
|
||
, m_loadedLineCount(0)
|
||
, m_lastLoadHadColor(false)
|
||
{
|
||
}
|
||
|
||
PointCloudConverter::~PointCloudConverter()
|
||
{
|
||
}
|
||
|
||
void PointCloudConverter::setPointCloudRange(const PointCloudXYZRange& range)
|
||
{
|
||
m_pointCloudRange = range;
|
||
}
|
||
|
||
bool PointCloudConverter::validatePointCloudRange()
|
||
{
|
||
if (m_pointCloudRange.isValid()) {
|
||
return true;
|
||
}
|
||
|
||
m_lastError = "Invalid XYZ point-cloud range";
|
||
m_loadedPointCount = 0;
|
||
m_loadedLineCount = 0;
|
||
m_lastLoadHadColor = false;
|
||
return false;
|
||
}
|
||
|
||
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)
|
||
{
|
||
PointCloudXYZRGB rgbCloud;
|
||
const int result = loadFromTxt(fileName, rgbCloud);
|
||
if (result != 0)
|
||
{
|
||
cloud.clear();
|
||
return result;
|
||
}
|
||
|
||
cloud.clear();
|
||
cloud.reserve(rgbCloud.size());
|
||
for (size_t index = 0; index < rgbCloud.points.size(); ++index)
|
||
{
|
||
const Point3DRGB& sourcePoint = rgbCloud.points[index];
|
||
const int lineIndex = (index < rgbCloud.lineIndices.size())
|
||
? rgbCloud.lineIndices[index] : -1;
|
||
const int pointIndex = (index < rgbCloud.pointInLineIndices.size())
|
||
? rgbCloud.pointInLineIndices[index] : -1;
|
||
cloud.push_back(Point3D(sourcePoint.x, sourcePoint.y, sourcePoint.z),
|
||
lineIndex, pointIndex);
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
int PointCloudConverter::loadFromFileByCallback(
|
||
const std::string& fileName,
|
||
const PointCloudLoadCallback& lineCallback,
|
||
const PointCloudLoadFinishedCallback& finishedCallback)
|
||
{
|
||
// 旧回调只关心点云数据，忽略过滤后的源扫描线索引。
|
||
PointCloudLoadIndexedCallback indexedCallback;
|
||
if (lineCallback)
|
||
{
|
||
indexedCallback = [&](const PointCloudXYZRGB& cloud, bool hasColor,
|
||
int, int)
|
||
{
|
||
lineCallback(cloud, hasColor);
|
||
};
|
||
}
|
||
return loadFromFileByIndexedCallback(fileName, indexedCallback, finishedCallback);
|
||
}
|
||
|
||
int PointCloudConverter::loadFromFileByCallback(
|
||
const std::string& fileName,
|
||
const PointCloudLoadIndexedCallback& lineCallback,
|
||
const PointCloudLoadFinishedCallback& finishedCallback)
|
||
{
|
||
return loadFromFileByIndexedCallback(fileName, lineCallback, finishedCallback);
|
||
}
|
||
|
||
int PointCloudConverter::loadFromFileByIndexedCallback(
|
||
const std::string& fileName,
|
||
const PointCloudLoadIndexedCallback& lineCallback,
|
||
const PointCloudLoadFinishedCallback& finishedCallback)
|
||
{
|
||
if (!validatePointCloudRange())
|
||
{
|
||
if (finishedCallback)
|
||
{
|
||
finishedCallback(-1, 0, 0, false);
|
||
}
|
||
return -1;
|
||
}
|
||
// 加载期间会处理界面事件，因此必须固定本次加载使用的范围。
|
||
const PointCloudXYZRange loadRange = m_pointCloudRange;
|
||
|
||
LaserDataLoader loader;
|
||
size_t pointCount = 0;
|
||
int lineCount = 0;
|
||
bool hasColor = false;
|
||
|
||
const int result = loader.LoadLaserScanDataByIndexedCallback(
|
||
fileName,
|
||
[&](EVzResultDataType dataType, SVzLaserLineData& lineData,
|
||
const LaserLineLoadInfo& loadInfo)
|
||
{
|
||
const int currentLineIndex = lineCount;
|
||
++lineCount;
|
||
PointCloudXYZRGB lineCloud;
|
||
lineCloud.reserve(static_cast<size_t>(std::max(0, lineData.nPointCount)));
|
||
|
||
const bool lineHasColor = dataType == keResultDataType_PointXYZRGBA;
|
||
if (lineHasColor)
|
||
{
|
||
hasColor = true;
|
||
if (lineData.p3DPoint)
|
||
{
|
||
const SVzNLPointXYZRGBA* points =
|
||
static_cast<const SVzNLPointXYZRGBA*>(lineData.p3DPoint);
|
||
for (int i = 0; i < lineData.nPointCount; ++i)
|
||
{
|
||
const SVzNLPointXYZRGBA& point = points[i];
|
||
const uint8_t r = static_cast<uint8_t>(point.nRGB & 0xFF);
|
||
const uint8_t g = static_cast<uint8_t>((point.nRGB >> 8) & 0xFF);
|
||
const uint8_t b = static_cast<uint8_t>((point.nRGB >> 16) & 0xFF);
|
||
const uint8_t a = static_cast<uint8_t>((point.nRGB >> 24) & 0xFF);
|
||
lineCloud.push_back(
|
||
Point3DRGB(point.x, point.y, point.z,
|
||
r, g, b, a > 1 ? static_cast<float>(a) : 0.0f),
|
||
currentLineIndex,
|
||
(static_cast<size_t>(i) < loadInfo.sourcePointIndices.size())
|
||
? loadInfo.sourcePointIndices[static_cast<size_t>(i)] : i);
|
||
}
|
||
}
|
||
}
|
||
else if (dataType == keResultDataType_Position ||
|
||
dataType == keResultDataType_PositionF)
|
||
{
|
||
if (lineData.p3DPoint)
|
||
{
|
||
const SVzNL3DPosition* points =
|
||
static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
|
||
for (int i = 0; i < lineData.nPointCount; ++i)
|
||
{
|
||
const SVzNL3DPosition& point = points[i];
|
||
lineCloud.push_back(
|
||
Point3DRGB(static_cast<float>(point.pt3D.x),
|
||
static_cast<float>(point.pt3D.y),
|
||
static_cast<float>(point.pt3D.z),
|
||
255, 255, 255),
|
||
currentLineIndex,
|
||
(static_cast<size_t>(i) < loadInfo.sourcePointIndices.size())
|
||
? loadInfo.sourcePointIndices[static_cast<size_t>(i)]
|
||
: point.nPointIdx);
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
return;
|
||
}
|
||
|
||
pointCount += lineCloud.size();
|
||
if (lineCallback)
|
||
{
|
||
// 每条源扫描线固定回调一次，空块也不能改变后续源线号。
|
||
lineCallback(lineCloud, lineHasColor, currentLineIndex,
|
||
loadInfo.sourcePointCount);
|
||
}
|
||
},
|
||
[&](int finishedResult, int finishedLineCount, float, int, int)
|
||
{
|
||
if (finishedLineCount > lineCount)
|
||
{
|
||
lineCount = finishedLineCount;
|
||
}
|
||
if (finishedCallback)
|
||
{
|
||
finishedCallback(finishedResult, lineCount, pointCount, hasColor);
|
||
}
|
||
},
|
||
loadRange);
|
||
|
||
if (result != 0)
|
||
{
|
||
m_lastError = "加载文件失败: " + loader.GetLastError();
|
||
}
|
||
m_loadedPointCount = pointCount;
|
||
m_loadedLineCount = lineCount;
|
||
m_lastLoadHadColor = hasColor;
|
||
return result;
|
||
}
|
||
|
||
int PointCloudConverter::loadFromTxt(const std::string& fileName, PointCloudXYZRGB& cloud)
|
||
{
|
||
cloud.clear();
|
||
int finishedResult = -1;
|
||
const int result = loadFromFileByIndexedCallback(
|
||
fileName,
|
||
[&](const PointCloudXYZRGB& chunk, bool, int sourceLineIndex, int)
|
||
{
|
||
for (size_t index = 0; index < chunk.points.size(); ++index)
|
||
{
|
||
const int lineIndex = (index < chunk.lineIndices.size())
|
||
? chunk.lineIndices[index] : sourceLineIndex;
|
||
const int pointIndex = (index < chunk.pointInLineIndices.size())
|
||
? chunk.pointInLineIndices[index] : static_cast<int>(index);
|
||
cloud.push_back(chunk.points[index], lineIndex, pointIndex);
|
||
}
|
||
},
|
||
[&](int loadResult, int, size_t, bool)
|
||
{
|
||
finishedResult = loadResult;
|
||
});
|
||
|
||
if (result != 0 || finishedResult != 0)
|
||
{
|
||
cloud.clear();
|
||
return result != 0 ? result : finishedResult;
|
||
}
|
||
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)
|
||
{
|
||
if (!validatePointCloudRange()) {
|
||
cloud.clear();
|
||
return -1;
|
||
}
|
||
|
||
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 (ApplyPointCloudRange(pt.x, pt.y, pt.z, m_pointCloudRange)) {
|
||
cloud.push_back(pt);
|
||
}
|
||
}
|
||
} else {
|
||
// ASCII 格式
|
||
std::string line;
|
||
int recordsRead = 0;
|
||
while (recordsRead < numPoints && std::getline(file, line)) {
|
||
++recordsRead;
|
||
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 (ApplyPointCloudRange(pt.x, pt.y, pt.z, m_pointCloudRange)) {
|
||
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)
|
||
{
|
||
if (!validatePointCloudRange()) {
|
||
cloud.clear();
|
||
return -1;
|
||
}
|
||
|
||
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 (ApplyPointCloudRange(pt.x, pt.y, pt.z, m_pointCloudRange)) {
|
||
cloud.push_back(pt);
|
||
}
|
||
}
|
||
} else {
|
||
std::string line;
|
||
int recordsRead = 0;
|
||
while (recordsRead < numPoints && std::getline(file, line)) {
|
||
++recordsRead;
|
||
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 (ApplyPointCloudRange(pt.x, pt.y, pt.z, m_pointCloudRange)) {
|
||
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" || ext == "dat") {
|
||
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" || ext == "dat") {
|
||
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;
|
||
}
|
||
|
||
// ============================================================
|
||
// PLY 保存（ASCII 格式）
|
||
// ============================================================
|
||
int PointCloudConverter::saveToPly(const std::string& fileName, const PointCloudXYZ& cloud)
|
||
{
|
||
if (cloud.empty()) {
|
||
m_lastError = "点云数据为空";
|
||
return -1;
|
||
}
|
||
|
||
// 统计有效点数（跳过 NaN/Inf）
|
||
size_t validCount = 0;
|
||
for (const auto& pt : cloud.points) {
|
||
if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z))
|
||
++validCount;
|
||
}
|
||
|
||
if (validCount == 0) {
|
||
m_lastError = "无有效点云数据";
|
||
return -1;
|
||
}
|
||
|
||
std::ofstream file(fileName, std::ios::out | std::ios::trunc);
|
||
if (!file.is_open()) {
|
||
m_lastError = "无法创建文件: " + fileName;
|
||
return -1;
|
||
}
|
||
|
||
// 写入 PLY 头
|
||
file << "ply\n";
|
||
file << "format ascii 1.0\n";
|
||
file << "comment Created by CloudView\n";
|
||
file << "element vertex " << validCount << "\n";
|
||
file << "property float x\n";
|
||
file << "property float y\n";
|
||
file << "property float z\n";
|
||
file << "end_header\n";
|
||
|
||
// 写入点数据（6位小数的定点格式，控制文件体积）
|
||
for (const auto& pt : cloud.points) {
|
||
if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z))
|
||
continue;
|
||
file << std::fixed << std::setprecision(6)
|
||
<< pt.x << " " << pt.y << " " << pt.z << "\n";
|
||
}
|
||
|
||
file.close();
|
||
if (!file.good()) {
|
||
m_lastError = "写入文件失败: " + fileName;
|
||
return -1;
|
||
}
|
||
|
||
LOG_INFO("[CloudView] Saved %zu points to PLY: %s\n", validCount, fileName.c_str());
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// PCD 保存（ASCII 格式，兼容 PCL）
|
||
// ============================================================
|
||
int PointCloudConverter::saveToPcd(const std::string& fileName, const PointCloudXYZ& cloud)
|
||
{
|
||
if (cloud.empty()) {
|
||
m_lastError = "点云数据为空";
|
||
return -1;
|
||
}
|
||
|
||
// 统计有效点数
|
||
size_t validCount = 0;
|
||
for (const auto& pt : cloud.points) {
|
||
if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z))
|
||
++validCount;
|
||
}
|
||
|
||
if (validCount == 0) {
|
||
m_lastError = "无有效点云数据";
|
||
return -1;
|
||
}
|
||
|
||
std::ofstream file(fileName, std::ios::out | std::ios::trunc);
|
||
if (!file.is_open()) {
|
||
m_lastError = "无法创建文件: " + fileName;
|
||
return -1;
|
||
}
|
||
|
||
// 写入 PCD 头（与 PCL 兼容）
|
||
file << "# .PCD v0.7 - Point Cloud Data file format\n";
|
||
file << "VERSION 0.7\n";
|
||
file << "FIELDS x y z\n";
|
||
file << "SIZE 4 4 4\n";
|
||
file << "TYPE F F F\n";
|
||
file << "COUNT 1 1 1\n";
|
||
file << "WIDTH " << validCount << "\n";
|
||
file << "HEIGHT 1\n";
|
||
file << "VIEWPOINT 0 0 0 1 0 0 0\n";
|
||
file << "POINTS " << validCount << "\n";
|
||
file << "DATA ascii\n";
|
||
|
||
// 写入点数据
|
||
for (const auto& pt : cloud.points) {
|
||
if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z))
|
||
continue;
|
||
file << std::fixed << std::setprecision(6)
|
||
<< pt.x << " " << pt.y << " " << pt.z << "\n";
|
||
}
|
||
|
||
file.close();
|
||
if (!file.good()) {
|
||
m_lastError = "写入文件失败: " + fileName;
|
||
return -1;
|
||
}
|
||
|
||
LOG_INFO("[CloudView] Saved %zu points to PCD: %s\n", validCount, fileName.c_str());
|
||
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)
|
||
{
|
||
if (!validatePointCloudRange()) {
|
||
cloud.clear();
|
||
return -1;
|
||
}
|
||
|
||
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 (ApplyPointCloudRange(pt.x, pt.y, pt.z, m_pointCloudRange)) {
|
||
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 (ApplyPointCloudRange(x, y, z, m_pointCloudRange)) {
|
||
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)
|
||
{
|
||
if (!validatePointCloudRange()) {
|
||
cloud.clear();
|
||
return -1;
|
||
}
|
||
|
||
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 (ApplyPointCloudRange(pt.x, pt.y, pt.z, m_pointCloudRange)) {
|
||
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 (ApplyPointCloudRange(pt.x, pt.y, pt.z, m_pointCloudRange)) {
|
||
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;
|
||
}
|