YangOne/Utils
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 4 Wiki Activity

优化数据的存储提升效率;cloudview 增加ply的存储

Browse Source
This commit is contained in:
杰仔 2026-06-14 17:04:39 +08:00
parent c088ad39c4
commit 9fbc9e7edf
10 changed files with 3036 additions and 728 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
CloudUtils/Src/LaserDataLoader.cpp
View File

@ -1,13 +1,15 @@
#include "LaserDataLoader.h"
#include <fstream>
#include <sstream>
#include <cstdio>
#include <cstring>
#include <cstdlib>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <stdexcept>
#include <regex>
#include <cmath>
#include "VrLog.h"
#include <iomanip>
// 辅助函数：去除字符串末尾的 \r 字符（处理 Windows 格式的 CR LF 换行符）
static inline void TrimCarriageReturn(std::string& str)
@ -33,7 +35,7 @@ int LaserDataLoader::LoadLaserScanData(const std::string& fileName,
int& maxTimeStamp,
int& clockPerSecond)
{
LOG_INFO("Loading laser scan data from file: %s\n", fileName.c_str());
LOG_INFO("Loading from file: %s\n", fileName.c_str());
// 清空输出参数
laserLines.clear();
@ -167,7 +169,7 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName,
int maxTimeStamp,
int clockPerSecond)
{
LOG_INFO("Saving unified laser scan data to file: %s\n", fileName.c_str());
LOG_INFO("Saving to file: %s\n", fileName.c_str());
if (laserLines.empty() || lineNum <= 0) {
m_lastError = "Invalid input parameters for saving unified data";
@ -211,6 +213,25 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName,
sw << "{ " << points2D[i].ptLeft2D.x << "," << points2D[i].ptLeft2D.y << " }-";
sw << "{ " << points2D[i].ptRight2D.x << "," << points2D[i].ptRight2D.y << " }" << std::endl;
}
} else if (dataType == keResultDataType_PointXYZI && lineData.p3DPoint) {
// LiDAR 点云：snprintf 批量写入，每 256 点 flush
// 坐标 mm 范围可达 ±200000 → "%.6f" 最长 14 字符 → 整行 ≤73 字符
const SVzNLPointXYZI* points = static_cast<const SVzNLPointXYZI*>(lineData.p3DPoint);
const int n = lineData.nPointCount;
constexpr int BATCH = 256;
constexpr int PER_POINT = 80;
char buf[BATCH * PER_POINT];
for (int base = 0; base < n; base += BATCH)
{
int end = (base + BATCH < n) ? (base + BATCH) : n;
char* p = buf;
for (int i = base; i < end; ++i)
{
p += snprintf(p, PER_POINT, "{ %.6f, %.6f, %.6f } - { 0, 0 } - { 0, 0 }\n",
points[i].fData[0], points[i].fData[1], points[i].fData[2]);
}
sw.write(buf, p - buf);
}
} else if (dataType == keResultDataType_PointXYZRGBA && lineData.p3DPoint) {
// 写入RGBD格式数据
const SVzNLPointXYZRGBA* points = static_cast<const SVzNLPointXYZRGBA*>(lineData.p3DPoint);
@ -222,12 +243,12 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName,
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 << ","
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;

39
CloudView/Inc/CloudViewMainWindow.h
View File

@ -154,25 +154,26 @@ private slots:
void onEulerOrderChanged(int index);
/**
* @brief 从文件加载变换矩阵
* @brief 眼在手外矩阵变换（直接应用矩阵到点云）
*/
void onLoadMatrix();
void onEyeToHandTransform();
/**
* @brief 应用矩阵变换到所有点云
* @brief 眼在手上矩阵变换（手眼矩阵 + 机械臂位姿）
*/
void onApplyMatrix();
void onApplyEyeInHandTransform();
void onEyeInHandTransform();
/**
* @brief 重置矩阵为单位矩阵
*/
void onResetMatrix();
void onGeneratePointCloud();
void onRotateCloudByZ();
void onSavePointCloud();
void onSavePointCloud(); // 保存 txt 点云
void onSavePlyPcdCloud(); // 保存 ply/pcd 点云
void onConvertEulerMatrix();
/**
* @brief 姿态补偿计算
*/
void onPoseCompensation();
private:
/**
* @brief 初始化界面
@ -214,11 +215,6 @@ private:
*/
QWidget* createLinePage();
/**
* @brief 创建矩阵变换页面
*/
QWidget* createTransformPage();
/**
* @brief 创建选线拟合组
*/
@ -354,19 +350,6 @@ private:
QTableWidget* m_linePointsTable;
QVector<QVector3D> m_currentLinePoints; // 当前线的原始点坐标
// 矩阵变换控件
QTextEdit* m_matrixEdit;
QPushButton* m_btnLoadMatrix;
QPushButton* m_btnApplyMatrix;
QLineEdit* m_editRobotX;
QLineEdit* m_editRobotY;
QLineEdit* m_editRobotZ;
QLineEdit* m_editRobotRx;
QLineEdit* m_editRobotRy;
QLineEdit* m_editRobotRz;
QPushButton* m_btnApplyEyeInHand;
QPushButton* m_btnResetMatrix;
// 悬浮缩放按钮
QToolButton* m_btnZoomIn;
QToolButton* m_btnZoomOut;

16
CloudView/Inc/PointCloudConverter.h
View File

@ -91,6 +91,22 @@ public:
*/
int saveToTxt(const std::string& fileName, const PointCloudXYZ& cloud, int lineNum, int linePtNum);
/**
* @brief 保存点云到 PLY 文件（ASCII 格式）
* @param fileName 输出文件路径
* @param cloud 点云数据
* @return 0=成功, -1=失败
*/
int saveToPly(const std::string& fileName, const PointCloudXYZ& cloud);
/**
* @brief 保存点云到 PCD 文件（ASCII 格式，兼容 PCL）
* @param fileName 输出文件路径
* @param cloud 点云数据
* @return 0=成功, -1=失败
*/
int saveToPcd(const std::string& fileName, const PointCloudXYZ& cloud);
/**
* @brief 旋转点云（行列转置 + 交换XY）
* @param cloud 输入点云

1162
CloudView/Src/CloudViewMainWindow.cpp
View File

File diff suppressed because it is too large Load Diff

116
CloudView/Src/PointCloudConverter.cpp
View File

@ -4,6 +4,7 @@
#include "VrLog.h"
#include <fstream>
#include <iomanip>
#include <sstream>
#include <algorithm>
#include <cctype>
@ -517,6 +518,121 @@ int PointCloudConverter::saveToTxt(const std::string& fileName, const PointCloud
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;

15
CloudView/Src/PointCloudGLWidget.cpp
View File

@ -119,7 +119,9 @@ void PointCloudGLWidget::resizeGL(int w, int h)
m_projection.setToIdentity();
float aspect = static_cast<float>(w) / static_cast<float>(h > 0 ? h : 1);
m_projection.perspective(45.0f, aspect, 0.1f, 10000.0f);
// 放大裁剪范围以容纳激光雷达点云（最大可能跨越数十万 mm）
// near 取 1.0 保留 depth buffer 精度，far 放到 1e7 避免远点被裁掉
m_projection.perspective(45.0f, aspect, 1.0f, 1.0e7f);
}
void PointCloudGLWidget::paintGL()
@ -474,8 +476,9 @@ void PointCloudGLWidget::resetView()
QVector3D size = m_maxBound - m_minBound;
float maxSize = qMax(qMax(size.x(), size.y()), size.z());
// 确保最小视距
m_distance = qMax(maxSize * 2.0f, 10.0f);
// 视距至少为 maxSize 的 2.5 倍，并确保不小于 10
// 上限放宽至 1e7，适配激光雷达大范围点云
m_distance = qBound(10.0f, maxSize * 2.5f, 1.0e7f);
// 默认视角：面向 XY 平面，X 向右，Y 向下
// 绕 X 轴旋转 180°：Y 轴从向上翻转为向下
m_rotationX = 180.0f;
@ -494,14 +497,14 @@ void PointCloudGLWidget::resetView()
void PointCloudGLWidget::zoomIn()
{
m_distance *= 0.9f;
m_distance = qBound(0.1f, m_distance, 10000.0f);
m_distance = qBound(0.1f, m_distance, 1.0e7f);
update();
}
void PointCloudGLWidget::zoomOut()
{
m_distance *= 1.1f;
m_distance = qBound(0.1f, m_distance, 10000.0f);
m_distance = qBound(0.1f, m_distance, 1.0e7f);
update();
}
@ -1419,7 +1422,7 @@ void PointCloudGLWidget::wheelEvent(QWheelEvent* event)
{
float delta = event->angleDelta().y() / 120.0f;
m_distance *= (1.0f - delta * 0.1f);
m_distance = qBound(0.1f, m_distance, 10000.0f);
m_distance = qBound(0.1f, m_distance, 1.0e7f);
update();
}

2
CloudView/main.cpp
View File

@ -3,7 +3,7 @@
#include <QIcon>
#include "CloudViewMainWindow.h"
#define APP_VERSION "1.1.4"
#define APP_VERSION "1.1.5"
int main(int argc, char* argv[])
{

2328
VrCommon/Inc/VZNL_Types.h Normal file
View File

File diff suppressed because it is too large Load Diff

40
VrCommon/Inc/VrError.h
View File

@ -88,11 +88,41 @@ enum ErrorCode
DEV_UNSUPPORT, //1510
DEV_ARG_INVAILD,
DEV_BUSY,
DEV_CONFIG_ERR,
DEV_DATA_INVALID,
DEV_RESULT_EMPTY, //1515
DEV_UNACTIVATE, //未激活
};
DEV_CONFIG_ERR,
DEV_DATA_INVALID,
DEV_RESULT_EMPTY, //1515
DEV_UNACTIVATE, //未激活
// DroneScrew 业务错误
DRONESCREW_ERR_BASE = 1600,
DRONESCREW_ERR_INVALID_JSON,
DRONESCREW_ERR_UNKNOWN_CMD,
DRONESCREW_ERR_SINGLE_HANDLER_NOT_SET,
DRONESCREW_ERR_MACHINE_CODE_MISMATCH,
DRONESCREW_ERR_CAMERA_NOT_CONNECTED,
DRONESCREW_ERR_BINOCULAR_FRAME_TIMEOUT,
DRONESCREW_ERR_ALGO_NOT_INIT,
DRONESCREW_ERR_CACHED_FRAME_NOT_READY,
DRONESCREW_ERR_LEFT_ACQ_NOT_STARTED,
DRONESCREW_ERR_RIGHT_ACQ_NOT_STARTED,
DRONESCREW_ERR_GPIO_TRIGGER,
DRONESCREW_ERR_SINGLE_BUSY,
DRONESCREW_ERR_MODE_CONFLICT,
DRONESCREW_ERR_DETECT_CONFIG_LEFT,
DRONESCREW_ERR_DETECT_CONFIG_RIGHT,
DRONESCREW_ERR_DETECT_START_LEFT,
DRONESCREW_ERR_DETECT_START_RIGHT,
DRONESCREW_ERR_LIVE_CONFIG_LEFT,
DRONESCREW_ERR_LIVE_START_LEFT,
DRONESCREW_ERR_RTSP_INIT,
DRONESCREW_ERR_RTSP_START,
DRONESCREW_ERR_ZMQ_HANDLER_NOT_SET,
DRONESCREW_ERR_UDP_DISCOVERY,
DRONESCREW_ERR_RESPONSE_NOT_OK,
DRONESCREW_ERR_GPIO_EXPORT,
DRONESCREW_ERR_GPIO_DIRECTION,
DRONESCREW_ERR_GPIO_VALUE,
};
#define ERR_CODE(nCode) -std::abs((int)nCode)
#define ERR_CODE_RETURN(nCode) do{ if(SUCCESS != (nCode)) return ERR_CODE(nCode); }while(0)

1
VrUtils/VrUtils.pro
View File

@ -100,6 +100,7 @@ SOURCES += \
Src/VrFileUtils.cpp \
Src/VrLog.cpp \
Src/VrNetUtils.cpp \
Src/VrStringUtils.cpp \
Src/VrTimeUtils.cpp \
ini/ConvertUTF.c \
log4cpp/src/AbortAppender.cpp \