484 lines
16 KiB
C++
484 lines
16 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)
|
||
{
|
||
}
|
||
|
||
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)
|
||
{
|
||
PointCloudXYZ xyzCloud;
|
||
int result = loadFromTxt(fileName, xyzCloud);
|
||
if (result != 0) {
|
||
return result;
|
||
}
|
||
|
||
cloud.clear();
|
||
cloud.reserve(xyzCloud.size());
|
||
|
||
for (size_t i = 0; i < xyzCloud.points.size(); ++i) {
|
||
const auto& pt = xyzCloud.points[i];
|
||
Point3DRGB point(pt.x, pt.y, pt.z, 255, 255, 255);
|
||
int lineIdx = (i < xyzCloud.lineIndices.size()) ? xyzCloud.lineIndices[i] : 0;
|
||
cloud.push_back(point, lineIdx);
|
||
}
|
||
|
||
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 文件没有线信息
|
||
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 {
|
||
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 {
|
||
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::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;
|
||
}
|