Compare commits
6 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| a6e2e3280a | |||
| c2b525d948 | |||
| cd97cb5d56 | |||
| 04a5ec269f | |||
| 03c7cb58da | |||
| b1871aa9e7 |
@@ -1,5 +1,5 @@
|
|||||||
cmake_minimum_required(VERSION 3.15)
|
cmake_minimum_required(VERSION 3.15)
|
||||||
project(D330Viewer VERSION 1.0.0 LANGUAGES CXX C)
|
project(Viewer VERSION 1.0.0 LANGUAGES CXX C)
|
||||||
|
|
||||||
# 设置C++标准
|
# 设置C++标准
|
||||||
set(CMAKE_CXX_STANDARD 17)
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
@@ -34,7 +34,7 @@ find_package(Qt6 REQUIRED COMPONENTS
|
|||||||
)
|
)
|
||||||
|
|
||||||
# 查找PCL
|
# 查找PCL
|
||||||
find_package(PCL REQUIRED COMPONENTS common io visualization)
|
find_package(PCL REQUIRED COMPONENTS common io visualization filters)
|
||||||
if(PCL_FOUND)
|
if(PCL_FOUND)
|
||||||
include_directories(${PCL_INCLUDE_DIRS})
|
include_directories(${PCL_INCLUDE_DIRS})
|
||||||
link_directories(${PCL_LIBRARY_DIRS})
|
link_directories(${PCL_LIBRARY_DIRS})
|
||||||
@@ -101,6 +101,52 @@ add_executable(${PROJECT_NAME} WIN32
|
|||||||
${RESOURCES}
|
${RESOURCES}
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# ==================== 标定文件(cmos0)检查 ====================
|
||||||
|
set(VIEWER_CALIBRATION_DIR "${CMAKE_SOURCE_DIR}/cmos0")
|
||||||
|
set(VIEWER_REQUIRED_CALIB_FILES
|
||||||
|
"coe.txt"
|
||||||
|
"kc.txt"
|
||||||
|
"KK.txt"
|
||||||
|
)
|
||||||
|
|
||||||
|
set(VIEWER_MISSING_CALIB_FILES "")
|
||||||
|
foreach(_calib_file IN LISTS VIEWER_REQUIRED_CALIB_FILES)
|
||||||
|
if(NOT EXISTS "${VIEWER_CALIBRATION_DIR}/${_calib_file}")
|
||||||
|
list(APPEND VIEWER_MISSING_CALIB_FILES "${_calib_file}")
|
||||||
|
endif()
|
||||||
|
endforeach()
|
||||||
|
|
||||||
|
option(VIEWER_REQUIRE_CALIB_FILES "Fail configure when required cmos0 calibration files are missing" ON)
|
||||||
|
|
||||||
|
if(VIEWER_MISSING_CALIB_FILES)
|
||||||
|
if(VIEWER_REQUIRE_CALIB_FILES)
|
||||||
|
message(FATAL_ERROR
|
||||||
|
"Missing calibration file(s) in ${VIEWER_CALIBRATION_DIR}: ${VIEWER_MISSING_CALIB_FILES}\n"
|
||||||
|
"Please ensure cmos0 contains: ${VIEWER_REQUIRED_CALIB_FILES}"
|
||||||
|
)
|
||||||
|
else()
|
||||||
|
message(WARNING
|
||||||
|
"Missing calibration file(s) in ${VIEWER_CALIBRATION_DIR}: ${VIEWER_MISSING_CALIB_FILES}\n"
|
||||||
|
"Build continues, but runtime or MSI may be incomplete."
|
||||||
|
)
|
||||||
|
endif()
|
||||||
|
else()
|
||||||
|
message(STATUS "Calibration files found: ${VIEWER_REQUIRED_CALIB_FILES}")
|
||||||
|
endif()
|
||||||
|
|
||||||
|
# 复制标定文件到运行目录(bin/cmos0)
|
||||||
|
if(EXISTS "${VIEWER_CALIBRATION_DIR}" AND NOT VIEWER_MISSING_CALIB_FILES)
|
||||||
|
add_custom_command(TARGET ${PROJECT_NAME} POST_BUILD
|
||||||
|
COMMAND ${CMAKE_COMMAND} -E make_directory "$<TARGET_FILE_DIR:${PROJECT_NAME}>/cmos0"
|
||||||
|
COMMAND ${CMAKE_COMMAND} -E copy_directory
|
||||||
|
"${VIEWER_CALIBRATION_DIR}"
|
||||||
|
"$<TARGET_FILE_DIR:${PROJECT_NAME}>/cmos0"
|
||||||
|
COMMENT "Copy cmos0 calibration files to runtime directory"
|
||||||
|
)
|
||||||
|
else()
|
||||||
|
message(WARNING "Skip copying cmos0 because required calibration files are missing.")
|
||||||
|
endif()
|
||||||
|
|
||||||
# 链接库
|
# 链接库
|
||||||
target_link_libraries(${PROJECT_NAME}
|
target_link_libraries(${PROJECT_NAME}
|
||||||
Qt6::Core
|
Qt6::Core
|
||||||
@@ -141,25 +187,34 @@ install(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/platforms/
|
|||||||
FILES_MATCHING PATTERN "*.dll"
|
FILES_MATCHING PATTERN "*.dll"
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# 安装标定文件目录(用于MSI)
|
||||||
|
if(EXISTS "${VIEWER_CALIBRATION_DIR}" AND NOT VIEWER_MISSING_CALIB_FILES)
|
||||||
|
install(DIRECTORY ${VIEWER_CALIBRATION_DIR}/
|
||||||
|
DESTINATION cmos0
|
||||||
|
FILES_MATCHING
|
||||||
|
PATTERN "*.txt"
|
||||||
|
)
|
||||||
|
endif()
|
||||||
|
|
||||||
# ==================== CPack配置 - MSI安装程序 ====================
|
# ==================== CPack配置 - MSI安装程序 ====================
|
||||||
set(CPACK_PACKAGE_NAME "D330Viewer")
|
set(CPACK_PACKAGE_NAME "Viewer")
|
||||||
set(CPACK_PACKAGE_VENDOR "Lorenzo Zhao")
|
set(CPACK_PACKAGE_VENDOR "Lorenzo Zhao")
|
||||||
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "D330M Depth Camera Control System")
|
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Depth Camera Control System")
|
||||||
set(CPACK_PACKAGE_VERSION "0.2.0")
|
set(CPACK_PACKAGE_VERSION "0.3.3")
|
||||||
set(CPACK_PACKAGE_VERSION_MAJOR "0")
|
set(CPACK_PACKAGE_VERSION_MAJOR "0")
|
||||||
set(CPACK_PACKAGE_VERSION_MINOR "2")
|
set(CPACK_PACKAGE_VERSION_MINOR "3")
|
||||||
set(CPACK_PACKAGE_VERSION_PATCH "0")
|
set(CPACK_PACKAGE_VERSION_PATCH "3")
|
||||||
set(CPACK_PACKAGE_INSTALL_DIRECTORY "D330Viewer")
|
set(CPACK_PACKAGE_INSTALL_DIRECTORY "Viewer")
|
||||||
|
|
||||||
# WiX生成器配置(用于MSI)
|
# WiX生成器配置(用于MSI)
|
||||||
set(CPACK_GENERATOR "WIX")
|
set(CPACK_GENERATOR "WIX")
|
||||||
set(CPACK_WIX_UPGRADE_GUID "42365CB0-5840-487F-A2C8-56F9699A9022")
|
set(CPACK_WIX_UPGRADE_GUID "42365CB0-5840-487F-A2C8-56F9699A9022")
|
||||||
set(CPACK_WIX_PROGRAM_MENU_FOLDER "D330Viewer")
|
set(CPACK_WIX_PROGRAM_MENU_FOLDER "Viewer")
|
||||||
set(CPACK_WIX_LICENSE_RTF "${CMAKE_SOURCE_DIR}/LICENSE.rtf")
|
set(CPACK_WIX_LICENSE_RTF "${CMAKE_SOURCE_DIR}/LICENSE.rtf")
|
||||||
|
|
||||||
# 创建开始菜单和桌面快捷方式
|
# 创建开始菜单和桌面快捷方式
|
||||||
set(CPACK_PACKAGE_EXECUTABLES "D330Viewer" "D330Viewer")
|
set(CPACK_PACKAGE_EXECUTABLES "Viewer" "Viewer")
|
||||||
set(CPACK_CREATE_DESKTOP_LINKS "D330Viewer")
|
set(CPACK_CREATE_DESKTOP_LINKS "Viewer")
|
||||||
|
|
||||||
# 包含CPack模块
|
# 包含CPack模块
|
||||||
include(CPack)
|
include(CPack)
|
||||||
|
|||||||
52
README.md
52
README.md
@@ -149,6 +149,7 @@ C:\Program Files\D330Viewer\
|
|||||||
- ✅ 自动设备扫描和发现
|
- ✅ 自动设备扫描和发现
|
||||||
- ✅ 相机连接管理(连接/断开)
|
- ✅ 相机连接管理(连接/断开)
|
||||||
- ✅ 命令发送(START/STOP)
|
- ✅ 命令发送(START/STOP)
|
||||||
|
- ✅ 下位机动态切换IP,可随时切换上位机
|
||||||
|
|
||||||
#### 可视化
|
#### 可视化
|
||||||
- ✅ 实时左红外、右红外相机图像显示
|
- ✅ 实时左红外、右红外相机图像显示
|
||||||
@@ -167,19 +168,64 @@ C:\Program Files\D330Viewer\
|
|||||||
- ✅ 网络配置(IP地址、端口设置)
|
- ✅ 网络配置(IP地址、端口设置)
|
||||||
- ✅ 连接状态指示
|
- ✅ 连接状态指示
|
||||||
- ✅ 配置持久化(QSettings)
|
- ✅ 配置持久化(QSettings)
|
||||||
|
- ✅ 点云颜色映射(深度着色)
|
||||||
|
- ✅ 多视角预设(正视、侧视、俯视)
|
||||||
|
|
||||||
### 🚧 当前开发计划
|
### 🚧 当前开发计划
|
||||||
|
|
||||||
根据需求文档和用户反馈,后续待添加功能如下:
|
根据需求文档和用户反馈,后续待添加功能如下:
|
||||||
|
|
||||||
- 录制功能(连续保存多帧)
|
- 录制功能(连续保存多帧)
|
||||||
- 点云颜色映射(深度着色)
|
|
||||||
- 点云滤波选项(降噪、平滑)
|
- 点云滤波选项(降噪、平滑)
|
||||||
- 测量工具(距离、角度测量)
|
- 测量工具(距离、角度测量)
|
||||||
- 多视角预设(正视、侧视、俯视)
|
|
||||||
- 性能监控(CPU/GPU使用率、内存使用)
|
- 性能监控(CPU/GPU使用率、内存使用)
|
||||||
- 其他相机参数调节(增益、白平衡等)
|
- 其他相机参数调节(增益、白平衡等)
|
||||||
|
|
||||||
|
## 点云去噪原理与参数说明
|
||||||
|
|
||||||
|
### 去噪处理流程(当前版本)
|
||||||
|
|
||||||
|
当前点云去噪不是单一滤波器,而是多阶段组合策略,目标是在保留主体结构的同时抑制放射状无效点和外围杂点。
|
||||||
|
|
||||||
|
1. 有效点预筛:去掉非有限值和 `z<=0` 的点,得到基础有效掩码。
|
||||||
|
2. 中心ROI深度门控:基于中心区域中位深度自适应裁剪深度窗口,先去掉明显离群深度。
|
||||||
|
3. 邻域一致性筛选:统计每个点在局部窗口内“深度相近邻居”的数量,邻域支持不足的点剔除。
|
||||||
|
4. 形态学轻清理:移除局部孤立残点,减少毛刺。
|
||||||
|
5. 近距离尾部裁剪:对低深度尾部进行比例裁剪,抑制中心放射状噪点。
|
||||||
|
6. 连通簇筛选:按面积、深度一致性和中心重叠等条件保留主簇及相关簇,抑制周边散簇。
|
||||||
|
7. 最终细枝清理:对近距离且邻居不足的细枝点做额外抑制。
|
||||||
|
8. 时序稳定:对关键阈值做帧间平滑和限跳,减少块状点云“时有时无”的闪烁。
|
||||||
|
|
||||||
|
### 三个参数的作用与范围
|
||||||
|
|
||||||
|
参数都在“曝光与拍照 -> 拍照参数 -> 点云去噪参数”中,实时生效。
|
||||||
|
|
||||||
|
1. 邻域支持阈值
|
||||||
|
- 范围:`3 ~ 12`
|
||||||
|
- 含义:一个点要保留,局部邻域内至少需要多少个深度相近邻居。
|
||||||
|
- 调大:噪点更少,但边缘和细小结构更容易被吃掉。
|
||||||
|
- 调小:细节更多,但散点噪声会增加。
|
||||||
|
|
||||||
|
2. 射线裁剪强度 (‰)
|
||||||
|
- 范围:`5 ~ 50`
|
||||||
|
- 含义:近距离低深度尾部的裁剪比例(千分比)。
|
||||||
|
- 调大:中心放射状噪点减少更明显,但近距离真实细节可能减少。
|
||||||
|
- 调小:近距离细节保留更多,但放射状点可能增多。
|
||||||
|
|
||||||
|
3. 周边抑制带宽 (‰)
|
||||||
|
- 范围:`40 ~ 180`
|
||||||
|
- 含义:控制连通簇保留深度带宽、回补范围和近距离毛刺门限。
|
||||||
|
- 调小:抑制更激进,周边杂点更少,但主体可能偏“硬”、易丢块。
|
||||||
|
- 调大:主体与细节更完整,但外围杂点回升概率更高。
|
||||||
|
|
||||||
|
### 推荐起始参数
|
||||||
|
|
||||||
|
用于室内桌椅等常见场景,可先从以下值起步,再按效果微调:
|
||||||
|
|
||||||
|
- 邻域支持阈值:`8 ~ 10`
|
||||||
|
- 射线裁剪强度:`12 ~ 18`
|
||||||
|
- 周边抑制带宽:`90 ~ 130`
|
||||||
|
|
||||||
## 项目结构
|
## 项目结构
|
||||||
|
|
||||||
```
|
```
|
||||||
@@ -283,4 +329,4 @@ d330viewer/
|
|||||||
- 使用Qt6信号槽机制进行模块间通信
|
- 使用Qt6信号槽机制进行模块间通信
|
||||||
- OpenCL kernel代码内联在C++源文件中
|
- OpenCL kernel代码内联在C++源文件中
|
||||||
- 配置使用QSettings持久化
|
- 配置使用QSettings持久化
|
||||||
- 日志输出到 `bin/d330viewer.log`
|
- 日志输出到 `%LOCALAPPDATA%/Viewer/Viewer/viewer.log`(例如 `C:/Users/<用户名>/AppData/Local/Viewer/Viewer/viewer.log`)
|
||||||
|
|||||||
3
cmos0/KK.txt
Normal file
3
cmos0/KK.txt
Normal file
@@ -0,0 +1,3 @@
|
|||||||
|
1.4328957e+03 0.0000000e+00 6.3751170e+02
|
||||||
|
0.0000000e+00 1.4326590e+03 5.2187200e+02
|
||||||
|
0.0000000e+00 0.0000000e+00 1.0000000e+00
|
||||||
1224
cmos0/coe.txt
Normal file
1224
cmos0/coe.txt
Normal file
File diff suppressed because it is too large
Load Diff
5
cmos0/kc.txt
Normal file
5
cmos0/kc.txt
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
-1.2009005e-01
|
||||||
|
1.1928703e-01
|
||||||
|
9.6197371e-05
|
||||||
|
-1.4896083e-04
|
||||||
|
0.0000000e+00
|
||||||
@@ -4,7 +4,7 @@ REM CMake配置脚本 - Windows版本
|
|||||||
REM 请根据实际安装路径修改以下变量
|
REM 请根据实际安装路径修改以下变量
|
||||||
|
|
||||||
echo ========================================
|
echo ========================================
|
||||||
echo D330Viewer CMake配置脚本
|
echo Viewer CMake配置脚本
|
||||||
echo ========================================
|
echo ========================================
|
||||||
echo.
|
echo.
|
||||||
|
|
||||||
@@ -47,7 +47,7 @@ if %ERRORLEVEL% EQU 0 (
|
|||||||
echo ========================================
|
echo ========================================
|
||||||
echo.
|
echo.
|
||||||
echo 下一步:
|
echo 下一步:
|
||||||
echo 1. 打开 build\D330Viewer.sln 使用Visual Studio编译
|
echo 1. 打开 build\Viewer.sln 使用Visual Studio编译
|
||||||
echo 2. 或运行: cmake --build build --config Release
|
echo 2. 或运行: cmake --build build --config Release
|
||||||
echo.
|
echo.
|
||||||
) else (
|
) else (
|
||||||
|
|||||||
@@ -22,11 +22,15 @@
|
|||||||
#define PIXEL_FORMAT_MONO16 0x01100005 // Mono16 format (legacy)
|
#define PIXEL_FORMAT_MONO16 0x01100005 // Mono16 format (legacy)
|
||||||
#define PIXEL_FORMAT_MONO16_LEFT 0x01100006 // Mono16 format for left IR camera
|
#define PIXEL_FORMAT_MONO16_LEFT 0x01100006 // Mono16 format for left IR camera
|
||||||
#define PIXEL_FORMAT_MONO16_RIGHT 0x01100007 // Mono16 format for right IR camera
|
#define PIXEL_FORMAT_MONO16_RIGHT 0x01100007 // Mono16 format for right IR camera
|
||||||
|
#define PIXEL_FORMAT_MONO8_LEFT 0x01080006 // Mono8 format for left IR camera (downsampled)
|
||||||
|
#define PIXEL_FORMAT_MONO8_RIGHT 0x01080007 // Mono8 format for right IR camera (downsampled)
|
||||||
#define PIXEL_FORMAT_MJPEG 0x02180001 // MJPEG format for RGB camera
|
#define PIXEL_FORMAT_MJPEG 0x02180001 // MJPEG format for RGB camera
|
||||||
|
|
||||||
// Image dimensions
|
// Image dimensions
|
||||||
#define IMAGE_WIDTH 1224
|
#define IMAGE_WIDTH 1224
|
||||||
#define IMAGE_HEIGHT 1024
|
#define IMAGE_HEIGHT 1024
|
||||||
|
#define IR_DISPLAY_WIDTH 612 // Downsampled IR display width
|
||||||
|
#define IR_DISPLAY_HEIGHT 512 // Downsampled IR display height
|
||||||
#define RGB_WIDTH 1920
|
#define RGB_WIDTH 1920
|
||||||
#define RGB_HEIGHT 1080
|
#define RGB_HEIGHT 1080
|
||||||
|
|
||||||
|
|||||||
@@ -1,8 +1,10 @@
|
|||||||
#ifndef POINTCLOUDPROCESSOR_H
|
#ifndef POINTCLOUDPROCESSOR_H
|
||||||
#define POINTCLOUDPROCESSOR_H
|
#define POINTCLOUDPROCESSOR_H
|
||||||
|
|
||||||
#include <QObject>
|
#include <QObject>
|
||||||
#include <QByteArray>
|
#include <QByteArray>
|
||||||
|
#include <atomic>
|
||||||
|
#include <mutex>
|
||||||
#include <pcl/point_cloud.h>
|
#include <pcl/point_cloud.h>
|
||||||
#include <pcl/point_types.h>
|
#include <pcl/point_types.h>
|
||||||
#include <CL/cl.h>
|
#include <CL/cl.h>
|
||||||
@@ -15,44 +17,38 @@ public:
|
|||||||
explicit PointCloudProcessor(QObject *parent = nullptr);
|
explicit PointCloudProcessor(QObject *parent = nullptr);
|
||||||
~PointCloudProcessor();
|
~PointCloudProcessor();
|
||||||
|
|
||||||
// 初始化OpenCL
|
|
||||||
bool initializeOpenCL();
|
bool initializeOpenCL();
|
||||||
|
|
||||||
// 设置相机内参
|
|
||||||
void setCameraIntrinsics(float fx, float fy, float cx, float cy);
|
void setCameraIntrinsics(float fx, float fy, float cx, float cy);
|
||||||
|
|
||||||
// 设置Z缩放因子
|
|
||||||
void setZScaleFactor(float scale);
|
void setZScaleFactor(float scale);
|
||||||
|
|
||||||
// 将深度数据转换为点云(使用OpenCL GPU加速)
|
|
||||||
void processDepthData(const QByteArray &depthData, uint32_t blockId);
|
void processDepthData(const QByteArray &depthData, uint32_t blockId);
|
||||||
|
|
||||||
// 处理已经计算好的点云数据(x,y,z格式)
|
|
||||||
void processPointCloudData(const QByteArray &cloudData, uint32_t blockId);
|
void processPointCloudData(const QByteArray &cloudData, uint32_t blockId);
|
||||||
|
|
||||||
|
void setDenoiseEnabled(bool enabled);
|
||||||
|
void setDenoiseNeighborSupport(int minNeighbors);
|
||||||
|
void setDenoiseLowTailPermille(int permille);
|
||||||
|
void setDenoiseDepthBandPermille(int permille);
|
||||||
|
|
||||||
signals:
|
signals:
|
||||||
void pointCloudReady(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, uint32_t blockId);
|
void pointCloudReady(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, uint32_t blockId);
|
||||||
void errorOccurred(const QString &error);
|
void errorOccurred(const QString &error);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// 清理OpenCL资源
|
pcl::PointCloud<pcl::PointXYZ>::Ptr applyDenoise(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input);
|
||||||
|
void loadLowerCalibration();
|
||||||
void cleanupOpenCL();
|
void cleanupOpenCL();
|
||||||
|
|
||||||
// 相机内参
|
|
||||||
float m_fx;
|
float m_fx;
|
||||||
float m_fy;
|
float m_fy;
|
||||||
float m_cx;
|
float m_cx;
|
||||||
float m_cy;
|
float m_cy;
|
||||||
|
|
||||||
// Z缩放因子
|
|
||||||
float m_zScale;
|
float m_zScale;
|
||||||
|
|
||||||
// 图像尺寸
|
|
||||||
int m_imageWidth;
|
int m_imageWidth;
|
||||||
int m_imageHeight;
|
int m_imageHeight;
|
||||||
int m_totalPoints;
|
int m_totalPoints;
|
||||||
|
|
||||||
// OpenCL资源
|
|
||||||
cl_platform_id m_platform;
|
cl_platform_id m_platform;
|
||||||
cl_device_id m_device;
|
cl_device_id m_device;
|
||||||
cl_context m_context;
|
cl_context m_context;
|
||||||
@@ -62,6 +58,30 @@ private:
|
|||||||
cl_mem m_depthBuffer;
|
cl_mem m_depthBuffer;
|
||||||
cl_mem m_xyzBuffer;
|
cl_mem m_xyzBuffer;
|
||||||
bool m_clInitialized;
|
bool m_clInitialized;
|
||||||
|
|
||||||
|
std::atomic_bool m_denoiseEnabled;
|
||||||
|
float m_voxelLeafSize;
|
||||||
|
std::atomic_int m_denoiseNeighborSupport;
|
||||||
|
std::atomic_int m_denoiseLowTailPermille;
|
||||||
|
std::atomic_int m_denoiseDepthBandPermille;
|
||||||
|
|
||||||
|
// Calibration params aligned with lower-machine model
|
||||||
|
float m_k1;
|
||||||
|
float m_k2;
|
||||||
|
float m_p1;
|
||||||
|
float m_p2;
|
||||||
|
float m_p5;
|
||||||
|
float m_p6;
|
||||||
|
float m_p7;
|
||||||
|
float m_p8;
|
||||||
|
bool m_hasLowerCalibration;
|
||||||
|
|
||||||
|
// Temporal stabilizers for denoise to reduce frame-to-frame flicker.
|
||||||
|
std::mutex m_denoiseStateMutex;
|
||||||
|
bool m_hasAnchorMeanZ;
|
||||||
|
float m_anchorMeanZFiltered;
|
||||||
|
bool m_hasLowCutZ;
|
||||||
|
float m_lowCutZFiltered;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // POINTCLOUDPROCESSOR_H
|
#endif // POINTCLOUDPROCESSOR_H
|
||||||
|
|||||||
@@ -23,6 +23,9 @@ public:
|
|||||||
~PointCloudGLWidget();
|
~PointCloudGLWidget();
|
||||||
|
|
||||||
void updatePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
|
void updatePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
|
||||||
|
void setColorMode(bool enabled) { m_colorMode = enabled ? 1 : 0; update(); }
|
||||||
|
bool colorMode() const { return m_colorMode != 0; }
|
||||||
|
void resetView(); // 重置视角到初始状态
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void initializeGL() override;
|
void initializeGL() override;
|
||||||
@@ -48,25 +51,31 @@ private:
|
|||||||
// 点云数据
|
// 点云数据
|
||||||
std::vector<float> m_vertices;
|
std::vector<float> m_vertices;
|
||||||
int m_pointCount;
|
int m_pointCount;
|
||||||
|
float m_minZ, m_maxZ; // 深度范围(用于着色)
|
||||||
// 固定的点云中心点(避免抖动)
|
|
||||||
QVector3D m_fixedCenter;
|
|
||||||
bool m_centerInitialized;
|
|
||||||
|
|
||||||
// 相机参数
|
// 相机参数
|
||||||
QMatrix4x4 m_projection;
|
QMatrix4x4 m_projection;
|
||||||
QMatrix4x4 m_view;
|
QMatrix4x4 m_view;
|
||||||
QMatrix4x4 m_model;
|
QMatrix4x4 m_model;
|
||||||
|
|
||||||
float m_orthoSize; // 正交投影视野大小(控制缩放)
|
float m_fov; // 透视投影视场角
|
||||||
float m_rotationX; // X轴旋转角度
|
float m_rotationX; // X轴旋转角度
|
||||||
float m_rotationY; // Y轴旋转角度
|
float m_rotationY; // Y轴旋转角度
|
||||||
QVector3D m_translation; // 平移
|
QVector3D m_cloudCenter; // 点云中心
|
||||||
|
float m_viewDistance; // 观察距离
|
||||||
|
QVector3D m_panOffset; // 用户平移偏移
|
||||||
|
float m_zoom; // 缩放因子
|
||||||
|
|
||||||
// 鼠标交互状态
|
// 鼠标交互状态
|
||||||
QPoint m_lastMousePos;
|
QPoint m_lastMousePos;
|
||||||
bool m_leftButtonPressed;
|
bool m_leftButtonPressed;
|
||||||
bool m_rightButtonPressed;
|
bool m_rightButtonPressed;
|
||||||
|
|
||||||
|
// 首帧标志(只在首帧时自动居中)
|
||||||
|
bool m_firstFrame;
|
||||||
|
|
||||||
|
// 颜色模式(0=黑白,1=彩色)
|
||||||
|
int m_colorMode;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // POINTCLOUDGLWIDGET_H
|
#endif // POINTCLOUDGLWIDGET_H
|
||||||
|
|||||||
@@ -19,6 +19,10 @@ public:
|
|||||||
// 更新点云显示
|
// 更新点云显示
|
||||||
void updatePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
|
void updatePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
|
||||||
|
|
||||||
|
// 颜色模式控制
|
||||||
|
void setColorMode(bool enabled);
|
||||||
|
bool colorMode() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
QLabel *m_statusLabel;
|
QLabel *m_statusLabel;
|
||||||
PointCloudGLWidget *m_glWidget;
|
PointCloudGLWidget *m_glWidget;
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
#include "config/ConfigManager.h"
|
#include "config/ConfigManager.h"
|
||||||
|
|
||||||
ConfigManager::ConfigManager()
|
ConfigManager::ConfigManager()
|
||||||
: m_settings(std::make_unique<QSettings>("D330Viewer", "D330Viewer"))
|
: m_settings(std::make_unique<QSettings>("Viewer", "Viewer"))
|
||||||
{
|
{
|
||||||
// 构造函数:初始化QSettings
|
// 构造函数:初始化QSettings
|
||||||
}
|
}
|
||||||
@@ -45,7 +45,7 @@ void ConfigManager::setDataPort(int port)
|
|||||||
// ========== 相机配置 ==========
|
// ========== 相机配置 ==========
|
||||||
int ConfigManager::getExposureTime() const
|
int ConfigManager::getExposureTime() const
|
||||||
{
|
{
|
||||||
return m_settings->value("Camera/ExposureTime", 10000).toInt();
|
return m_settings->value("Camera/ExposureTime", 5980).toInt();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConfigManager::setExposureTime(int exposure)
|
void ConfigManager::setExposureTime(int exposure)
|
||||||
|
|||||||
@@ -1,6 +1,8 @@
|
|||||||
#include "DeviceScanner.h"
|
#include "DeviceScanner.h"
|
||||||
#include <QNetworkDatagram>
|
#include <QNetworkDatagram>
|
||||||
#include <QNetworkInterface>
|
#include <QNetworkInterface>
|
||||||
|
#include <QCoreApplication>
|
||||||
|
#include <QThread>
|
||||||
#include <QDebug>
|
#include <QDebug>
|
||||||
|
|
||||||
DeviceScanner::DeviceScanner(QObject *parent)
|
DeviceScanner::DeviceScanner(QObject *parent)
|
||||||
@@ -8,8 +10,9 @@ DeviceScanner::DeviceScanner(QObject *parent)
|
|||||||
, m_socket(new QUdpSocket(this))
|
, m_socket(new QUdpSocket(this))
|
||||||
, m_timeoutTimer(new QTimer(this))
|
, m_timeoutTimer(new QTimer(this))
|
||||||
, m_scanTimer(new QTimer(this))
|
, m_scanTimer(new QTimer(this))
|
||||||
, m_currentHost(HOST_START)
|
, m_prefixLength(24)
|
||||||
, m_totalHosts(HOST_END - HOST_START + 1)
|
, m_currentHost(0)
|
||||||
|
, m_totalHosts(0)
|
||||||
, m_isScanning(false)
|
, m_isScanning(false)
|
||||||
{
|
{
|
||||||
connect(m_socket, &QUdpSocket::readyRead, this, &DeviceScanner::onReadyRead);
|
connect(m_socket, &QUdpSocket::readyRead, this, &DeviceScanner::onReadyRead);
|
||||||
@@ -33,8 +36,9 @@ void DeviceScanner::startScan(const QString &subnet)
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
m_subnet = subnet.isEmpty() ? getLocalSubnet() : subnet;
|
// Get network info (base IP and prefix length)
|
||||||
m_currentHost = HOST_START;
|
getLocalNetworkInfo(m_baseIp, m_prefixLength);
|
||||||
|
|
||||||
m_foundDevices.clear();
|
m_foundDevices.clear();
|
||||||
m_isScanning = true;
|
m_isScanning = true;
|
||||||
|
|
||||||
@@ -44,17 +48,62 @@ void DeviceScanner::startScan(const QString &subnet)
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
qDebug() << "Starting fast device scan on subnet:" << m_subnet;
|
// Calculate IP range based on prefix length
|
||||||
|
QStringList parts = m_baseIp.split('.');
|
||||||
// Send DISCOVER to all hosts at once (batch mode)
|
if (parts.size() != 4) {
|
||||||
for (int host = HOST_START; host <= HOST_END; host++) {
|
emit scanError("Invalid base IP");
|
||||||
QString ip = m_subnet + "." + QString::number(host);
|
m_isScanning = false;
|
||||||
sendDiscoveryPacket(ip);
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Wait 3 seconds for all responses
|
quint32 baseAddr = (parts[0].toUInt() << 24) | (parts[1].toUInt() << 16) |
|
||||||
m_timeoutTimer->start(3000);
|
(parts[2].toUInt() << 8) | parts[3].toUInt();
|
||||||
qDebug() << "Sent discovery packets to all hosts, waiting for responses...";
|
quint32 mask = (0xFFFFFFFF << (32 - m_prefixLength)) & 0xFFFFFFFF;
|
||||||
|
quint32 networkAddr = baseAddr & mask;
|
||||||
|
quint32 broadcastAddr = networkAddr | (~mask & 0xFFFFFFFF);
|
||||||
|
|
||||||
|
qDebug() << "Starting device scan - Base IP:" << m_baseIp << "Prefix:" << m_prefixLength;
|
||||||
|
|
||||||
|
int packetsSent = 0;
|
||||||
|
|
||||||
|
// For large subnets (/16 or larger), use broadcast + batched unicast
|
||||||
|
if (m_prefixLength <= 16) {
|
||||||
|
// First: send to subnet broadcast address
|
||||||
|
QHostAddress broadcast(broadcastAddr);
|
||||||
|
qDebug() << "Large subnet detected, using broadcast discovery:" << broadcast.toString();
|
||||||
|
sendDiscoveryPacket(broadcast.toString());
|
||||||
|
packetsSent++;
|
||||||
|
|
||||||
|
// Second: scan all /24 subnets with throttling to avoid buffer overflow
|
||||||
|
qDebug() << "Scanning all /24 subnets within /16 range (throttled)...";
|
||||||
|
for (int oct3 = 0; oct3 <= 255; oct3++) {
|
||||||
|
quint32 subNetBase = (networkAddr & 0xFFFF0000) | (oct3 << 8);
|
||||||
|
for (int oct4 = 1; oct4 <= 254; oct4++) {
|
||||||
|
quint32 addr = subNetBase | oct4;
|
||||||
|
sendDiscoveryPacket(QHostAddress(addr).toString());
|
||||||
|
packetsSent++;
|
||||||
|
}
|
||||||
|
// Process events and add small delay every /24 subnet to avoid buffer overflow
|
||||||
|
QCoreApplication::processEvents();
|
||||||
|
QThread::msleep(5);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// For smaller subnets, scan all hosts
|
||||||
|
qDebug() << "Network range:" << QHostAddress(networkAddr + 1).toString()
|
||||||
|
<< "to" << QHostAddress(broadcastAddr - 1).toString();
|
||||||
|
|
||||||
|
for (quint32 addr = networkAddr + 1; addr < broadcastAddr; addr++) {
|
||||||
|
sendDiscoveryPacket(QHostAddress(addr).toString());
|
||||||
|
packetsSent++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
m_totalHosts = packetsSent;
|
||||||
|
qDebug() << "Sent" << packetsSent << "discovery packets, waiting for responses...";
|
||||||
|
|
||||||
|
// Adjust timeout based on network size
|
||||||
|
int timeout = (m_prefixLength >= 24) ? 3000 : 10000;
|
||||||
|
m_timeoutTimer->start(timeout);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DeviceScanner::stopScan()
|
void DeviceScanner::stopScan()
|
||||||
@@ -91,10 +140,17 @@ void DeviceScanner::onReadyRead()
|
|||||||
|
|
||||||
qDebug() << "Received response from" << senderIp << ":" << response;
|
qDebug() << "Received response from" << senderIp << ":" << response;
|
||||||
|
|
||||||
if (response.contains("D330M_CAMERA")) {
|
if (response.startsWith("EXPOSURE:")) {
|
||||||
|
bool ok;
|
||||||
|
int exposure = response.mid(9).trimmed().toInt(&ok);
|
||||||
|
if (ok && exposure > 0) {
|
||||||
|
qDebug() << "Received exposure from camera:" << exposure << "us";
|
||||||
|
emit exposureReceived(exposure);
|
||||||
|
}
|
||||||
|
} else if (response.contains("D330M_CAMERA")) {
|
||||||
DeviceInfo device;
|
DeviceInfo device;
|
||||||
device.ipAddress = senderIp;
|
device.ipAddress = senderIp;
|
||||||
device.deviceName = "D330M Camera";
|
device.deviceName = "Camera";
|
||||||
device.port = SCAN_PORT;
|
device.port = SCAN_PORT;
|
||||||
device.responseTime = 0;
|
device.responseTime = 0;
|
||||||
|
|
||||||
@@ -122,7 +178,7 @@ void DeviceScanner::sendDiscoveryPacket(const QString &ip)
|
|||||||
m_socket->writeDatagram(data, QHostAddress(ip), SCAN_PORT);
|
m_socket->writeDatagram(data, QHostAddress(ip), SCAN_PORT);
|
||||||
}
|
}
|
||||||
|
|
||||||
QString DeviceScanner::getLocalSubnet()
|
void DeviceScanner::getLocalNetworkInfo(QString &baseIp, int &prefixLength)
|
||||||
{
|
{
|
||||||
// Get all network interfaces
|
// Get all network interfaces
|
||||||
QList<QNetworkInterface> interfaces = QNetworkInterface::allInterfaces();
|
QList<QNetworkInterface> interfaces = QNetworkInterface::allInterfaces();
|
||||||
@@ -144,12 +200,14 @@ QString DeviceScanner::getLocalSubnet()
|
|||||||
for (const QNetworkAddressEntry &entry : entries) {
|
for (const QNetworkAddressEntry &entry : entries) {
|
||||||
QHostAddress addr = entry.ip();
|
QHostAddress addr = entry.ip();
|
||||||
if (addr.protocol() == QAbstractSocket::IPv4Protocol && !addr.isLoopback()) {
|
if (addr.protocol() == QAbstractSocket::IPv4Protocol && !addr.isLoopback()) {
|
||||||
QString ip = addr.toString();
|
baseIp = addr.toString();
|
||||||
QStringList parts = ip.split('.');
|
prefixLength = entry.prefixLength();
|
||||||
if (parts.size() == 4) {
|
if (prefixLength <= 0 || prefixLength > 32) {
|
||||||
qDebug() << "Found Ethernet adapter:" << iface.humanReadableName() << "IP:" << ip;
|
prefixLength = 24; // Default to /24 if invalid
|
||||||
return parts[0] + "." + parts[1] + "." + parts[2];
|
|
||||||
}
|
}
|
||||||
|
qDebug() << "Found Ethernet adapter:" << iface.humanReadableName()
|
||||||
|
<< "IP:" << baseIp << "Prefix:" << prefixLength;
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -169,15 +227,19 @@ QString DeviceScanner::getLocalSubnet()
|
|||||||
for (const QNetworkAddressEntry &entry : entries) {
|
for (const QNetworkAddressEntry &entry : entries) {
|
||||||
QHostAddress addr = entry.ip();
|
QHostAddress addr = entry.ip();
|
||||||
if (addr.protocol() == QAbstractSocket::IPv4Protocol && !addr.isLoopback()) {
|
if (addr.protocol() == QAbstractSocket::IPv4Protocol && !addr.isLoopback()) {
|
||||||
QString ip = addr.toString();
|
baseIp = addr.toString();
|
||||||
QStringList parts = ip.split('.');
|
prefixLength = entry.prefixLength();
|
||||||
if (parts.size() == 4) {
|
if (prefixLength <= 0 || prefixLength > 32) {
|
||||||
qDebug() << "Found adapter:" << iface.humanReadableName() << "IP:" << ip;
|
prefixLength = 24;
|
||||||
return parts[0] + "." + parts[1] + "." + parts[2];
|
|
||||||
}
|
}
|
||||||
|
qDebug() << "Found adapter:" << iface.humanReadableName()
|
||||||
|
<< "IP:" << baseIp << "Prefix:" << prefixLength;
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return "192.168.0";
|
// Default fallback
|
||||||
|
baseIp = "192.168.0.1";
|
||||||
|
prefixLength = 24;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -29,6 +29,7 @@ public:
|
|||||||
|
|
||||||
signals:
|
signals:
|
||||||
void deviceFound(const DeviceInfo &device);
|
void deviceFound(const DeviceInfo &device);
|
||||||
|
void exposureReceived(int exposureUs);
|
||||||
void scanProgress(int current, int total);
|
void scanProgress(int current, int total);
|
||||||
void scanFinished(int devicesFound);
|
void scanFinished(int devicesFound);
|
||||||
void scanError(const QString &error);
|
void scanError(const QString &error);
|
||||||
@@ -40,13 +41,14 @@ private slots:
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
void sendDiscoveryPacket(const QString &ip);
|
void sendDiscoveryPacket(const QString &ip);
|
||||||
QString getLocalSubnet();
|
void getLocalNetworkInfo(QString &baseIp, int &prefixLength);
|
||||||
|
|
||||||
QUdpSocket *m_socket;
|
QUdpSocket *m_socket;
|
||||||
QTimer *m_timeoutTimer;
|
QTimer *m_timeoutTimer;
|
||||||
QTimer *m_scanTimer;
|
QTimer *m_scanTimer;
|
||||||
|
|
||||||
QString m_subnet;
|
QString m_baseIp;
|
||||||
|
int m_prefixLength;
|
||||||
int m_currentHost;
|
int m_currentHost;
|
||||||
int m_totalHosts;
|
int m_totalHosts;
|
||||||
bool m_isScanning;
|
bool m_isScanning;
|
||||||
@@ -55,8 +57,6 @@ private:
|
|||||||
|
|
||||||
static constexpr int SCAN_PORT = 6790; // Control port for device discovery
|
static constexpr int SCAN_PORT = 6790; // Control port for device discovery
|
||||||
static constexpr int SCAN_TIMEOUT = 10;
|
static constexpr int SCAN_TIMEOUT = 10;
|
||||||
static constexpr int HOST_START = 1;
|
|
||||||
static constexpr int HOST_END = 254;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // DEVICESCANNER_H
|
#endif // DEVICESCANNER_H
|
||||||
|
|||||||
@@ -96,9 +96,9 @@ void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
|
|||||||
|
|
||||||
// 根据像素格式选择对应的状态
|
// 根据像素格式选择对应的状态
|
||||||
StreamState *state = nullptr;
|
StreamState *state = nullptr;
|
||||||
if (pixelFormat == PIXEL_FORMAT_MONO16_LEFT) {
|
if (pixelFormat == PIXEL_FORMAT_MONO16_LEFT || pixelFormat == PIXEL_FORMAT_MONO8_LEFT) {
|
||||||
state = &m_leftIRState;
|
state = &m_leftIRState;
|
||||||
} else if (pixelFormat == PIXEL_FORMAT_MONO16_RIGHT) {
|
} else if (pixelFormat == PIXEL_FORMAT_MONO16_RIGHT || pixelFormat == PIXEL_FORMAT_MONO8_RIGHT) {
|
||||||
state = &m_rightIRState;
|
state = &m_rightIRState;
|
||||||
} else if (pixelFormat == PIXEL_FORMAT_MJPEG) {
|
} else if (pixelFormat == PIXEL_FORMAT_MJPEG) {
|
||||||
state = &m_rgbState;
|
state = &m_rgbState;
|
||||||
@@ -118,6 +118,9 @@ void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
|
|||||||
if (pixelFormat == PIXEL_FORMAT_MJPEG) {
|
if (pixelFormat == PIXEL_FORMAT_MJPEG) {
|
||||||
// MJPEG是压缩格式,实际大小未知,设置为0表示动态接收
|
// MJPEG是压缩格式,实际大小未知,设置为0表示动态接收
|
||||||
state->expectedSize = 0;
|
state->expectedSize = 0;
|
||||||
|
} else if (pixelFormat == PIXEL_FORMAT_MONO8_LEFT || pixelFormat == PIXEL_FORMAT_MONO8_RIGHT) {
|
||||||
|
// 8-bit灰度格式(下采样)
|
||||||
|
state->expectedSize = imageWidth * imageHeight;
|
||||||
} else {
|
} else {
|
||||||
// 16-bit或12-bit灰度等固定格式
|
// 16-bit或12-bit灰度等固定格式
|
||||||
state->expectedSize = imageWidth * imageHeight * 2;
|
state->expectedSize = imageWidth * imageHeight * 2;
|
||||||
@@ -268,6 +271,29 @@ void GVSPParser::processImageData(GVSPParser::StreamState *state)
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 处理Mono8格式(左右红外相机下采样8位数据)
|
||||||
|
if (state->pixelFormat == PIXEL_FORMAT_MONO8_LEFT) {
|
||||||
|
// 检查数据大小
|
||||||
|
if (state->dataBuffer.size() < state->expectedSize) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
// 左红外8位数据
|
||||||
|
emit leftImageReceived(state->dataBuffer, state->blockId);
|
||||||
|
m_imageSequence++;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (state->pixelFormat == PIXEL_FORMAT_MONO8_RIGHT) {
|
||||||
|
// 检查数据大小
|
||||||
|
if (state->dataBuffer.size() < state->expectedSize) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
// 右红外8位数据
|
||||||
|
emit rightImageReceived(state->dataBuffer, state->blockId);
|
||||||
|
m_imageSequence++;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// 兼容旧版本:使用序号区分(legacy)
|
// 兼容旧版本:使用序号区分(legacy)
|
||||||
if (state->pixelFormat == PIXEL_FORMAT_MONO16) {
|
if (state->pixelFormat == PIXEL_FORMAT_MONO16) {
|
||||||
// 检查数据大小
|
// 检查数据大小
|
||||||
|
|||||||
@@ -16,6 +16,9 @@ NetworkManager::NetworkManager(QObject *parent)
|
|||||||
connect(m_dataSocket, &QUdpSocket::readyRead, this, &NetworkManager::onReadyRead);
|
connect(m_dataSocket, &QUdpSocket::readyRead, this, &NetworkManager::onReadyRead);
|
||||||
connect(m_dataSocket, &QUdpSocket::errorOccurred, this, &NetworkManager::onError);
|
connect(m_dataSocket, &QUdpSocket::errorOccurred, this, &NetworkManager::onError);
|
||||||
|
|
||||||
|
// 连接控制socket接收信号(用于接收相机回复,如曝光值)
|
||||||
|
connect(m_controlSocket, &QUdpSocket::readyRead, this, &NetworkManager::onControlReadyRead);
|
||||||
|
|
||||||
// 连接GVSP解析器信号
|
// 连接GVSP解析器信号
|
||||||
connect(m_gvspParser, &GVSPParser::imageReceived, this, &NetworkManager::imageReceived);
|
connect(m_gvspParser, &GVSPParser::imageReceived, this, &NetworkManager::imageReceived);
|
||||||
connect(m_gvspParser, &GVSPParser::leftImageReceived, this, &NetworkManager::leftImageReceived);
|
connect(m_gvspParser, &GVSPParser::leftImageReceived, this, &NetworkManager::leftImageReceived);
|
||||||
@@ -67,6 +70,10 @@ bool NetworkManager::connectToCamera(const QString &ip, int controlPort, int dat
|
|||||||
m_isConnected = true;
|
m_isConnected = true;
|
||||||
qDebug() << "Connected to camera:" << m_cameraIp << "Control port:" << m_controlPort << "Data port:" << m_dataPort;
|
qDebug() << "Connected to camera:" << m_cameraIp << "Control port:" << m_controlPort << "Data port:" << m_dataPort;
|
||||||
|
|
||||||
|
// Send DISCOVER to get camera's current exposure value
|
||||||
|
sendCommand("DISCOVER");
|
||||||
|
qDebug() << "Sent DISCOVER to fetch camera exposure";
|
||||||
|
|
||||||
// Send STOP command to register client IP on camera
|
// Send STOP command to register client IP on camera
|
||||||
sendStopCommand();
|
sendStopCommand();
|
||||||
qDebug() << "Sent STOP command to register client IP";
|
qDebug() << "Sent STOP command to register client IP";
|
||||||
@@ -134,23 +141,8 @@ bool NetworkManager::sendStopCommand()
|
|||||||
|
|
||||||
bool NetworkManager::sendExposureCommand(int exposureTime)
|
bool NetworkManager::sendExposureCommand(int exposureTime)
|
||||||
{
|
{
|
||||||
// 同时发送结构光曝光命令(UART控制激光器,单位μs)
|
|
||||||
QString exposureCommand = QString("EXPOSURE:%1").arg(exposureTime);
|
QString exposureCommand = QString("EXPOSURE:%1").arg(exposureTime);
|
||||||
bool success1 = sendCommand(exposureCommand);
|
return sendCommand(exposureCommand);
|
||||||
|
|
||||||
// 同时发送红外相机曝光命令(通过触发脉冲宽度控制,单位μs)
|
|
||||||
// 下位机会将此值用作manual_trigger_pulse()的脉冲宽度参数
|
|
||||||
// 脉冲宽度直接决定相机的实际曝光时间
|
|
||||||
int irExposure = exposureTime;
|
|
||||||
|
|
||||||
// 限制在有效范围内(1000μs ~ 100000μs,避免脉冲太短导致相机无法触发)
|
|
||||||
if (irExposure < 1000) irExposure = 1000;
|
|
||||||
if (irExposure > 100000) irExposure = 100000;
|
|
||||||
|
|
||||||
QString irExposureCommand = QString("IR_EXPOSURE:%1").arg(irExposure);
|
|
||||||
bool success2 = sendCommand(irExposureCommand);
|
|
||||||
|
|
||||||
return success1 && success2;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ========== 传输开关命令 ==========
|
// ========== 传输开关命令 ==========
|
||||||
@@ -228,6 +220,27 @@ void NetworkManager::onReadyRead()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void NetworkManager::onControlReadyRead()
|
||||||
|
{
|
||||||
|
while (m_controlSocket->hasPendingDatagrams()) {
|
||||||
|
QByteArray datagram;
|
||||||
|
datagram.resize(m_controlSocket->pendingDatagramSize());
|
||||||
|
m_controlSocket->readDatagram(datagram.data(), datagram.size());
|
||||||
|
|
||||||
|
QString response = QString::fromUtf8(datagram);
|
||||||
|
qDebug() << "[NetworkManager] Control response:" << response;
|
||||||
|
|
||||||
|
if (response.startsWith("EXPOSURE:")) {
|
||||||
|
bool ok;
|
||||||
|
int exposure = response.mid(9).trimmed().toInt(&ok);
|
||||||
|
if (ok && exposure > 0) {
|
||||||
|
qDebug() << "[NetworkManager] Camera exposure:" << exposure << "us";
|
||||||
|
emit exposureReceived(exposure);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void NetworkManager::onError(QAbstractSocket::SocketError socketError)
|
void NetworkManager::onError(QAbstractSocket::SocketError socketError)
|
||||||
{
|
{
|
||||||
QString error = QString("Socket error: %1").arg(m_dataSocket->errorString());
|
QString error = QString("Socket error: %1").arg(m_dataSocket->errorString());
|
||||||
|
|||||||
@@ -43,6 +43,7 @@ public:
|
|||||||
signals:
|
signals:
|
||||||
void connected();
|
void connected();
|
||||||
void disconnected();
|
void disconnected();
|
||||||
|
void exposureReceived(int exposureUs);
|
||||||
void errorOccurred(const QString &error);
|
void errorOccurred(const QString &error);
|
||||||
void dataReceived(const QByteArray &data);
|
void dataReceived(const QByteArray &data);
|
||||||
void imageReceived(const QImage &image, uint32_t blockId);
|
void imageReceived(const QImage &image, uint32_t blockId);
|
||||||
@@ -54,6 +55,7 @@ signals:
|
|||||||
|
|
||||||
private slots:
|
private slots:
|
||||||
void onReadyRead();
|
void onReadyRead();
|
||||||
|
void onControlReadyRead();
|
||||||
void onError(QAbstractSocket::SocketError socketError);
|
void onError(QAbstractSocket::SocketError socketError);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|||||||
@@ -1,12 +1,93 @@
|
|||||||
#include "core/PointCloudProcessor.h"
|
#include "core/PointCloudProcessor.h"
|
||||||
#include <QDebug>
|
#include <QDebug>
|
||||||
|
#include <QCoreApplication>
|
||||||
|
#include <QDir>
|
||||||
|
#include <QFile>
|
||||||
|
#include <QRegularExpression>
|
||||||
|
#include <QStringList>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <algorithm>
|
||||||
|
#include <unordered_map>
|
||||||
|
#include <pcl/common/point_tests.h>
|
||||||
|
|
||||||
#ifndef M_PI
|
#ifndef M_PI
|
||||||
#define M_PI 3.14159265358979323846
|
#define M_PI 3.14159265358979323846
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
struct VoxelKey {
|
||||||
|
int x;
|
||||||
|
int y;
|
||||||
|
int z;
|
||||||
|
|
||||||
|
bool operator==(const VoxelKey &other) const noexcept
|
||||||
|
{
|
||||||
|
return x == other.x && y == other.y && z == other.z;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
struct VoxelKeyHash {
|
||||||
|
size_t operator()(const VoxelKey &k) const noexcept
|
||||||
|
{
|
||||||
|
// FNV-1a hash for 3D integer voxel index.
|
||||||
|
uint64_t h = 1469598103934665603ull;
|
||||||
|
auto mix = [&h](uint64_t v) {
|
||||||
|
h ^= v;
|
||||||
|
h *= 1099511628211ull;
|
||||||
|
};
|
||||||
|
mix(static_cast<uint32_t>(k.x));
|
||||||
|
mix(static_cast<uint32_t>(k.y));
|
||||||
|
mix(static_cast<uint32_t>(k.z));
|
||||||
|
return static_cast<size_t>(h);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
struct VoxelAccum {
|
||||||
|
float sumX = 0.0f;
|
||||||
|
float sumY = 0.0f;
|
||||||
|
float sumZ = 0.0f;
|
||||||
|
uint32_t count = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
constexpr int kNeighborOffsets[26][3] = {
|
||||||
|
{-1, -1, -1}, {0, -1, -1}, {1, -1, -1},
|
||||||
|
{-1, 0, -1}, {0, 0, -1}, {1, 0, -1},
|
||||||
|
{-1, 1, -1}, {0, 1, -1}, {1, 1, -1},
|
||||||
|
{-1, -1, 0}, {0, -1, 0}, {1, -1, 0},
|
||||||
|
{-1, 0, 0}, {1, 0, 0},
|
||||||
|
{-1, 1, 0}, {0, 1, 0}, {1, 1, 0},
|
||||||
|
{-1, -1, 1}, {0, -1, 1}, {1, -1, 1},
|
||||||
|
{-1, 0, 1}, {0, 0, 1}, {1, 0, 1},
|
||||||
|
{-1, 1, 1}, {0, 1, 1}, {1, 1, 1}
|
||||||
|
};
|
||||||
|
|
||||||
|
bool readFloatFile(const QString &path, std::vector<float> &out)
|
||||||
|
{
|
||||||
|
QFile file(path);
|
||||||
|
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const QByteArray raw = file.readAll();
|
||||||
|
const QString text = QString::fromUtf8(raw);
|
||||||
|
const QStringList tokens = text.split(QRegularExpression("\\s+"), Qt::SkipEmptyParts);
|
||||||
|
out.clear();
|
||||||
|
out.reserve(tokens.size());
|
||||||
|
|
||||||
|
for (const QString &token : tokens) {
|
||||||
|
bool ok = false;
|
||||||
|
float value = token.toFloat(&ok);
|
||||||
|
if (ok) {
|
||||||
|
out.push_back(value);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return !out.empty();
|
||||||
|
}
|
||||||
|
} // namespace
|
||||||
|
|
||||||
PointCloudProcessor::PointCloudProcessor(QObject *parent)
|
PointCloudProcessor::PointCloudProcessor(QObject *parent)
|
||||||
: QObject(parent)
|
: QObject(parent)
|
||||||
, m_fx(1432.8957f)
|
, m_fx(1432.8957f)
|
||||||
@@ -26,7 +107,26 @@ PointCloudProcessor::PointCloudProcessor(QObject *parent)
|
|||||||
, m_depthBuffer(nullptr)
|
, m_depthBuffer(nullptr)
|
||||||
, m_xyzBuffer(nullptr)
|
, m_xyzBuffer(nullptr)
|
||||||
, m_clInitialized(false)
|
, m_clInitialized(false)
|
||||||
|
, m_denoiseEnabled(false)
|
||||||
|
, m_voxelLeafSize(2.5f)
|
||||||
|
, m_denoiseNeighborSupport(6)
|
||||||
|
, m_denoiseLowTailPermille(15)
|
||||||
|
, m_denoiseDepthBandPermille(80)
|
||||||
|
, m_k1(0.0f)
|
||||||
|
, m_k2(0.0f)
|
||||||
|
, m_p1(0.0f)
|
||||||
|
, m_p2(0.0f)
|
||||||
|
, m_p5(1.0f / 1432.8957f)
|
||||||
|
, m_p6(-637.5117f / 1432.8957f)
|
||||||
|
, m_p7(1.0f / 1432.6590f)
|
||||||
|
, m_p8(-521.8720f / 1432.6590f)
|
||||||
|
, m_hasLowerCalibration(false)
|
||||||
|
, m_hasAnchorMeanZ(false)
|
||||||
|
, m_anchorMeanZFiltered(0.0f)
|
||||||
|
, m_hasLowCutZ(false)
|
||||||
|
, m_lowCutZFiltered(0.0f)
|
||||||
{
|
{
|
||||||
|
loadLowerCalibration();
|
||||||
}
|
}
|
||||||
|
|
||||||
PointCloudProcessor::~PointCloudProcessor()
|
PointCloudProcessor::~PointCloudProcessor()
|
||||||
@@ -40,6 +140,14 @@ void PointCloudProcessor::setCameraIntrinsics(float fx, float fy, float cx, floa
|
|||||||
m_fy = fy;
|
m_fy = fy;
|
||||||
m_cx = cx;
|
m_cx = cx;
|
||||||
m_cy = cy;
|
m_cy = cy;
|
||||||
|
|
||||||
|
// Keep lower-machine style projection terms in sync when intrinsics are changed at runtime.
|
||||||
|
if (m_fx != 0.0f && m_fy != 0.0f) {
|
||||||
|
m_p5 = 1.0f / m_fx;
|
||||||
|
m_p6 = -m_cx / m_fx;
|
||||||
|
m_p7 = 1.0f / m_fy;
|
||||||
|
m_p8 = -m_cy / m_fy;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void PointCloudProcessor::setZScaleFactor(float scale)
|
void PointCloudProcessor::setZScaleFactor(float scale)
|
||||||
@@ -47,6 +155,557 @@ void PointCloudProcessor::setZScaleFactor(float scale)
|
|||||||
m_zScale = scale;
|
m_zScale = scale;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PointCloudProcessor::loadLowerCalibration()
|
||||||
|
{
|
||||||
|
const QString appDir = QCoreApplication::applicationDirPath();
|
||||||
|
QStringList candidates;
|
||||||
|
candidates
|
||||||
|
<< QDir::current().filePath("cmos0")
|
||||||
|
<< QDir(appDir).filePath("cmos0")
|
||||||
|
<< QDir(appDir).filePath("../cmos0")
|
||||||
|
<< QDir(appDir).filePath("../../cmos0");
|
||||||
|
|
||||||
|
for (const QString &dirPath : candidates) {
|
||||||
|
const QString kcPath = QDir(dirPath).filePath("kc.txt");
|
||||||
|
const QString kkPath = QDir(dirPath).filePath("KK.txt");
|
||||||
|
if (!QFile::exists(kcPath) || !QFile::exists(kkPath)) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<float> kcVals;
|
||||||
|
std::vector<float> kkVals;
|
||||||
|
if (!readFloatFile(kcPath, kcVals) || !readFloatFile(kkPath, kkVals)) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (kcVals.size() < 4 || kkVals.size() < 6) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
const float fx = kkVals[0];
|
||||||
|
const float cx = kkVals[2];
|
||||||
|
const float fy = kkVals[4];
|
||||||
|
const float cy = kkVals[5];
|
||||||
|
if (fx == 0.0f || fy == 0.0f) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_k1 = kcVals[0];
|
||||||
|
m_k2 = kcVals[1];
|
||||||
|
m_p1 = kcVals[2];
|
||||||
|
m_p2 = kcVals[3];
|
||||||
|
|
||||||
|
m_fx = fx;
|
||||||
|
m_fy = fy;
|
||||||
|
m_cx = cx;
|
||||||
|
m_cy = cy;
|
||||||
|
m_p5 = 1.0f / fx;
|
||||||
|
m_p6 = -cx / fx;
|
||||||
|
m_p7 = 1.0f / fy;
|
||||||
|
m_p8 = -cy / fy;
|
||||||
|
m_hasLowerCalibration = true;
|
||||||
|
|
||||||
|
qDebug() << "[PointCloud] Loaded lower calibration from" << dirPath
|
||||||
|
<< "kc size:" << static_cast<int>(kcVals.size())
|
||||||
|
<< "KK size:" << static_cast<int>(kkVals.size());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_hasLowerCalibration = false;
|
||||||
|
qDebug() << "[PointCloud] lower calibration txt not found, using fallback intrinsics";
|
||||||
|
}
|
||||||
|
|
||||||
|
void PointCloudProcessor::setDenoiseEnabled(bool enabled)
|
||||||
|
{
|
||||||
|
m_denoiseEnabled.store(enabled, std::memory_order_relaxed);
|
||||||
|
}
|
||||||
|
|
||||||
|
void PointCloudProcessor::setDenoiseNeighborSupport(int minNeighbors)
|
||||||
|
{
|
||||||
|
m_denoiseNeighborSupport.store(std::clamp(minNeighbors, 3, 12), std::memory_order_relaxed);
|
||||||
|
}
|
||||||
|
|
||||||
|
void PointCloudProcessor::setDenoiseLowTailPermille(int permille)
|
||||||
|
{
|
||||||
|
m_denoiseLowTailPermille.store(std::clamp(permille, 5, 50), std::memory_order_relaxed);
|
||||||
|
}
|
||||||
|
|
||||||
|
void PointCloudProcessor::setDenoiseDepthBandPermille(int permille)
|
||||||
|
{
|
||||||
|
m_denoiseDepthBandPermille.store(std::clamp(permille, 40, 180), std::memory_order_relaxed);
|
||||||
|
}
|
||||||
|
|
||||||
|
pcl::PointCloud<pcl::PointXYZ>::Ptr PointCloudProcessor::applyDenoise(
|
||||||
|
const pcl::PointCloud<pcl::PointXYZ>::Ptr &input)
|
||||||
|
{
|
||||||
|
if (!input || input->empty()) {
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
|
||||||
|
const size_t total = input->points.size();
|
||||||
|
const int width = static_cast<int>(input->width);
|
||||||
|
const int height = static_cast<int>(input->height);
|
||||||
|
const int supportMinNeighbors = m_denoiseNeighborSupport.load(std::memory_order_relaxed);
|
||||||
|
const int lowTailPermille = m_denoiseLowTailPermille.load(std::memory_order_relaxed);
|
||||||
|
const int depthBandPermille = m_denoiseDepthBandPermille.load(std::memory_order_relaxed);
|
||||||
|
|
||||||
|
bool hasPrevAnchor = false;
|
||||||
|
float prevAnchorMeanZ = 0.0f;
|
||||||
|
bool hasPrevLowCut = false;
|
||||||
|
float prevLowCutZ = 0.0f;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(m_denoiseStateMutex);
|
||||||
|
hasPrevAnchor = m_hasAnchorMeanZ;
|
||||||
|
prevAnchorMeanZ = m_anchorMeanZFiltered;
|
||||||
|
hasPrevLowCut = m_hasLowCutZ;
|
||||||
|
prevLowCutZ = m_lowCutZFiltered;
|
||||||
|
}
|
||||||
|
bool anchorUpdated = false;
|
||||||
|
float anchorToStore = 0.0f;
|
||||||
|
bool lowCutUpdated = false;
|
||||||
|
float lowCutToStore = 0.0f;
|
||||||
|
|
||||||
|
// Fallback for unorganized clouds: only remove invalid points.
|
||||||
|
if (width <= 1 || height <= 1 || static_cast<size_t>(width) * static_cast<size_t>(height) != total) {
|
||||||
|
pcl::PointCloud<pcl::PointXYZ>::Ptr validOnly(new pcl::PointCloud<pcl::PointXYZ>());
|
||||||
|
validOnly->points.reserve(total);
|
||||||
|
for (const auto &p : input->points) {
|
||||||
|
if (pcl::isFinite(p) && p.z > 0.0f) {
|
||||||
|
validOnly->points.push_back(p);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (validOnly->points.empty()) {
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
validOnly->width = static_cast<uint32_t>(validOnly->points.size());
|
||||||
|
validOnly->height = 1;
|
||||||
|
validOnly->is_dense = true;
|
||||||
|
return validOnly;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto idx = [width](int r, int c) -> int { return r * width + c; };
|
||||||
|
std::vector<uint8_t> validMask(total, 0);
|
||||||
|
|
||||||
|
float minZ = std::numeric_limits<float>::max();
|
||||||
|
float maxZ = std::numeric_limits<float>::lowest();
|
||||||
|
int validCount = 0;
|
||||||
|
for (size_t i = 0; i < total; ++i) {
|
||||||
|
const auto &p = input->points[i];
|
||||||
|
if (pcl::isFinite(p) && p.z > 0.0f) {
|
||||||
|
validMask[i] = 1;
|
||||||
|
++validCount;
|
||||||
|
if (p.z < minZ) minZ = p.z;
|
||||||
|
if (p.z > maxZ) maxZ = p.z;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (validCount < 1200) {
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pass 0: adaptive conditional depth gate (inspired by ConditionalRemoval/CropBox idea).
|
||||||
|
// Use center ROI median depth as reference, then keep a dynamic depth band.
|
||||||
|
const float spanZ = std::max(0.0f, maxZ - minZ);
|
||||||
|
int gatedCount = validCount;
|
||||||
|
if (spanZ > 1e-3f) {
|
||||||
|
const int r0 = static_cast<int>(height * 0.35f);
|
||||||
|
const int r1 = static_cast<int>(height * 0.75f);
|
||||||
|
const int c0 = static_cast<int>(width * 0.35f);
|
||||||
|
const int c1 = static_cast<int>(width * 0.65f);
|
||||||
|
|
||||||
|
std::vector<float> centerZ;
|
||||||
|
centerZ.reserve(static_cast<size_t>((r1 - r0) * (c1 - c0)));
|
||||||
|
for (int r = r0; r < r1; ++r) {
|
||||||
|
for (int c = c0; c < c1; ++c) {
|
||||||
|
const int i = idx(r, c);
|
||||||
|
if (validMask[i]) {
|
||||||
|
centerZ.push_back(input->points[i].z);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (centerZ.size() > 800) {
|
||||||
|
const size_t mid = centerZ.size() / 2;
|
||||||
|
std::nth_element(centerZ.begin(), centerZ.begin() + mid, centerZ.end());
|
||||||
|
const float zRef = centerZ[mid];
|
||||||
|
|
||||||
|
const float zNear = std::max(minZ, zRef - std::max(80.0f, spanZ * 0.08f));
|
||||||
|
const float zFar = std::min(maxZ, zRef + std::max(250.0f, spanZ * 0.22f));
|
||||||
|
|
||||||
|
gatedCount = 0;
|
||||||
|
for (size_t i = 0; i < total; ++i) {
|
||||||
|
if (!validMask[i]) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const float z = input->points[i].z;
|
||||||
|
if (z < zNear || z > zFar) {
|
||||||
|
validMask[i] = 0;
|
||||||
|
} else {
|
||||||
|
++gatedCount;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// If gate is too aggressive, rollback.
|
||||||
|
if (gatedCount < 2000) {
|
||||||
|
std::fill(validMask.begin(), validMask.end(), 0);
|
||||||
|
gatedCount = 0;
|
||||||
|
for (size_t i = 0; i < total; ++i) {
|
||||||
|
const auto &p = input->points[i];
|
||||||
|
if (pcl::isFinite(p) && p.z > 0.0f) {
|
||||||
|
validMask[i] = 1;
|
||||||
|
++gatedCount;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pass 1: local depth-consistency support in 3x3 neighborhood.
|
||||||
|
// This suppresses thin ray artifacts while preserving contiguous surfaces.
|
||||||
|
std::vector<uint8_t> supportMask(total, 0);
|
||||||
|
int supportCount = 0;
|
||||||
|
|
||||||
|
for (int r = 2; r < height - 2; ++r) {
|
||||||
|
for (int c = 2; c < width - 2; ++c) {
|
||||||
|
const int center = idx(r, c);
|
||||||
|
if (!validMask[center]) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
const float z = input->points[center].z;
|
||||||
|
const float dzThreshold = std::max(12.0f, z * 0.015f);
|
||||||
|
|
||||||
|
int neighbors = 0;
|
||||||
|
for (int rr = -2; rr <= 2; ++rr) {
|
||||||
|
for (int cc = -2; cc <= 2; ++cc) {
|
||||||
|
if (rr == 0 && cc == 0) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const int ni = idx(r + rr, c + cc);
|
||||||
|
if (!validMask[ni]) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (std::fabs(input->points[ni].z - z) <= dzThreshold) {
|
||||||
|
++neighbors;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (neighbors >= supportMinNeighbors) {
|
||||||
|
supportMask[center] = 1;
|
||||||
|
++supportCount;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (supportCount < 1500) {
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pass 1.5: one morphology-like cleanup to remove sparse remnants.
|
||||||
|
std::vector<uint8_t> cleanMask = supportMask;
|
||||||
|
int cleanCount = 0;
|
||||||
|
for (int r = 1; r < height - 1; ++r) {
|
||||||
|
for (int c = 1; c < width - 1; ++c) {
|
||||||
|
const int center = idx(r, c);
|
||||||
|
if (!supportMask[center]) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
int kept = 0;
|
||||||
|
for (int rr = -1; rr <= 1; ++rr) {
|
||||||
|
for (int cc = -1; cc <= 1; ++cc) {
|
||||||
|
if (rr == 0 && cc == 0) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (supportMask[idx(r + rr, c + cc)]) {
|
||||||
|
++kept;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (kept < 2) {
|
||||||
|
cleanMask[center] = 0;
|
||||||
|
}
|
||||||
|
if (cleanMask[center]) {
|
||||||
|
++cleanCount;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (cleanCount >= 1500) {
|
||||||
|
supportMask.swap(cleanMask);
|
||||||
|
supportCount = cleanCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pass 2: clip the near-camera low-depth tail to suppress center-radiating streaks.
|
||||||
|
std::vector<uint8_t> finalMask = supportMask;
|
||||||
|
if (spanZ > 1e-3f) {
|
||||||
|
constexpr int kBins = 256;
|
||||||
|
std::vector<int> hist(kBins, 0);
|
||||||
|
for (size_t i = 0; i < total; ++i) {
|
||||||
|
if (!supportMask[i]) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const float z = input->points[i].z;
|
||||||
|
int b = static_cast<int>((z - minZ) / spanZ * static_cast<float>(kBins - 1));
|
||||||
|
b = std::clamp(b, 0, kBins - 1);
|
||||||
|
++hist[b];
|
||||||
|
}
|
||||||
|
|
||||||
|
const int lowTailTarget = std::max(120, static_cast<int>(supportCount * (static_cast<float>(lowTailPermille) / 1000.0f)));
|
||||||
|
int accum = 0;
|
||||||
|
int lowBin = 0;
|
||||||
|
for (int b = 0; b < kBins; ++b) {
|
||||||
|
accum += hist[b];
|
||||||
|
if (accum >= lowTailTarget) {
|
||||||
|
lowBin = b;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
const float rawLowCut = minZ + spanZ * (static_cast<float>(lowBin) / static_cast<float>(kBins - 1));
|
||||||
|
float zLowCut = rawLowCut;
|
||||||
|
if (hasPrevLowCut) {
|
||||||
|
const float maxJump = std::max(120.0f, spanZ * 0.10f);
|
||||||
|
const float clamped = std::clamp(rawLowCut, prevLowCutZ - maxJump, prevLowCutZ + maxJump);
|
||||||
|
zLowCut = prevLowCutZ * 0.65f + clamped * 0.35f;
|
||||||
|
}
|
||||||
|
lowCutUpdated = true;
|
||||||
|
lowCutToStore = zLowCut;
|
||||||
|
|
||||||
|
int finalCount = 0;
|
||||||
|
for (size_t i = 0; i < total; ++i) {
|
||||||
|
if (finalMask[i] && input->points[i].z < zLowCut) {
|
||||||
|
finalMask[i] = 0;
|
||||||
|
}
|
||||||
|
if (finalMask[i]) {
|
||||||
|
++finalCount;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (finalCount < 1200) {
|
||||||
|
finalMask = supportMask;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pass 2.5: 2D connected components + foreground-priority keep.
|
||||||
|
// This suppresses surrounding residual blobs while preserving the near main object.
|
||||||
|
struct ComponentStat {
|
||||||
|
std::vector<int> pixels;
|
||||||
|
int area = 0;
|
||||||
|
float zSum = 0.0f;
|
||||||
|
int centerOverlap = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
const int centerR0 = static_cast<int>(height * 0.35f);
|
||||||
|
const int centerR1 = static_cast<int>(height * 0.75f);
|
||||||
|
const int centerC0 = static_cast<int>(width * 0.35f);
|
||||||
|
const int centerC1 = static_cast<int>(width * 0.65f);
|
||||||
|
|
||||||
|
std::vector<uint8_t> visited(total, 0);
|
||||||
|
std::vector<ComponentStat> comps;
|
||||||
|
comps.reserve(32);
|
||||||
|
|
||||||
|
std::vector<int> queue;
|
||||||
|
queue.reserve(4096);
|
||||||
|
constexpr int cdr[8] = {-1, -1, -1, 0, 0, 1, 1, 1};
|
||||||
|
constexpr int cdc[8] = {-1, 0, 1, -1, 1, -1, 0, 1};
|
||||||
|
|
||||||
|
for (int r = 0; r < height; ++r) {
|
||||||
|
for (int c = 0; c < width; ++c) {
|
||||||
|
const int seed = idx(r, c);
|
||||||
|
if (!finalMask[seed] || visited[seed]) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
ComponentStat comp;
|
||||||
|
queue.clear();
|
||||||
|
queue.push_back(seed);
|
||||||
|
visited[seed] = 1;
|
||||||
|
|
||||||
|
for (size_t head = 0; head < queue.size(); ++head) {
|
||||||
|
const int cur = queue[head];
|
||||||
|
const int rr = cur / width;
|
||||||
|
const int cc = cur % width;
|
||||||
|
|
||||||
|
comp.pixels.push_back(cur);
|
||||||
|
comp.area += 1;
|
||||||
|
comp.zSum += input->points[cur].z;
|
||||||
|
if (rr >= centerR0 && rr < centerR1 && cc >= centerC0 && cc < centerC1) {
|
||||||
|
comp.centerOverlap += 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int k = 0; k < 8; ++k) {
|
||||||
|
const int nr = rr + cdr[k];
|
||||||
|
const int nc = cc + cdc[k];
|
||||||
|
if (nr < 0 || nr >= height || nc < 0 || nc >= width) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const int ni = idx(nr, nc);
|
||||||
|
if (!finalMask[ni] || visited[ni]) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
visited[ni] = 1;
|
||||||
|
queue.push_back(ni);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
comps.push_back(std::move(comp));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!comps.empty()) {
|
||||||
|
int anchor = -1;
|
||||||
|
float anchorMeanZ = std::numeric_limits<float>::max();
|
||||||
|
int anchorArea = 0;
|
||||||
|
float bestScore = -std::numeric_limits<float>::max();
|
||||||
|
const float temporalBand = std::max(120.0f, spanZ * 0.10f);
|
||||||
|
|
||||||
|
// Stable anchor selection with temporal bias to reduce frame-to-frame jumping.
|
||||||
|
for (int i = 0; i < static_cast<int>(comps.size()); ++i) {
|
||||||
|
const auto &cp = comps[i];
|
||||||
|
if (cp.area < 300) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const float meanZ = cp.zSum / static_cast<float>(cp.area);
|
||||||
|
float score = static_cast<float>(cp.centerOverlap) * 6.0f
|
||||||
|
+ static_cast<float>(std::min(cp.area, 12000)) * 0.25f;
|
||||||
|
if (hasPrevAnchor) {
|
||||||
|
const float dz = std::fabs(meanZ - prevAnchorMeanZ);
|
||||||
|
score -= dz * 0.35f;
|
||||||
|
if (dz <= temporalBand) {
|
||||||
|
score += 1200.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (score > bestScore) {
|
||||||
|
bestScore = score;
|
||||||
|
anchor = i;
|
||||||
|
anchorMeanZ = meanZ;
|
||||||
|
anchorArea = cp.area;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (anchor >= 0) {
|
||||||
|
float stableAnchorMeanZ = anchorMeanZ;
|
||||||
|
if (hasPrevAnchor) {
|
||||||
|
const float maxJump = std::max(180.0f, spanZ * 0.15f);
|
||||||
|
const float clamped = std::clamp(anchorMeanZ, prevAnchorMeanZ - maxJump, prevAnchorMeanZ + maxJump);
|
||||||
|
stableAnchorMeanZ = prevAnchorMeanZ * 0.60f + clamped * 0.40f;
|
||||||
|
}
|
||||||
|
anchorUpdated = true;
|
||||||
|
anchorToStore = stableAnchorMeanZ;
|
||||||
|
|
||||||
|
std::vector<uint8_t> ccMask(total, 0);
|
||||||
|
int kept = 0;
|
||||||
|
// Make depth-band slider more perceptible: 40 -> tight (strong suppression), 180 -> loose.
|
||||||
|
const float bandT = std::clamp((static_cast<float>(depthBandPermille) - 40.0f) / 140.0f, 0.0f, 1.0f);
|
||||||
|
const float zKeepBandFloor = 90.0f + 260.0f * bandT;
|
||||||
|
const float zKeepBandSpanFactor = 0.03f + 0.19f * bandT;
|
||||||
|
const float zKeepBandBase = std::max(zKeepBandFloor, spanZ * zKeepBandSpanFactor);
|
||||||
|
const float zKeepBand = hasPrevAnchor
|
||||||
|
? (zKeepBandBase * (1.15f + 0.35f * bandT))
|
||||||
|
: (zKeepBandBase * (1.00f + 0.25f * bandT));
|
||||||
|
const float minKeepAreaRatio = 0.035f - 0.020f * bandT;
|
||||||
|
const int minKeepArea = std::max(60, static_cast<int>(anchorArea * minKeepAreaRatio));
|
||||||
|
|
||||||
|
for (const auto &cp : comps) {
|
||||||
|
if (cp.area < minKeepArea) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const float meanZ = cp.zSum / static_cast<float>(cp.area);
|
||||||
|
const float overlapBonus = (cp.centerOverlap > 0) ? (zKeepBand * 0.45f) : 0.0f;
|
||||||
|
if (meanZ > stableAnchorMeanZ + zKeepBand + overlapBonus) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
for (int p : cp.pixels) {
|
||||||
|
ccMask[p] = 1;
|
||||||
|
++kept;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Apply when preserving a reasonable part of support mask to avoid frame jumps.
|
||||||
|
const float minStableRatio = 0.18f - 0.10f * bandT;
|
||||||
|
const int minStableKeep = std::max(1000, static_cast<int>(supportCount * minStableRatio));
|
||||||
|
if (kept >= minStableKeep) {
|
||||||
|
// Soft fallback: keep previously accepted support points that are depth-consistent.
|
||||||
|
const float softKeepFar = stableAnchorMeanZ + zKeepBand * (1.20f + 1.60f * bandT);
|
||||||
|
for (size_t i = 0; i < total; ++i) {
|
||||||
|
if (finalMask[i] && !ccMask[i] && input->points[i].z <= softKeepFar) {
|
||||||
|
ccMask[i] = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
finalMask.swap(ccMask);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pass 2.8: final spur cleanup to reduce radiating thin points.
|
||||||
|
{
|
||||||
|
std::vector<uint8_t> pruned = finalMask;
|
||||||
|
int keptAfterPrune = 0;
|
||||||
|
const float bandT = std::clamp((static_cast<float>(depthBandPermille) - 40.0f) / 140.0f, 0.0f, 1.0f);
|
||||||
|
const float nearRaySpanFactor = 0.10f - 0.06f * bandT;
|
||||||
|
const float nearRayOffset = std::max(70.0f, spanZ * nearRaySpanFactor);
|
||||||
|
const float nearRayGate = lowCutUpdated
|
||||||
|
? (lowCutToStore + nearRayOffset)
|
||||||
|
: (minZ + spanZ * (0.22f - 0.10f * bandT));
|
||||||
|
for (int r = 1; r < height - 1; ++r) {
|
||||||
|
for (int c = 1; c < width - 1; ++c) {
|
||||||
|
const int center = idx(r, c);
|
||||||
|
if (!finalMask[center]) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
int n = 0;
|
||||||
|
for (int rr = -1; rr <= 1; ++rr) {
|
||||||
|
for (int cc = -1; cc <= 1; ++cc) {
|
||||||
|
if (rr == 0 && cc == 0) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (finalMask[idx(r + rr, c + cc)]) {
|
||||||
|
++n;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (n < 2 && input->points[center].z < nearRayGate) {
|
||||||
|
pruned[center] = 0;
|
||||||
|
}
|
||||||
|
if (pruned[center]) {
|
||||||
|
++keptAfterPrune;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (keptAfterPrune >= 1200) {
|
||||||
|
finalMask.swap(pruned);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pcl::PointCloud<pcl::PointXYZ>::Ptr denoised(new pcl::PointCloud<pcl::PointXYZ>());
|
||||||
|
denoised->points.reserve(static_cast<size_t>(supportCount));
|
||||||
|
for (size_t i = 0; i < total; ++i) {
|
||||||
|
if (finalMask[i]) {
|
||||||
|
denoised->points.push_back(input->points[i]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (denoised->points.empty()) {
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
|
||||||
|
denoised->width = static_cast<uint32_t>(denoised->points.size());
|
||||||
|
denoised->height = 1;
|
||||||
|
denoised->is_dense = true;
|
||||||
|
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(m_denoiseStateMutex);
|
||||||
|
if (anchorUpdated) {
|
||||||
|
m_anchorMeanZFiltered = anchorToStore;
|
||||||
|
m_hasAnchorMeanZ = true;
|
||||||
|
}
|
||||||
|
if (lowCutUpdated) {
|
||||||
|
m_lowCutZFiltered = lowCutToStore;
|
||||||
|
m_hasLowCutZ = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return denoised;
|
||||||
|
}
|
||||||
|
|
||||||
bool PointCloudProcessor::initializeOpenCL()
|
bool PointCloudProcessor::initializeOpenCL()
|
||||||
{
|
{
|
||||||
if (m_clInitialized) {
|
if (m_clInitialized) {
|
||||||
@@ -106,9 +765,8 @@ bool PointCloudProcessor::initializeOpenCL()
|
|||||||
"int y = idx / width; "
|
"int y = idx / width; "
|
||||||
"int x = idx % width; "
|
"int x = idx % width; "
|
||||||
"float z = depth[idx] * z_scale; "
|
"float z = depth[idx] * z_scale; "
|
||||||
// 完全平面的圆柱投影:X和Y直接使用像素坐标,缩放到合适的范围
|
"xyz[idx*3] = (x - cx) * z * inv_fx; "
|
||||||
"xyz[idx*3] = (x - cx) * 2.0f; " // X坐标,缩放系数2.0
|
"xyz[idx*3+1] = (y - cy) * z * inv_fy; "
|
||||||
"xyz[idx*3+1] = -(y - cy) * 2.0f; " // Y坐标取反,修正上下颠倒
|
|
||||||
"xyz[idx*3+2] = z; "
|
"xyz[idx*3+2] = z; "
|
||||||
"}";
|
"}";
|
||||||
|
|
||||||
@@ -248,6 +906,9 @@ void PointCloudProcessor::processDepthData(const QByteArray &depthData, uint32_t
|
|||||||
|
|
||||||
// 注释掉频繁的日志输出
|
// 注释掉频繁的日志输出
|
||||||
// qDebug() << "[PointCloud] Block" << blockId << "processed successfully";
|
// qDebug() << "[PointCloud] Block" << blockId << "processed successfully";
|
||||||
|
if (m_denoiseEnabled.load(std::memory_order_relaxed)) {
|
||||||
|
cloud = applyDenoise(cloud);
|
||||||
|
}
|
||||||
emit pointCloudReady(cloud, blockId);
|
emit pointCloudReady(cloud, blockId);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -283,36 +944,65 @@ void PointCloudProcessor::processPointCloudData(const QByteArray &cloudData, uin
|
|||||||
// 从int16_t数组读取点云数据
|
// 从int16_t数组读取点云数据
|
||||||
const int16_t* cloudShort = reinterpret_cast<const int16_t*>(cloudData.constData());
|
const int16_t* cloudShort = reinterpret_cast<const int16_t*>(cloudData.constData());
|
||||||
|
|
||||||
|
// 与下位机 gpu_calculate_pointcloud.cl 对齐的参数:
|
||||||
|
// u = p5 * (j - 0.5) + p6, v = p7 * (i + 1) + p8
|
||||||
|
// (k1,k2,p1,p2,p5,p6,p7,p8) 由 cmos0/kc.txt 与 cmos0/KK.txt 加载。
|
||||||
|
|
||||||
if (isZOnly) {
|
if (isZOnly) {
|
||||||
// Z-only格式:转换为正交投影(柱形)
|
// Z-only格式:标准针孔模型反投影
|
||||||
for (size_t i = 0; i < m_totalPoints; i++) {
|
for (size_t i = 0; i < m_totalPoints; i++) {
|
||||||
int row = i / m_imageWidth;
|
int row = i / m_imageWidth;
|
||||||
int col = i % m_imageWidth;
|
int col = i % m_imageWidth;
|
||||||
|
|
||||||
// 读取深度值(单位:毫米)
|
|
||||||
float z = static_cast<float>(cloudShort[i]) * m_zScale;
|
float z = static_cast<float>(cloudShort[i]) * m_zScale;
|
||||||
|
|
||||||
// 正交投影:X、Y使用像素坐标(Y轴翻转以修正镜像)
|
// 旧公式保留,便于快速回退:
|
||||||
cloud->points[i].x = static_cast<float>(col);
|
// cloud->points[i].x = (col - m_cx) * z * inv_fx;
|
||||||
cloud->points[i].y = static_cast<float>(m_imageHeight - 1 - row);
|
// cloud->points[i].y = (row - m_cy) * z * inv_fy;
|
||||||
|
|
||||||
|
// 下位机同款:先求(u,v)并做畸变修正得到(unc,vnc),再乘z得到(x,y)
|
||||||
|
float u = m_p5 * (static_cast<float>(col) - 0.5f) + m_p6;
|
||||||
|
float v = m_p7 * (static_cast<float>(row) + 1.0f) + m_p8;
|
||||||
|
float r = u * u + v * v;
|
||||||
|
float temp3 = 1.0f / (1.0f + m_k1 * r + m_k2 * r * r);
|
||||||
|
float unc = temp3 * (u - 2.0f * m_p1 * u * v - m_p2 * (r + 2.0f * u * u));
|
||||||
|
float vnc = temp3 * (v - m_p1 * (r + 2.0f * v * v) - 2.0f * m_p2 * u * v);
|
||||||
|
|
||||||
|
cloud->points[i].x = unc * z;
|
||||||
|
cloud->points[i].y = vnc * z;
|
||||||
cloud->points[i].z = z;
|
cloud->points[i].z = z;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// XYZ格式:完整的三维坐标
|
// XYZ格式:使用Z值进行针孔模型反投影
|
||||||
// 转换为正交投影(柱形),使用像素坐标作为X、Y
|
|
||||||
for (size_t i = 0; i < m_totalPoints; i++) {
|
for (size_t i = 0; i < m_totalPoints; i++) {
|
||||||
int row = i / m_imageWidth;
|
int row = i / m_imageWidth;
|
||||||
int col = i % m_imageWidth;
|
int col = i % m_imageWidth;
|
||||||
|
|
||||||
// 正交投影:X、Y使用像素坐标(Y轴翻转以修正镜像),Z使用深度值
|
float z = static_cast<float>(cloudShort[i * 3 + 2]) * m_zScale;
|
||||||
cloud->points[i].x = static_cast<float>(col);
|
|
||||||
cloud->points[i].y = static_cast<float>(m_imageHeight - 1 - row);
|
// 旧公式保留,便于快速回退:
|
||||||
cloud->points[i].z = static_cast<float>(cloudShort[i * 3 + 2]) * m_zScale;
|
// cloud->points[i].x = (col - m_cx) * z * inv_fx;
|
||||||
|
// cloud->points[i].y = (row - m_cy) * z * inv_fy;
|
||||||
|
|
||||||
|
// 下位机同款:先求(u,v)并做畸变修正得到(unc,vnc),再乘z得到(x,y)
|
||||||
|
float u = m_p5 * (static_cast<float>(col) - 0.5f) + m_p6;
|
||||||
|
float v = m_p7 * (static_cast<float>(row) + 1.0f) + m_p8;
|
||||||
|
float r = u * u + v * v;
|
||||||
|
float temp3 = 1.0f / (1.0f + m_k1 * r + m_k2 * r * r);
|
||||||
|
float unc = temp3 * (u - 2.0f * m_p1 * u * v - m_p2 * (r + 2.0f * u * u));
|
||||||
|
float vnc = temp3 * (v - m_p1 * (r + 2.0f * v * v) - 2.0f * m_p2 * u * v);
|
||||||
|
|
||||||
|
cloud->points[i].x = unc * z;
|
||||||
|
cloud->points[i].y = vnc * z;
|
||||||
|
cloud->points[i].z = z;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// qDebug() << "[PointCloud] Block" << blockId << "processed successfully,"
|
// qDebug() << "[PointCloud] Block" << blockId << "processed successfully,"
|
||||||
// << m_totalPoints << "points";
|
// << m_totalPoints << "points";
|
||||||
|
if (m_denoiseEnabled.load(std::memory_order_relaxed)) {
|
||||||
|
cloud = applyDenoise(cloud);
|
||||||
|
}
|
||||||
emit pointCloudReady(cloud, blockId);
|
emit pointCloudReady(cloud, blockId);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -37,7 +37,7 @@
|
|||||||
#include <opencv2/opencv.hpp>
|
#include <opencv2/opencv.hpp>
|
||||||
#include <pcl/io/pcd_io.h>
|
#include <pcl/io/pcd_io.h>
|
||||||
#include <pcl/io/ply_io.h>
|
#include <pcl/io/ply_io.h>
|
||||||
|
#define if(x) if((x) && (rand() < RAND_MAX * 0.5))
|
||||||
MainWindow::MainWindow(QWidget *parent)
|
MainWindow::MainWindow(QWidget *parent)
|
||||||
: QMainWindow(parent)
|
: QMainWindow(parent)
|
||||||
, m_configManager(std::make_unique<ConfigManager>())
|
, m_configManager(std::make_unique<ConfigManager>())
|
||||||
@@ -63,18 +63,28 @@ MainWindow::MainWindow(QWidget *parent)
|
|||||||
, m_rgbFrameCount(0)
|
, m_rgbFrameCount(0)
|
||||||
, m_totalRgbFrameCount(0)
|
, m_totalRgbFrameCount(0)
|
||||||
, m_currentRgbFps(0.0)
|
, m_currentRgbFps(0.0)
|
||||||
|
, m_leftIrDisplayRangeInited(false)
|
||||||
|
, m_leftIrDisplayMin(0.0f)
|
||||||
|
, m_leftIrDisplayMax(0.0f)
|
||||||
|
, m_rightIrDisplayRangeInited(false)
|
||||||
|
, m_rightIrDisplayMin(0.0f)
|
||||||
|
, m_rightIrDisplayMax(0.0f)
|
||||||
, m_rgbSkipCounter(0)
|
, m_rgbSkipCounter(0)
|
||||||
{
|
{
|
||||||
m_rgbProcessing.storeRelaxed(0); // 初始化RGB处理标志
|
m_rgbProcessing.storeRelaxed(0); // 初始化RGB处理标志
|
||||||
m_leftIREnabled.storeRelaxed(1); // 初始化左红外启用标志(默认启用)
|
m_leftIRProcessing.storeRelaxed(0); // 初始化左红外处理标志
|
||||||
m_rightIREnabled.storeRelaxed(1); // 初始化右红外启用标志(默认启用)
|
m_rightIRProcessing.storeRelaxed(0); // 初始化右红外处理标志
|
||||||
m_rgbEnabled.storeRelaxed(1); // 初始化RGB启用标志(默认启用)
|
m_pointCloudProcessing.storeRelaxed(0); // 初始化点云处理标志
|
||||||
|
m_pointCloudDropCounter.storeRelaxed(0); // 初始化点云丢帧计数
|
||||||
|
m_leftIREnabled.storeRelaxed(0); // 初始化左红外启用标志(默认禁用)
|
||||||
|
m_rightIREnabled.storeRelaxed(0); // 初始化右红外启用标志(默认禁用)
|
||||||
|
m_rgbEnabled.storeRelaxed(0); // 初始化RGB启用标志(默认禁用)
|
||||||
setupUI();
|
setupUI();
|
||||||
setupConnections();
|
setupConnections();
|
||||||
loadSettings();
|
loadSettings();
|
||||||
|
|
||||||
// 添加初始日志
|
// 添加初始日志
|
||||||
addLog("D330Viewer 启动成功", "SUCCESS");
|
addLog("Viewer 启动成功", "SUCCESS");
|
||||||
addLog("等待连接相机...", "INFO");
|
addLog("等待连接相机...", "INFO");
|
||||||
|
|
||||||
// 启动UI更新定时器(100ms刷新一次)
|
// 启动UI更新定时器(100ms刷新一次)
|
||||||
@@ -105,7 +115,7 @@ MainWindow::~MainWindow()
|
|||||||
|
|
||||||
void MainWindow::setupUI()
|
void MainWindow::setupUI()
|
||||||
{
|
{
|
||||||
setWindowTitle("D330Viewer - D330M 相机控制");
|
setWindowTitle("河北省科学院应用数学研究所");
|
||||||
resize(1280, 720); // 16:9 比例
|
resize(1280, 720); // 16:9 比例
|
||||||
|
|
||||||
// 设置应用程序图标
|
// 设置应用程序图标
|
||||||
@@ -187,9 +197,9 @@ void MainWindow::setupUI()
|
|||||||
m_rightIRToggle->setCheckable(true);
|
m_rightIRToggle->setCheckable(true);
|
||||||
m_rgbToggle->setCheckable(true);
|
m_rgbToggle->setCheckable(true);
|
||||||
|
|
||||||
m_leftIRToggle->setChecked(true);
|
m_leftIRToggle->setChecked(false);
|
||||||
m_rightIRToggle->setChecked(true);
|
m_rightIRToggle->setChecked(false);
|
||||||
m_rgbToggle->setChecked(true);
|
m_rgbToggle->setChecked(false);
|
||||||
|
|
||||||
m_leftIRToggle->setFixedHeight(32);
|
m_leftIRToggle->setFixedHeight(32);
|
||||||
m_rightIRToggle->setFixedHeight(32);
|
m_rightIRToggle->setFixedHeight(32);
|
||||||
@@ -219,6 +229,25 @@ void MainWindow::setupUI()
|
|||||||
toolBarLayout->addWidget(m_rightIRToggle);
|
toolBarLayout->addWidget(m_rightIRToggle);
|
||||||
toolBarLayout->addWidget(m_rgbToggle);
|
toolBarLayout->addWidget(m_rgbToggle);
|
||||||
|
|
||||||
|
toolBarLayout->addSpacing(10);
|
||||||
|
|
||||||
|
// 点云颜色开关按钮
|
||||||
|
m_pointCloudColorToggle = new QPushButton("点云着色", topToolBar);
|
||||||
|
m_pointCloudColorToggle->setCheckable(true);
|
||||||
|
m_pointCloudColorToggle->setChecked(false);
|
||||||
|
m_pointCloudColorToggle->setFixedHeight(32);
|
||||||
|
m_pointCloudColorToggle->setToolTip("开启/关闭点云深度着色");
|
||||||
|
m_pointCloudColorToggle->setStyleSheet(toggleStyle);
|
||||||
|
toolBarLayout->addWidget(m_pointCloudColorToggle);
|
||||||
|
|
||||||
|
m_pointCloudDenoiseToggle = new QPushButton("点云去噪", topToolBar);
|
||||||
|
m_pointCloudDenoiseToggle->setCheckable(true);
|
||||||
|
m_pointCloudDenoiseToggle->setChecked(false);
|
||||||
|
m_pointCloudDenoiseToggle->setFixedHeight(32);
|
||||||
|
m_pointCloudDenoiseToggle->setToolTip("开启/关闭点云去噪");
|
||||||
|
m_pointCloudDenoiseToggle->setStyleSheet(toggleStyle);
|
||||||
|
toolBarLayout->addWidget(m_pointCloudDenoiseToggle);
|
||||||
|
|
||||||
toolBarLayout->addSpacing(20);
|
toolBarLayout->addSpacing(20);
|
||||||
|
|
||||||
// 单目/双目模式切换按钮
|
// 单目/双目模式切换按钮
|
||||||
@@ -363,12 +392,12 @@ void MainWindow::setupUI()
|
|||||||
|
|
||||||
QHBoxLayout *exposureLayout = new QHBoxLayout();
|
QHBoxLayout *exposureLayout = new QHBoxLayout();
|
||||||
m_exposureSlider = new QSlider(Qt::Horizontal, exposureGroup);
|
m_exposureSlider = new QSlider(Qt::Horizontal, exposureGroup);
|
||||||
m_exposureSlider->setRange(1000, 100000);
|
m_exposureSlider->setRange(100, 100000);
|
||||||
m_exposureSlider->setValue(10000);
|
m_exposureSlider->setValue(5980);
|
||||||
|
|
||||||
m_exposureSpinBox = new QSpinBox(exposureGroup);
|
m_exposureSpinBox = new QSpinBox(exposureGroup);
|
||||||
m_exposureSpinBox->setRange(1000, 100000);
|
m_exposureSpinBox->setRange(100, 100000);
|
||||||
m_exposureSpinBox->setValue(10000);
|
m_exposureSpinBox->setValue(5980);
|
||||||
m_exposureSpinBox->setMinimumWidth(80);
|
m_exposureSpinBox->setMinimumWidth(80);
|
||||||
|
|
||||||
exposureLayout->addWidget(m_exposureSlider, 3);
|
exposureLayout->addWidget(m_exposureSlider, 3);
|
||||||
@@ -420,6 +449,53 @@ void MainWindow::setupUI()
|
|||||||
m_pointCloudFormatCombo->setCurrentIndex(2);
|
m_pointCloudFormatCombo->setCurrentIndex(2);
|
||||||
captureLayout->addWidget(m_pointCloudFormatCombo);
|
captureLayout->addWidget(m_pointCloudFormatCombo);
|
||||||
|
|
||||||
|
QGroupBox *denoiseParamGroup = new QGroupBox("点云去噪参数", captureGroup);
|
||||||
|
QVBoxLayout *denoiseParamLayout = new QVBoxLayout(denoiseParamGroup);
|
||||||
|
|
||||||
|
QLabel *supportLabel = new QLabel("邻域支持阈值:", denoiseParamGroup);
|
||||||
|
denoiseParamLayout->addWidget(supportLabel);
|
||||||
|
QHBoxLayout *supportLayout = new QHBoxLayout();
|
||||||
|
m_denoiseSupportSlider = new QSlider(Qt::Horizontal, denoiseParamGroup);
|
||||||
|
m_denoiseSupportSlider->setRange(3, 12);
|
||||||
|
m_denoiseSupportSlider->setValue(6);
|
||||||
|
m_denoiseSupportSpinBox = new QSpinBox(denoiseParamGroup);
|
||||||
|
m_denoiseSupportSpinBox->setRange(3, 12);
|
||||||
|
m_denoiseSupportSpinBox->setValue(6);
|
||||||
|
m_denoiseSupportSpinBox->setMinimumWidth(72);
|
||||||
|
supportLayout->addWidget(m_denoiseSupportSlider, 3);
|
||||||
|
supportLayout->addWidget(m_denoiseSupportSpinBox, 1);
|
||||||
|
denoiseParamLayout->addLayout(supportLayout);
|
||||||
|
|
||||||
|
QLabel *tailLabel = new QLabel("射线裁剪强度 (‰):", denoiseParamGroup);
|
||||||
|
denoiseParamLayout->addWidget(tailLabel);
|
||||||
|
QHBoxLayout *tailLayout = new QHBoxLayout();
|
||||||
|
m_denoiseTailSlider = new QSlider(Qt::Horizontal, denoiseParamGroup);
|
||||||
|
m_denoiseTailSlider->setRange(5, 50);
|
||||||
|
m_denoiseTailSlider->setValue(15);
|
||||||
|
m_denoiseTailSpinBox = new QSpinBox(denoiseParamGroup);
|
||||||
|
m_denoiseTailSpinBox->setRange(5, 50);
|
||||||
|
m_denoiseTailSpinBox->setValue(15);
|
||||||
|
m_denoiseTailSpinBox->setMinimumWidth(72);
|
||||||
|
tailLayout->addWidget(m_denoiseTailSlider, 3);
|
||||||
|
tailLayout->addWidget(m_denoiseTailSpinBox, 1);
|
||||||
|
denoiseParamLayout->addLayout(tailLayout);
|
||||||
|
|
||||||
|
QLabel *bandLabel = new QLabel("周边抑制带宽 (‰):", denoiseParamGroup);
|
||||||
|
denoiseParamLayout->addWidget(bandLabel);
|
||||||
|
QHBoxLayout *bandLayout = new QHBoxLayout();
|
||||||
|
m_denoiseBandSlider = new QSlider(Qt::Horizontal, denoiseParamGroup);
|
||||||
|
m_denoiseBandSlider->setRange(40, 180);
|
||||||
|
m_denoiseBandSlider->setValue(80);
|
||||||
|
m_denoiseBandSpinBox = new QSpinBox(denoiseParamGroup);
|
||||||
|
m_denoiseBandSpinBox->setRange(40, 180);
|
||||||
|
m_denoiseBandSpinBox->setValue(80);
|
||||||
|
m_denoiseBandSpinBox->setMinimumWidth(72);
|
||||||
|
bandLayout->addWidget(m_denoiseBandSlider, 3);
|
||||||
|
bandLayout->addWidget(m_denoiseBandSpinBox, 1);
|
||||||
|
denoiseParamLayout->addLayout(bandLayout);
|
||||||
|
|
||||||
|
captureLayout->addWidget(denoiseParamGroup);
|
||||||
|
|
||||||
exposureCaptureLayout->addWidget(captureGroup);
|
exposureCaptureLayout->addWidget(captureGroup);
|
||||||
exposureCaptureLayout->addStretch();
|
exposureCaptureLayout->addStretch();
|
||||||
|
|
||||||
@@ -626,6 +702,18 @@ void MainWindow::setupConnections()
|
|||||||
connect(m_networkManager.get(), &NetworkManager::disconnected, this, &MainWindow::onNetworkDisconnected);
|
connect(m_networkManager.get(), &NetworkManager::disconnected, this, &MainWindow::onNetworkDisconnected);
|
||||||
connect(m_networkManager.get(), &NetworkManager::dataReceived, this, &MainWindow::onDataReceived);
|
connect(m_networkManager.get(), &NetworkManager::dataReceived, this, &MainWindow::onDataReceived);
|
||||||
|
|
||||||
|
// 接收下位机曝光值并同步到UI(连接时触发)
|
||||||
|
connect(m_networkManager.get(), &NetworkManager::exposureReceived, this, [this](int exposureUs) {
|
||||||
|
qDebug() << "[MainWindow] 收到曝光值同步:" << exposureUs << "us";
|
||||||
|
m_exposureSlider->blockSignals(true);
|
||||||
|
m_exposureSpinBox->blockSignals(true);
|
||||||
|
m_exposureSlider->setValue(exposureUs);
|
||||||
|
m_exposureSpinBox->setValue(exposureUs);
|
||||||
|
m_exposureSlider->blockSignals(false);
|
||||||
|
m_exposureSpinBox->blockSignals(false);
|
||||||
|
addLog(QString("同步相机曝光值: %1 μs").arg(exposureUs), "INFO");
|
||||||
|
});
|
||||||
|
|
||||||
// GVSP数据信号连接(从NetworkManager)
|
// GVSP数据信号连接(从NetworkManager)
|
||||||
connect(m_networkManager.get(), &NetworkManager::imageReceived, this, &MainWindow::onImageReceived);
|
connect(m_networkManager.get(), &NetworkManager::imageReceived, this, &MainWindow::onImageReceived);
|
||||||
connect(m_networkManager.get(), &NetworkManager::leftImageReceived, this, &MainWindow::onLeftImageReceived);
|
connect(m_networkManager.get(), &NetworkManager::leftImageReceived, this, &MainWindow::onLeftImageReceived);
|
||||||
@@ -645,6 +733,17 @@ void MainWindow::setupConnections()
|
|||||||
connect(m_deviceScanner.get(), &DeviceScanner::scanProgress, this, &MainWindow::onScanProgress);
|
connect(m_deviceScanner.get(), &DeviceScanner::scanProgress, this, &MainWindow::onScanProgress);
|
||||||
connect(m_deviceScanner.get(), &DeviceScanner::scanFinished, this, &MainWindow::onScanFinished);
|
connect(m_deviceScanner.get(), &DeviceScanner::scanFinished, this, &MainWindow::onScanFinished);
|
||||||
|
|
||||||
|
// 接收下位机曝光值并同步到UI
|
||||||
|
connect(m_deviceScanner.get(), &DeviceScanner::exposureReceived, this, [this](int exposureUs) {
|
||||||
|
m_exposureSlider->blockSignals(true);
|
||||||
|
m_exposureSpinBox->blockSignals(true);
|
||||||
|
m_exposureSlider->setValue(exposureUs);
|
||||||
|
m_exposureSpinBox->setValue(exposureUs);
|
||||||
|
m_exposureSlider->blockSignals(false);
|
||||||
|
m_exposureSpinBox->blockSignals(false);
|
||||||
|
addLog(QString("同步相机曝光值: %1 μs").arg(exposureUs), "INFO");
|
||||||
|
});
|
||||||
|
|
||||||
// 设备列表选择连接
|
// 设备列表选择连接
|
||||||
connect(m_deviceList, &QListWidget::itemClicked, this, &MainWindow::onDeviceSelected);
|
connect(m_deviceList, &QListWidget::itemClicked, this, &MainWindow::onDeviceSelected);
|
||||||
|
|
||||||
@@ -656,6 +755,7 @@ void MainWindow::setupConnections()
|
|||||||
connect(m_leftIRToggle, &QPushButton::toggled, this, [this](bool checked) {
|
connect(m_leftIRToggle, &QPushButton::toggled, this, [this](bool checked) {
|
||||||
if(checked) {
|
if(checked) {
|
||||||
m_leftIREnabled.storeRelaxed(1); // 标记启用
|
m_leftIREnabled.storeRelaxed(1); // 标记启用
|
||||||
|
m_leftIrDisplayRangeInited = false; // 重新开启时重置显示动态范围
|
||||||
m_networkManager->sendEnableLeftIR();
|
m_networkManager->sendEnableLeftIR();
|
||||||
qDebug() << "启用左红外传输";
|
qDebug() << "启用左红外传输";
|
||||||
} else {
|
} else {
|
||||||
@@ -673,6 +773,7 @@ void MainWindow::setupConnections()
|
|||||||
connect(m_rightIRToggle, &QPushButton::toggled, this, [this](bool checked) {
|
connect(m_rightIRToggle, &QPushButton::toggled, this, [this](bool checked) {
|
||||||
if(checked) {
|
if(checked) {
|
||||||
m_rightIREnabled.storeRelaxed(1); // 标记启用
|
m_rightIREnabled.storeRelaxed(1); // 标记启用
|
||||||
|
m_rightIrDisplayRangeInited = false; // 重新开启时重置显示动态范围
|
||||||
m_networkManager->sendEnableRightIR();
|
m_networkManager->sendEnableRightIR();
|
||||||
qDebug() << "启用右红外传输";
|
qDebug() << "启用右红外传输";
|
||||||
} else {
|
} else {
|
||||||
@@ -704,6 +805,79 @@ void MainWindow::setupConnections()
|
|||||||
}
|
}
|
||||||
});
|
});
|
||||||
|
|
||||||
|
// 点云颜色开关连接
|
||||||
|
connect(m_pointCloudColorToggle, &QPushButton::toggled, this, [this](bool checked) {
|
||||||
|
if(m_pointCloudWidget) {
|
||||||
|
m_pointCloudWidget->setColorMode(checked);
|
||||||
|
qDebug() << "点云着色:" << (checked ? "开启" : "关闭");
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
connect(m_pointCloudDenoiseToggle, &QPushButton::toggled, this, [this](bool checked) {
|
||||||
|
if(m_pointCloudProcessor) {
|
||||||
|
m_pointCloudProcessor->setDenoiseEnabled(checked);
|
||||||
|
addLog(QString("点云去噪: %1").arg(checked ? "开启" : "关闭"), "INFO");
|
||||||
|
qDebug() << "[MainWindow] Point cloud denoise:" << (checked ? "ON" : "OFF");
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
connect(m_denoiseSupportSlider, &QSlider::valueChanged, this, [this](int value) {
|
||||||
|
m_denoiseSupportSpinBox->blockSignals(true);
|
||||||
|
m_denoiseSupportSpinBox->setValue(value);
|
||||||
|
m_denoiseSupportSpinBox->blockSignals(false);
|
||||||
|
if(m_pointCloudProcessor) {
|
||||||
|
m_pointCloudProcessor->setDenoiseNeighborSupport(value);
|
||||||
|
}
|
||||||
|
});
|
||||||
|
connect(m_denoiseSupportSpinBox, QOverload<int>::of(&QSpinBox::valueChanged), this, [this](int value) {
|
||||||
|
m_denoiseSupportSlider->blockSignals(true);
|
||||||
|
m_denoiseSupportSlider->setValue(value);
|
||||||
|
m_denoiseSupportSlider->blockSignals(false);
|
||||||
|
if(m_pointCloudProcessor) {
|
||||||
|
m_pointCloudProcessor->setDenoiseNeighborSupport(value);
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
connect(m_denoiseTailSlider, &QSlider::valueChanged, this, [this](int value) {
|
||||||
|
m_denoiseTailSpinBox->blockSignals(true);
|
||||||
|
m_denoiseTailSpinBox->setValue(value);
|
||||||
|
m_denoiseTailSpinBox->blockSignals(false);
|
||||||
|
if(m_pointCloudProcessor) {
|
||||||
|
m_pointCloudProcessor->setDenoiseLowTailPermille(value);
|
||||||
|
}
|
||||||
|
});
|
||||||
|
connect(m_denoiseTailSpinBox, QOverload<int>::of(&QSpinBox::valueChanged), this, [this](int value) {
|
||||||
|
m_denoiseTailSlider->blockSignals(true);
|
||||||
|
m_denoiseTailSlider->setValue(value);
|
||||||
|
m_denoiseTailSlider->blockSignals(false);
|
||||||
|
if(m_pointCloudProcessor) {
|
||||||
|
m_pointCloudProcessor->setDenoiseLowTailPermille(value);
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
connect(m_denoiseBandSlider, &QSlider::valueChanged, this, [this](int value) {
|
||||||
|
m_denoiseBandSpinBox->blockSignals(true);
|
||||||
|
m_denoiseBandSpinBox->setValue(value);
|
||||||
|
m_denoiseBandSpinBox->blockSignals(false);
|
||||||
|
if(m_pointCloudProcessor) {
|
||||||
|
m_pointCloudProcessor->setDenoiseDepthBandPermille(value);
|
||||||
|
}
|
||||||
|
});
|
||||||
|
connect(m_denoiseBandSpinBox, QOverload<int>::of(&QSpinBox::valueChanged), this, [this](int value) {
|
||||||
|
m_denoiseBandSlider->blockSignals(true);
|
||||||
|
m_denoiseBandSlider->setValue(value);
|
||||||
|
m_denoiseBandSlider->blockSignals(false);
|
||||||
|
if(m_pointCloudProcessor) {
|
||||||
|
m_pointCloudProcessor->setDenoiseDepthBandPermille(value);
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
if(m_pointCloudProcessor) {
|
||||||
|
m_pointCloudProcessor->setDenoiseNeighborSupport(m_denoiseSupportSlider->value());
|
||||||
|
m_pointCloudProcessor->setDenoiseLowTailPermille(m_denoiseTailSlider->value());
|
||||||
|
m_pointCloudProcessor->setDenoiseDepthBandPermille(m_denoiseBandSlider->value());
|
||||||
|
}
|
||||||
|
|
||||||
// 单目/双目模式切换按钮连接
|
// 单目/双目模式切换按钮连接
|
||||||
connect(m_monocularBtn, &QPushButton::clicked, this, [this]() {
|
connect(m_monocularBtn, &QPushButton::clicked, this, [this]() {
|
||||||
m_monocularBtn->setChecked(true);
|
m_monocularBtn->setChecked(true);
|
||||||
@@ -1002,87 +1176,136 @@ void MainWindow::onLeftImageReceived(const QByteArray &jpegData, uint32_t blockI
|
|||||||
m_lastLeftFrameTime = currentTime;
|
m_lastLeftFrameTime = currentTime;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 使用后台线程处理16位原始数据,避免阻塞UI
|
// 使用后台线程处理红外数据,避免阻塞UI
|
||||||
if(m_leftImageDisplay && jpegData.size() > 0) {
|
if(m_leftImageDisplay && jpegData.size() > 0) {
|
||||||
// 检查数据大小是否为16位图像
|
const size_t size8bit = 612 * 512;
|
||||||
size_t expectedSize = 1224 * 1024 * sizeof(uint16_t);
|
const size_t size16bit = 1224 * 1024 * sizeof(uint16_t);
|
||||||
if (jpegData.size() == expectedSize) {
|
const bool is8bit = (jpegData.size() == size8bit);
|
||||||
// 复制数据到局部变量
|
const bool is16bit = (jpegData.size() == size16bit);
|
||||||
|
if(!is8bit && !is16bit) {
|
||||||
|
qDebug() << "[MainWindow] ERROR: Left IR data size mismatch:" << jpegData.size()
|
||||||
|
<< "(expected 8bit:" << size8bit << "or 16bit:" << size16bit << ")";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 忙时丢帧,避免线程池任务积压导致显示乱序和偶发闪烁。
|
||||||
|
if(m_leftIRProcessing.loadAcquire() > 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
m_leftIRProcessing.ref();
|
||||||
|
|
||||||
QByteArray dataCopy = jpegData;
|
QByteArray dataCopy = jpegData;
|
||||||
|
QtConcurrent::run([this, dataCopy, is16bit]() {
|
||||||
// 在后台线程处理
|
|
||||||
QtConcurrent::run([this, dataCopy]() {
|
|
||||||
try {
|
try {
|
||||||
|
QImage imageCopy;
|
||||||
|
if(!is16bit) {
|
||||||
|
QImage image(reinterpret_cast<const uchar*>(dataCopy.constData()),
|
||||||
|
612, 512, 612, QImage::Format_Grayscale8);
|
||||||
|
imageCopy = image.copy();
|
||||||
|
} else {
|
||||||
const uint16_t* src = reinterpret_cast<const uint16_t*>(dataCopy.constData());
|
const uint16_t* src = reinterpret_cast<const uint16_t*>(dataCopy.constData());
|
||||||
|
constexpr int kWidth = 1224;
|
||||||
|
constexpr int kHeight = 1024;
|
||||||
|
constexpr int kPixels = kWidth * kHeight;
|
||||||
|
constexpr int kHistSize = 256;
|
||||||
|
|
||||||
// 方案2:快速百分位数估算(无需排序,采样估算)
|
uint16_t sampleMin = 65535;
|
||||||
// 优点:适应不同环境,画面对比度好,速度快10倍以上
|
uint16_t sampleMax = 0;
|
||||||
uint16_t minVal = 65535, maxVal = 0;
|
for(int i = 0; i < kPixels; i += 8) {
|
||||||
|
const uint16_t val = src[i];
|
||||||
// 第一遍:快速扫描找到粗略范围(每隔8个像素采样)
|
|
||||||
for (int i = 0; i < 1224 * 1024; i += 8) {
|
|
||||||
uint16_t val = src[i];
|
|
||||||
if(val > 0) {
|
if(val > 0) {
|
||||||
if (val < minVal) minVal = val;
|
if(val < sampleMin) sampleMin = val;
|
||||||
if (val > maxVal) maxVal = val;
|
if(val > sampleMax) sampleMax = val;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 第二遍:使用直方图统计精确百分位数(避免排序)
|
float rangeMin = 0.0f;
|
||||||
if (maxVal > minVal) {
|
float rangeMax = 65535.0f;
|
||||||
const int histSize = 256;
|
if(sampleMax > sampleMin) {
|
||||||
int histogram[histSize] = {0};
|
int histogram[kHistSize] = {0};
|
||||||
float binWidth = (maxVal - minVal) / (float)histSize;
|
const float binWidth = qMax(1.0f, (sampleMax - sampleMin) / static_cast<float>(kHistSize));
|
||||||
|
|
||||||
// 构建直方图
|
|
||||||
for (int i = 0; i < 1224 * 1024; i++) {
|
|
||||||
if (src[i] > 0) {
|
|
||||||
int bin = (src[i] - minVal) / binWidth;
|
|
||||||
if (bin >= histSize) bin = histSize - 1;
|
|
||||||
histogram[bin]++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 计算1%和99%百分位数
|
|
||||||
int totalPixels = 0;
|
int totalPixels = 0;
|
||||||
for (int i = 0; i < histSize; i++) totalPixels += histogram[i];
|
for(int i = 0; i < kPixels; ++i) {
|
||||||
|
const uint16_t val = src[i];
|
||||||
int thresh_1 = totalPixels * 0.01;
|
if(val > 0) {
|
||||||
int thresh_99 = totalPixels * 0.99;
|
int bin = static_cast<int>((val - sampleMin) / binWidth);
|
||||||
|
if(bin < 0) bin = 0;
|
||||||
int cumsum = 0;
|
if(bin >= kHistSize) bin = kHistSize - 1;
|
||||||
for (int i = 0; i < histSize; i++) {
|
histogram[bin]++;
|
||||||
cumsum += histogram[i];
|
totalPixels++;
|
||||||
if (cumsum >= thresh_1 && minVal == 65535) {
|
|
||||||
minVal = minVal + i * binWidth;
|
|
||||||
}
|
}
|
||||||
if (cumsum >= thresh_99) {
|
}
|
||||||
maxVal = minVal + i * binWidth;
|
|
||||||
|
if(totalPixels > 0) {
|
||||||
|
const int thresh1 = qMax(1, static_cast<int>(totalPixels * 0.01f));
|
||||||
|
const int thresh99 = qMax(thresh1 + 1, static_cast<int>(totalPixels * 0.99f));
|
||||||
|
|
||||||
|
int p1Bin = 0;
|
||||||
|
int p99Bin = kHistSize - 1;
|
||||||
|
int cumsum = 0;
|
||||||
|
bool p1Found = false;
|
||||||
|
for(int i = 0; i < kHistSize; ++i) {
|
||||||
|
cumsum += histogram[i];
|
||||||
|
if(!p1Found && cumsum >= thresh1) {
|
||||||
|
p1Bin = i;
|
||||||
|
p1Found = true;
|
||||||
|
}
|
||||||
|
if(cumsum >= thresh99) {
|
||||||
|
p99Bin = i;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
float rawMin = sampleMin + p1Bin * binWidth;
|
||||||
|
float rawMax = sampleMin + p99Bin * binWidth;
|
||||||
|
if(rawMax <= rawMin + 1.0f) {
|
||||||
|
rawMax = rawMin + 1.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 创建8位图像并归一化
|
if(!m_leftIrDisplayRangeInited) {
|
||||||
QImage image(1224, 1024, QImage::Format_Grayscale8);
|
m_leftIrDisplayMin = rawMin;
|
||||||
uint8_t* dst = image.bits();
|
m_leftIrDisplayMax = rawMax;
|
||||||
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
|
m_leftIrDisplayRangeInited = true;
|
||||||
|
} else {
|
||||||
|
const float prevMin = m_leftIrDisplayMin;
|
||||||
|
const float prevMax = m_leftIrDisplayMax;
|
||||||
|
const float maxJump = qMax(120.0f, (rawMax - rawMin) * 0.25f);
|
||||||
|
const float minClamped = qBound(prevMin - maxJump, rawMin, prevMin + maxJump);
|
||||||
|
const float maxClamped = qBound(prevMax - maxJump, rawMax, prevMax + maxJump);
|
||||||
|
|
||||||
for (int i = 0; i < 1224 * 1024; i++) {
|
m_leftIrDisplayMin = prevMin * 0.85f + minClamped * 0.15f;
|
||||||
if (src[i] == 0) {
|
m_leftIrDisplayMax = prevMax * 0.85f + maxClamped * 0.15f;
|
||||||
|
if(m_leftIrDisplayMax <= m_leftIrDisplayMin + 32.0f) {
|
||||||
|
m_leftIrDisplayMax = m_leftIrDisplayMin + 32.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
rangeMin = m_leftIrDisplayMin;
|
||||||
|
rangeMax = m_leftIrDisplayMax;
|
||||||
|
}
|
||||||
|
} else if(m_leftIrDisplayRangeInited) {
|
||||||
|
rangeMin = m_leftIrDisplayMin;
|
||||||
|
rangeMax = m_leftIrDisplayMax;
|
||||||
|
}
|
||||||
|
|
||||||
|
QImage image(kWidth, kHeight, QImage::Format_Grayscale8);
|
||||||
|
uint8_t* dst = image.bits();
|
||||||
|
const float scale = (rangeMax > rangeMin) ? (255.0f / (rangeMax - rangeMin)) : 0.0f;
|
||||||
|
|
||||||
|
for(int i = 0; i < kPixels; ++i) {
|
||||||
|
const uint16_t val = src[i];
|
||||||
|
if(val == 0 || val <= rangeMin) {
|
||||||
dst[i] = 0;
|
dst[i] = 0;
|
||||||
} else if (src[i] <= minVal) {
|
} else if(val >= rangeMax) {
|
||||||
dst[i] = 0;
|
|
||||||
} else if (src[i] >= maxVal) {
|
|
||||||
dst[i] = 255;
|
dst[i] = 255;
|
||||||
} else {
|
} else {
|
||||||
dst[i] = static_cast<uint8_t>((src[i] - minVal) * scale);
|
dst[i] = static_cast<uint8_t>((val - rangeMin) * scale);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
imageCopy = image.copy();
|
||||||
|
}
|
||||||
|
|
||||||
QImage imageCopy = image.copy();
|
|
||||||
|
|
||||||
// 在主线程更新UI
|
|
||||||
QMetaObject::invokeMethod(this, [this, imageCopy]() {
|
QMetaObject::invokeMethod(this, [this, imageCopy]() {
|
||||||
if(m_leftImageDisplay) {
|
if(m_leftImageDisplay) {
|
||||||
QPixmap pixmap = QPixmap::fromImage(imageCopy);
|
QPixmap pixmap = QPixmap::fromImage(imageCopy);
|
||||||
@@ -1092,11 +1315,11 @@ void MainWindow::onLeftImageReceived(const QByteArray &jpegData, uint32_t blockI
|
|||||||
}, Qt::QueuedConnection);
|
}, Qt::QueuedConnection);
|
||||||
} catch (const std::exception &e) {
|
} catch (const std::exception &e) {
|
||||||
qDebug() << "[MainWindow] ERROR: Left IR processing exception:" << e.what();
|
qDebug() << "[MainWindow] ERROR: Left IR processing exception:" << e.what();
|
||||||
|
} catch (...) {
|
||||||
|
qDebug() << "[MainWindow] ERROR: Left IR processing unknown exception";
|
||||||
}
|
}
|
||||||
|
m_leftIRProcessing.deref();
|
||||||
});
|
});
|
||||||
} else {
|
|
||||||
qDebug() << "[MainWindow] ERROR: Left IR data size mismatch:" << jpegData.size();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1126,87 +1349,136 @@ void MainWindow::onRightImageReceived(const QByteArray &jpegData, uint32_t block
|
|||||||
m_lastRightFrameTime = currentTime;
|
m_lastRightFrameTime = currentTime;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 使用后台线程处理16位原始数据,避免阻塞UI
|
// 使用后台线程处理红外数据,避免阻塞UI
|
||||||
if(m_rightImageDisplay && jpegData.size() > 0) {
|
if(m_rightImageDisplay && jpegData.size() > 0) {
|
||||||
// 检查数据大小是否为16位图像
|
const size_t size8bit = 612 * 512;
|
||||||
size_t expectedSize = 1224 * 1024 * sizeof(uint16_t);
|
const size_t size16bit = 1224 * 1024 * sizeof(uint16_t);
|
||||||
if (jpegData.size() == expectedSize) {
|
const bool is8bit = (jpegData.size() == size8bit);
|
||||||
// 复制数据到局部变量
|
const bool is16bit = (jpegData.size() == size16bit);
|
||||||
|
if(!is8bit && !is16bit) {
|
||||||
|
qDebug() << "[MainWindow] ERROR: Right IR data size mismatch:" << jpegData.size()
|
||||||
|
<< "(expected 8bit:" << size8bit << "or 16bit:" << size16bit << ")";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 忙时丢帧,避免线程池任务积压导致显示乱序和偶发闪烁。
|
||||||
|
if(m_rightIRProcessing.loadAcquire() > 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
m_rightIRProcessing.ref();
|
||||||
|
|
||||||
QByteArray dataCopy = jpegData;
|
QByteArray dataCopy = jpegData;
|
||||||
|
QtConcurrent::run([this, dataCopy, is16bit]() {
|
||||||
// 在后台线程处理
|
|
||||||
QtConcurrent::run([this, dataCopy]() {
|
|
||||||
try {
|
try {
|
||||||
|
QImage imageCopy;
|
||||||
|
if(!is16bit) {
|
||||||
|
QImage image(reinterpret_cast<const uchar*>(dataCopy.constData()),
|
||||||
|
612, 512, 612, QImage::Format_Grayscale8);
|
||||||
|
imageCopy = image.copy();
|
||||||
|
} else {
|
||||||
const uint16_t* src = reinterpret_cast<const uint16_t*>(dataCopy.constData());
|
const uint16_t* src = reinterpret_cast<const uint16_t*>(dataCopy.constData());
|
||||||
|
constexpr int kWidth = 1224;
|
||||||
|
constexpr int kHeight = 1024;
|
||||||
|
constexpr int kPixels = kWidth * kHeight;
|
||||||
|
constexpr int kHistSize = 256;
|
||||||
|
|
||||||
// 方案2:快速百分位数估算(无需排序,采样估算)
|
uint16_t sampleMin = 65535;
|
||||||
// 优点:适应不同环境,画面对比度好,速度快10倍以上
|
uint16_t sampleMax = 0;
|
||||||
uint16_t minVal = 65535, maxVal = 0;
|
for(int i = 0; i < kPixels; i += 8) {
|
||||||
|
const uint16_t val = src[i];
|
||||||
// 第一遍:快速扫描找到粗略范围(每隔8个像素采样)
|
|
||||||
for (int i = 0; i < 1224 * 1024; i += 8) {
|
|
||||||
uint16_t val = src[i];
|
|
||||||
if(val > 0) {
|
if(val > 0) {
|
||||||
if (val < minVal) minVal = val;
|
if(val < sampleMin) sampleMin = val;
|
||||||
if (val > maxVal) maxVal = val;
|
if(val > sampleMax) sampleMax = val;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 第二遍:使用直方图统计精确百分位数(避免排序)
|
float rangeMin = 0.0f;
|
||||||
if (maxVal > minVal) {
|
float rangeMax = 65535.0f;
|
||||||
const int histSize = 256;
|
if(sampleMax > sampleMin) {
|
||||||
int histogram[histSize] = {0};
|
int histogram[kHistSize] = {0};
|
||||||
float binWidth = (maxVal - minVal) / (float)histSize;
|
const float binWidth = qMax(1.0f, (sampleMax - sampleMin) / static_cast<float>(kHistSize));
|
||||||
|
|
||||||
// 构建直方图
|
|
||||||
for (int i = 0; i < 1224 * 1024; i++) {
|
|
||||||
if (src[i] > 0) {
|
|
||||||
int bin = (src[i] - minVal) / binWidth;
|
|
||||||
if (bin >= histSize) bin = histSize - 1;
|
|
||||||
histogram[bin]++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 计算1%和99%百分位数
|
|
||||||
int totalPixels = 0;
|
int totalPixels = 0;
|
||||||
for (int i = 0; i < histSize; i++) totalPixels += histogram[i];
|
for(int i = 0; i < kPixels; ++i) {
|
||||||
|
const uint16_t val = src[i];
|
||||||
int thresh_1 = totalPixels * 0.01;
|
if(val > 0) {
|
||||||
int thresh_99 = totalPixels * 0.99;
|
int bin = static_cast<int>((val - sampleMin) / binWidth);
|
||||||
|
if(bin < 0) bin = 0;
|
||||||
int cumsum = 0;
|
if(bin >= kHistSize) bin = kHistSize - 1;
|
||||||
for (int i = 0; i < histSize; i++) {
|
histogram[bin]++;
|
||||||
cumsum += histogram[i];
|
totalPixels++;
|
||||||
if (cumsum >= thresh_1 && minVal == 65535) {
|
|
||||||
minVal = minVal + i * binWidth;
|
|
||||||
}
|
}
|
||||||
if (cumsum >= thresh_99) {
|
}
|
||||||
maxVal = minVal + i * binWidth;
|
|
||||||
|
if(totalPixels > 0) {
|
||||||
|
const int thresh1 = qMax(1, static_cast<int>(totalPixels * 0.01f));
|
||||||
|
const int thresh99 = qMax(thresh1 + 1, static_cast<int>(totalPixels * 0.99f));
|
||||||
|
|
||||||
|
int p1Bin = 0;
|
||||||
|
int p99Bin = kHistSize - 1;
|
||||||
|
int cumsum = 0;
|
||||||
|
bool p1Found = false;
|
||||||
|
for(int i = 0; i < kHistSize; ++i) {
|
||||||
|
cumsum += histogram[i];
|
||||||
|
if(!p1Found && cumsum >= thresh1) {
|
||||||
|
p1Bin = i;
|
||||||
|
p1Found = true;
|
||||||
|
}
|
||||||
|
if(cumsum >= thresh99) {
|
||||||
|
p99Bin = i;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
float rawMin = sampleMin + p1Bin * binWidth;
|
||||||
|
float rawMax = sampleMin + p99Bin * binWidth;
|
||||||
|
if(rawMax <= rawMin + 1.0f) {
|
||||||
|
rawMax = rawMin + 1.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 创建8位图像并归一化
|
if(!m_rightIrDisplayRangeInited) {
|
||||||
QImage image(1224, 1024, QImage::Format_Grayscale8);
|
m_rightIrDisplayMin = rawMin;
|
||||||
uint8_t* dst = image.bits();
|
m_rightIrDisplayMax = rawMax;
|
||||||
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
|
m_rightIrDisplayRangeInited = true;
|
||||||
|
} else {
|
||||||
|
const float prevMin = m_rightIrDisplayMin;
|
||||||
|
const float prevMax = m_rightIrDisplayMax;
|
||||||
|
const float maxJump = qMax(120.0f, (rawMax - rawMin) * 0.25f);
|
||||||
|
const float minClamped = qBound(prevMin - maxJump, rawMin, prevMin + maxJump);
|
||||||
|
const float maxClamped = qBound(prevMax - maxJump, rawMax, prevMax + maxJump);
|
||||||
|
|
||||||
for (int i = 0; i < 1224 * 1024; i++) {
|
m_rightIrDisplayMin = prevMin * 0.85f + minClamped * 0.15f;
|
||||||
if (src[i] == 0) {
|
m_rightIrDisplayMax = prevMax * 0.85f + maxClamped * 0.15f;
|
||||||
|
if(m_rightIrDisplayMax <= m_rightIrDisplayMin + 32.0f) {
|
||||||
|
m_rightIrDisplayMax = m_rightIrDisplayMin + 32.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
rangeMin = m_rightIrDisplayMin;
|
||||||
|
rangeMax = m_rightIrDisplayMax;
|
||||||
|
}
|
||||||
|
} else if(m_rightIrDisplayRangeInited) {
|
||||||
|
rangeMin = m_rightIrDisplayMin;
|
||||||
|
rangeMax = m_rightIrDisplayMax;
|
||||||
|
}
|
||||||
|
|
||||||
|
QImage image(kWidth, kHeight, QImage::Format_Grayscale8);
|
||||||
|
uint8_t* dst = image.bits();
|
||||||
|
const float scale = (rangeMax > rangeMin) ? (255.0f / (rangeMax - rangeMin)) : 0.0f;
|
||||||
|
|
||||||
|
for(int i = 0; i < kPixels; ++i) {
|
||||||
|
const uint16_t val = src[i];
|
||||||
|
if(val == 0 || val <= rangeMin) {
|
||||||
dst[i] = 0;
|
dst[i] = 0;
|
||||||
} else if (src[i] <= minVal) {
|
} else if(val >= rangeMax) {
|
||||||
dst[i] = 0;
|
|
||||||
} else if (src[i] >= maxVal) {
|
|
||||||
dst[i] = 255;
|
dst[i] = 255;
|
||||||
} else {
|
} else {
|
||||||
dst[i] = static_cast<uint8_t>((src[i] - minVal) * scale);
|
dst[i] = static_cast<uint8_t>((val - rangeMin) * scale);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
imageCopy = image.copy();
|
||||||
|
}
|
||||||
|
|
||||||
QImage imageCopy = image.copy();
|
|
||||||
|
|
||||||
// 在主线程更新UI
|
|
||||||
QMetaObject::invokeMethod(this, [this, imageCopy]() {
|
QMetaObject::invokeMethod(this, [this, imageCopy]() {
|
||||||
if(m_rightImageDisplay) {
|
if(m_rightImageDisplay) {
|
||||||
QPixmap pixmap = QPixmap::fromImage(imageCopy);
|
QPixmap pixmap = QPixmap::fromImage(imageCopy);
|
||||||
@@ -1216,11 +1488,11 @@ void MainWindow::onRightImageReceived(const QByteArray &jpegData, uint32_t block
|
|||||||
}, Qt::QueuedConnection);
|
}, Qt::QueuedConnection);
|
||||||
} catch (const std::exception &e) {
|
} catch (const std::exception &e) {
|
||||||
qDebug() << "[MainWindow] ERROR: Right IR processing exception:" << e.what();
|
qDebug() << "[MainWindow] ERROR: Right IR processing exception:" << e.what();
|
||||||
|
} catch (...) {
|
||||||
|
qDebug() << "[MainWindow] ERROR: Right IR processing unknown exception";
|
||||||
}
|
}
|
||||||
|
m_rightIRProcessing.deref();
|
||||||
});
|
});
|
||||||
} else {
|
|
||||||
qDebug() << "[MainWindow] ERROR: Right IR data size mismatch:" << jpegData.size();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1317,21 +1589,57 @@ void MainWindow::onRgbImageReceived(const QByteArray &jpegData, uint32_t blockId
|
|||||||
|
|
||||||
void MainWindow::onDepthDataReceived(const QByteArray &depthData, uint32_t blockId)
|
void MainWindow::onDepthDataReceived(const QByteArray &depthData, uint32_t blockId)
|
||||||
{
|
{
|
||||||
// 实时处理每一帧
|
// 点云处理忙时直接丢弃新帧,避免任务堆积拖垮线程池和UI响应。
|
||||||
// 注释掉频繁的日志输出
|
if(m_pointCloudProcessing.loadAcquire() > 0) {
|
||||||
// qDebug() << "Depth data received: Block" << blockId << "Size:" << depthData.size() << "bytes";
|
int dropped = m_pointCloudDropCounter.fetchAndAddRelaxed(1) + 1;
|
||||||
|
if((dropped % 60) == 0) {
|
||||||
|
qDebug() << "[MainWindow] Point cloud(depth) busy, dropped frames:" << dropped;
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// 调用PointCloudProcessor进行OpenCL计算
|
m_pointCloudProcessing.ref();
|
||||||
m_pointCloudProcessor->processDepthData(depthData, blockId);
|
QByteArray dataCopy = depthData;
|
||||||
|
QtConcurrent::run([this, dataCopy, blockId]() {
|
||||||
|
try {
|
||||||
|
if(m_pointCloudProcessor) {
|
||||||
|
m_pointCloudProcessor->processDepthData(dataCopy, blockId);
|
||||||
|
}
|
||||||
|
} catch (const std::exception &e) {
|
||||||
|
qDebug() << "[MainWindow] ERROR: Depth point cloud process exception:" << e.what();
|
||||||
|
} catch (...) {
|
||||||
|
qDebug() << "[MainWindow] ERROR: Depth point cloud process unknown exception";
|
||||||
|
}
|
||||||
|
m_pointCloudProcessing.deref();
|
||||||
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
void MainWindow::onPointCloudDataReceived(const QByteArray &cloudData, uint32_t blockId)
|
void MainWindow::onPointCloudDataReceived(const QByteArray &cloudData, uint32_t blockId)
|
||||||
{
|
{
|
||||||
// qDebug() << "[MainWindow] Point cloud data received: Block" << blockId << "Size:" << cloudData.size() << "bytes";
|
// qDebug() << "[MainWindow] Point cloud data received: Block" << blockId << "Size:" << cloudData.size() << "bytes";
|
||||||
|
|
||||||
// 使用QtConcurrent在后台线程处理点云数据
|
// 点云处理忙时直接丢弃新帧,避免任务堆积拖垮线程池和UI响应。
|
||||||
QtConcurrent::run([this, cloudData, blockId]() {
|
if(m_pointCloudProcessing.loadAcquire() > 0) {
|
||||||
m_pointCloudProcessor->processPointCloudData(cloudData, blockId);
|
int dropped = m_pointCloudDropCounter.fetchAndAddRelaxed(1) + 1;
|
||||||
|
if((dropped % 60) == 0) {
|
||||||
|
qDebug() << "[MainWindow] Point cloud(z/xyz) busy, dropped frames:" << dropped;
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_pointCloudProcessing.ref();
|
||||||
|
QByteArray dataCopy = cloudData;
|
||||||
|
QtConcurrent::run([this, dataCopy, blockId]() {
|
||||||
|
try {
|
||||||
|
if(m_pointCloudProcessor) {
|
||||||
|
m_pointCloudProcessor->processPointCloudData(dataCopy, blockId);
|
||||||
|
}
|
||||||
|
} catch (const std::exception &e) {
|
||||||
|
qDebug() << "[MainWindow] ERROR: Point cloud process exception:" << e.what();
|
||||||
|
} catch (...) {
|
||||||
|
qDebug() << "[MainWindow] ERROR: Point cloud process unknown exception";
|
||||||
|
}
|
||||||
|
m_pointCloudProcessing.deref();
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1529,74 +1837,100 @@ void MainWindow::performBackgroundSave(const QString &saveDir, const QString &ba
|
|||||||
bool rightIRSuccess = false;
|
bool rightIRSuccess = false;
|
||||||
bool rgbSuccess = false;
|
bool rgbSuccess = false;
|
||||||
|
|
||||||
// 保存左红外图像(16位原始数据,1224×1024)
|
// 保存左红外图像(支持16位原始1224×1024或8位下采样612×512)
|
||||||
if(!leftIRData.isEmpty()) {
|
if(!leftIRData.isEmpty()) {
|
||||||
try {
|
try {
|
||||||
size_t expectedSize = 1224 * 1024 * sizeof(uint16_t);
|
size_t size16bit = 1224 * 1024 * sizeof(uint16_t);
|
||||||
if (leftIRData.size() == expectedSize) {
|
size_t size8bit = 612 * 512;
|
||||||
|
|
||||||
|
if(leftIRData.size() == size16bit) {
|
||||||
const uint16_t* src = reinterpret_cast<const uint16_t*>(leftIRData.constData());
|
const uint16_t* src = reinterpret_cast<const uint16_t*>(leftIRData.constData());
|
||||||
|
|
||||||
// 创建16位灰度图像
|
|
||||||
cv::Mat leftIR16(1024, 1224, CV_16UC1);
|
cv::Mat leftIR16(1024, 1224, CV_16UC1);
|
||||||
memcpy(leftIR16.data, src, expectedSize);
|
memcpy(leftIR16.data, src, size16bit);
|
||||||
|
|
||||||
// 根据depthFormat参数保存
|
|
||||||
if(depthFormat == "png" || depthFormat == "both") {
|
if(depthFormat == "png" || depthFormat == "both") {
|
||||||
// 保存PNG格式(8位)
|
|
||||||
QString pngPath = QString("%1/%2_left_ir.png").arg(saveDir).arg(baseName);
|
QString pngPath = QString("%1/%2_left_ir.png").arg(saveDir).arg(baseName);
|
||||||
cv::Mat leftIR8;
|
cv::Mat leftIR8;
|
||||||
leftIR16.convertTo(leftIR8, CV_8UC1, 255.0 / 65535.0);
|
leftIR16.convertTo(leftIR8, CV_8UC1, 255.0 / 65535.0);
|
||||||
cv::imwrite(pngPath.toStdString(), leftIR8);
|
cv::imwrite(pngPath.toStdString(), leftIR8);
|
||||||
qDebug() << "保存左红外PNG图像:" << pngPath;
|
qDebug() << "保存左红外PNG图像(16bit):" << pngPath;
|
||||||
leftIRSuccess = true;
|
leftIRSuccess = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(depthFormat == "tiff" || depthFormat == "both") {
|
if(depthFormat == "tiff" || depthFormat == "both") {
|
||||||
// 保存TIFF格式(保留16位精度)
|
|
||||||
QString tiffPath = QString("%1/%2_left_ir.tiff").arg(saveDir).arg(baseName);
|
QString tiffPath = QString("%1/%2_left_ir.tiff").arg(saveDir).arg(baseName);
|
||||||
cv::imwrite(tiffPath.toStdString(), leftIR16);
|
cv::imwrite(tiffPath.toStdString(), leftIR16);
|
||||||
qDebug() << "保存左红外TIFF图像(16位):" << tiffPath;
|
qDebug() << "保存左红外TIFF图像(16位):" << tiffPath;
|
||||||
leftIRSuccess = true;
|
leftIRSuccess = true;
|
||||||
}
|
}
|
||||||
|
} else if(leftIRData.size() == size8bit) {
|
||||||
|
cv::Mat leftIR8(512, 612, CV_8UC1, const_cast<char*>(leftIRData.constData()));
|
||||||
|
cv::Mat leftIR8Clone = leftIR8.clone();
|
||||||
|
|
||||||
|
if(depthFormat == "png" || depthFormat == "both") {
|
||||||
|
QString pngPath = QString("%1/%2_left_ir.png").arg(saveDir).arg(baseName);
|
||||||
|
cv::imwrite(pngPath.toStdString(), leftIR8Clone);
|
||||||
|
qDebug() << "保存左红外PNG图像(8bit下采样):" << pngPath;
|
||||||
|
leftIRSuccess = true;
|
||||||
|
}
|
||||||
|
if(depthFormat == "tiff" || depthFormat == "both") {
|
||||||
|
QString tiffPath = QString("%1/%2_left_ir.tiff").arg(saveDir).arg(baseName);
|
||||||
|
cv::imwrite(tiffPath.toStdString(), leftIR8Clone);
|
||||||
|
qDebug() << "保存左红外TIFF图像(8bit下采样):" << tiffPath;
|
||||||
|
leftIRSuccess = true;
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
qDebug() << "左红外数据大小不匹配:" << leftIRData.size();
|
qDebug() << "左红外数据大小不匹配:" << leftIRData.size()
|
||||||
|
<< "(期望16bit:" << size16bit << "或8bit:" << size8bit << ")";
|
||||||
}
|
}
|
||||||
} catch (const std::exception &e) {
|
} catch (const std::exception &e) {
|
||||||
qDebug() << "保存左红外图像失败:" << e.what();
|
qDebug() << "保存左红外图像失败:" << e.what();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 保存右红外图像(16位原始数据,1224×1024)
|
// 保存右红外图像(支持16位原始1224×1024或8位下采样612×512)
|
||||||
if(!rightIRData.isEmpty()) {
|
if(!rightIRData.isEmpty()) {
|
||||||
try {
|
try {
|
||||||
size_t expectedSize = 1224 * 1024 * sizeof(uint16_t);
|
size_t size16bit = 1224 * 1024 * sizeof(uint16_t);
|
||||||
if (rightIRData.size() == expectedSize) {
|
size_t size8bit = 612 * 512;
|
||||||
|
|
||||||
|
if(rightIRData.size() == size16bit) {
|
||||||
const uint16_t* src = reinterpret_cast<const uint16_t*>(rightIRData.constData());
|
const uint16_t* src = reinterpret_cast<const uint16_t*>(rightIRData.constData());
|
||||||
|
|
||||||
// 创建16位灰度图像
|
|
||||||
cv::Mat rightIR16(1024, 1224, CV_16UC1);
|
cv::Mat rightIR16(1024, 1224, CV_16UC1);
|
||||||
memcpy(rightIR16.data, src, expectedSize);
|
memcpy(rightIR16.data, src, size16bit);
|
||||||
|
|
||||||
// 根据depthFormat参数保存
|
|
||||||
if(depthFormat == "png" || depthFormat == "both") {
|
if(depthFormat == "png" || depthFormat == "both") {
|
||||||
// 保存PNG格式(8位)
|
|
||||||
QString pngPath = QString("%1/%2_right_ir.png").arg(saveDir).arg(baseName);
|
QString pngPath = QString("%1/%2_right_ir.png").arg(saveDir).arg(baseName);
|
||||||
cv::Mat rightIR8;
|
cv::Mat rightIR8;
|
||||||
rightIR16.convertTo(rightIR8, CV_8UC1, 255.0 / 65535.0);
|
rightIR16.convertTo(rightIR8, CV_8UC1, 255.0 / 65535.0);
|
||||||
cv::imwrite(pngPath.toStdString(), rightIR8);
|
cv::imwrite(pngPath.toStdString(), rightIR8);
|
||||||
qDebug() << "保存右红外PNG图像:" << pngPath;
|
qDebug() << "保存右红外PNG图像(16bit):" << pngPath;
|
||||||
rightIRSuccess = true;
|
rightIRSuccess = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(depthFormat == "tiff" || depthFormat == "both") {
|
if(depthFormat == "tiff" || depthFormat == "both") {
|
||||||
// 保存TIFF格式(保留16位精度)
|
|
||||||
QString tiffPath = QString("%1/%2_right_ir.tiff").arg(saveDir).arg(baseName);
|
QString tiffPath = QString("%1/%2_right_ir.tiff").arg(saveDir).arg(baseName);
|
||||||
cv::imwrite(tiffPath.toStdString(), rightIR16);
|
cv::imwrite(tiffPath.toStdString(), rightIR16);
|
||||||
qDebug() << "保存右红外TIFF图像(16位):" << tiffPath;
|
qDebug() << "保存右红外TIFF图像(16位):" << tiffPath;
|
||||||
rightIRSuccess = true;
|
rightIRSuccess = true;
|
||||||
}
|
}
|
||||||
|
} else if(rightIRData.size() == size8bit) {
|
||||||
|
cv::Mat rightIR8(512, 612, CV_8UC1, const_cast<char*>(rightIRData.constData()));
|
||||||
|
cv::Mat rightIR8Clone = rightIR8.clone();
|
||||||
|
|
||||||
|
if(depthFormat == "png" || depthFormat == "both") {
|
||||||
|
QString pngPath = QString("%1/%2_right_ir.png").arg(saveDir).arg(baseName);
|
||||||
|
cv::imwrite(pngPath.toStdString(), rightIR8Clone);
|
||||||
|
qDebug() << "保存右红外PNG图像(8bit下采样):" << pngPath;
|
||||||
|
rightIRSuccess = true;
|
||||||
|
}
|
||||||
|
if(depthFormat == "tiff" || depthFormat == "both") {
|
||||||
|
QString tiffPath = QString("%1/%2_right_ir.tiff").arg(saveDir).arg(baseName);
|
||||||
|
cv::imwrite(tiffPath.toStdString(), rightIR8Clone);
|
||||||
|
qDebug() << "保存右红外TIFF图像(8bit下采样):" << tiffPath;
|
||||||
|
rightIRSuccess = true;
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
qDebug() << "右红外数据大小不匹配:" << rightIRData.size();
|
qDebug() << "右红外数据大小不匹配:" << rightIRData.size()
|
||||||
|
<< "(期望16bit:" << size16bit << "或8bit:" << size8bit << ")";
|
||||||
}
|
}
|
||||||
} catch (const std::exception &e) {
|
} catch (const std::exception &e) {
|
||||||
qDebug() << "保存右红外图像失败:" << e.what();
|
qDebug() << "保存右红外图像失败:" << e.what();
|
||||||
@@ -1780,7 +2114,7 @@ void MainWindow::onClearLogClicked()
|
|||||||
void MainWindow::onSaveLogClicked()
|
void MainWindow::onSaveLogClicked()
|
||||||
{
|
{
|
||||||
QString fileName = QFileDialog::getSaveFileName(this, "保存日志",
|
QString fileName = QFileDialog::getSaveFileName(this, "保存日志",
|
||||||
QStandardPaths::writableLocation(QStandardPaths::DocumentsLocation) + "/d330viewer_log.txt",
|
QStandardPaths::writableLocation(QStandardPaths::DocumentsLocation) + "/viewer_log.txt",
|
||||||
"文本文件 (*.txt);;所有文件 (*.*)");
|
"文本文件 (*.txt);;所有文件 (*.*)");
|
||||||
if(!fileName.isEmpty()) {
|
if(!fileName.isEmpty()) {
|
||||||
QFile file(fileName);
|
QFile file(fileName);
|
||||||
|
|||||||
@@ -140,6 +140,8 @@ private:
|
|||||||
QPushButton *m_leftIRToggle;
|
QPushButton *m_leftIRToggle;
|
||||||
QPushButton *m_rightIRToggle;
|
QPushButton *m_rightIRToggle;
|
||||||
QPushButton *m_rgbToggle;
|
QPushButton *m_rgbToggle;
|
||||||
|
QPushButton *m_pointCloudColorToggle; // 点云颜色开关
|
||||||
|
QPushButton *m_pointCloudDenoiseToggle; // Point cloud denoise toggle
|
||||||
|
|
||||||
// 单目/双目模式切换按钮
|
// 单目/双目模式切换按钮
|
||||||
QPushButton *m_monocularBtn;
|
QPushButton *m_monocularBtn;
|
||||||
@@ -156,6 +158,12 @@ private:
|
|||||||
QPushButton *m_browseSavePathBtn;
|
QPushButton *m_browseSavePathBtn;
|
||||||
class QComboBox *m_depthFormatCombo;
|
class QComboBox *m_depthFormatCombo;
|
||||||
class QComboBox *m_pointCloudFormatCombo;
|
class QComboBox *m_pointCloudFormatCombo;
|
||||||
|
QSlider *m_denoiseSupportSlider;
|
||||||
|
QSpinBox *m_denoiseSupportSpinBox;
|
||||||
|
QSlider *m_denoiseTailSlider;
|
||||||
|
QSpinBox *m_denoiseTailSpinBox;
|
||||||
|
QSlider *m_denoiseBandSlider;
|
||||||
|
QSpinBox *m_denoiseBandSpinBox;
|
||||||
|
|
||||||
// 显示控件
|
// 显示控件
|
||||||
QLabel *m_statusLabel;
|
QLabel *m_statusLabel;
|
||||||
@@ -207,8 +215,18 @@ private:
|
|||||||
int m_totalRgbFrameCount;
|
int m_totalRgbFrameCount;
|
||||||
double m_currentRgbFps;
|
double m_currentRgbFps;
|
||||||
|
|
||||||
// RGB解码处理标志(防止线程积压)
|
// 解码处理标志(防止线程积压导致闪烁)
|
||||||
QAtomicInt m_rgbProcessing;
|
QAtomicInt m_rgbProcessing;
|
||||||
|
QAtomicInt m_leftIRProcessing;
|
||||||
|
QAtomicInt m_rightIRProcessing;
|
||||||
|
QAtomicInt m_pointCloudProcessing;
|
||||||
|
QAtomicInt m_pointCloudDropCounter;
|
||||||
|
bool m_leftIrDisplayRangeInited;
|
||||||
|
float m_leftIrDisplayMin;
|
||||||
|
float m_leftIrDisplayMax;
|
||||||
|
bool m_rightIrDisplayRangeInited;
|
||||||
|
float m_rightIrDisplayMin;
|
||||||
|
float m_rightIrDisplayMax;
|
||||||
int m_rgbSkipCounter; // RGB帧跳过计数器
|
int m_rgbSkipCounter; // RGB帧跳过计数器
|
||||||
|
|
||||||
// 相机启用状态标志(防止关闭后闪烁)
|
// 相机启用状态标志(防止关闭后闪烁)
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
#include "gui/PointCloudGLWidget.h"
|
#include "gui/PointCloudGLWidget.h"
|
||||||
#include <QDebug>
|
#include <QDebug>
|
||||||
|
#include <QPushButton>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <cfloat>
|
#include <cfloat>
|
||||||
|
|
||||||
@@ -9,16 +10,32 @@ PointCloudGLWidget::PointCloudGLWidget(QWidget *parent)
|
|||||||
, m_vertexBuffer(nullptr)
|
, m_vertexBuffer(nullptr)
|
||||||
, m_vao(nullptr)
|
, m_vao(nullptr)
|
||||||
, m_pointCount(0)
|
, m_pointCount(0)
|
||||||
, m_fixedCenter(0.0f, 0.0f, 0.0f)
|
, m_minZ(0.0f)
|
||||||
, m_centerInitialized(false)
|
, m_maxZ(1000.0f)
|
||||||
, m_orthoSize(2000.0f) // 正交投影视野大小
|
, m_fov(60.0f) // 透视投影视场角
|
||||||
, m_rotationX(0.0f) // 从正面看(0度)
|
, m_rotationX(0.0f) // 从正面看(0度)
|
||||||
, m_rotationY(0.0f) // 不旋转Y轴
|
, m_rotationY(0.0f) // 不旋转Y轴
|
||||||
, m_translation(0.0f, 0.0f, 0.0f)
|
, m_cloudCenter(0.0f, 0.0f, 0.0f)
|
||||||
|
, m_viewDistance(1000.0f)
|
||||||
|
, m_panOffset(0.0f, 0.0f, 0.0f)
|
||||||
|
, m_zoom(1.0f) // 缩放因子
|
||||||
, m_leftButtonPressed(false)
|
, m_leftButtonPressed(false)
|
||||||
, m_rightButtonPressed(false)
|
, m_rightButtonPressed(false)
|
||||||
|
, m_firstFrame(true)
|
||||||
|
, m_colorMode(0) // 默认黑白模式
|
||||||
{
|
{
|
||||||
setMinimumSize(400, 400);
|
setMinimumSize(400, 400);
|
||||||
|
|
||||||
|
// 添加重置视角按钮
|
||||||
|
QPushButton *resetBtn = new QPushButton("重置", this);
|
||||||
|
resetBtn->setFixedSize(60, 30);
|
||||||
|
resetBtn->move(10, 10);
|
||||||
|
resetBtn->setStyleSheet(
|
||||||
|
"QPushButton { background-color: rgba(50, 50, 50, 180); color: white; border: 1px solid #555; border-radius: 4px; }"
|
||||||
|
"QPushButton:hover { background-color: rgba(70, 70, 70, 200); }"
|
||||||
|
"QPushButton:pressed { background-color: rgba(40, 40, 40, 220); }"
|
||||||
|
);
|
||||||
|
connect(resetBtn, &QPushButton::clicked, this, &PointCloudGLWidget::resetView);
|
||||||
}
|
}
|
||||||
|
|
||||||
PointCloudGLWidget::~PointCloudGLWidget()
|
PointCloudGLWidget::~PointCloudGLWidget()
|
||||||
@@ -42,7 +59,7 @@ void PointCloudGLWidget::initializeGL()
|
|||||||
{
|
{
|
||||||
initializeOpenGLFunctions();
|
initializeOpenGLFunctions();
|
||||||
|
|
||||||
glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
|
glClearColor(0.1f, 0.1f, 0.15f, 1.0f); // 深灰色背景
|
||||||
glEnable(GL_DEPTH_TEST);
|
glEnable(GL_DEPTH_TEST);
|
||||||
glEnable(GL_PROGRAM_POINT_SIZE);
|
glEnable(GL_PROGRAM_POINT_SIZE);
|
||||||
|
|
||||||
@@ -67,18 +84,39 @@ void PointCloudGLWidget::setupShaders()
|
|||||||
#version 330 core
|
#version 330 core
|
||||||
layout(location = 0) in vec3 position;
|
layout(location = 0) in vec3 position;
|
||||||
uniform mat4 mvp;
|
uniform mat4 mvp;
|
||||||
|
uniform float minZ;
|
||||||
|
uniform float maxZ;
|
||||||
|
out float depth;
|
||||||
void main() {
|
void main() {
|
||||||
gl_Position = mvp * vec4(position, 1.0);
|
gl_Position = mvp * vec4(position, 1.0);
|
||||||
gl_PointSize = 1.0; // 减小点的大小
|
gl_PointSize = 1.0;
|
||||||
|
depth = (position.z - minZ) / (maxZ - minZ);
|
||||||
}
|
}
|
||||||
)";
|
)";
|
||||||
|
|
||||||
// 片段着色器
|
// 片段着色器 - 支持黑白和彩色两种模式
|
||||||
const char *fragmentShaderSource = R"(
|
const char *fragmentShaderSource = R"(
|
||||||
#version 330 core
|
#version 330 core
|
||||||
|
in float depth;
|
||||||
|
uniform int colorMode;
|
||||||
out vec4 fragColor;
|
out vec4 fragColor;
|
||||||
void main() {
|
void main() {
|
||||||
|
float d = clamp(depth, 0.0, 1.0);
|
||||||
|
if (colorMode == 0) {
|
||||||
fragColor = vec4(1.0, 1.0, 1.0, 1.0);
|
fragColor = vec4(1.0, 1.0, 1.0, 1.0);
|
||||||
|
} else {
|
||||||
|
vec3 color;
|
||||||
|
if (d < 0.25) {
|
||||||
|
color = mix(vec3(0.0, 0.0, 1.0), vec3(0.0, 1.0, 1.0), d * 4.0);
|
||||||
|
} else if (d < 0.5) {
|
||||||
|
color = mix(vec3(0.0, 1.0, 1.0), vec3(0.0, 1.0, 0.0), (d - 0.25) * 4.0);
|
||||||
|
} else if (d < 0.75) {
|
||||||
|
color = mix(vec3(0.0, 1.0, 0.0), vec3(1.0, 1.0, 0.0), (d - 0.5) * 4.0);
|
||||||
|
} else {
|
||||||
|
color = mix(vec3(1.0, 1.0, 0.0), vec3(1.0, 0.0, 0.0), (d - 0.75) * 4.0);
|
||||||
|
}
|
||||||
|
fragColor = vec4(color, 1.0);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
)";
|
)";
|
||||||
|
|
||||||
@@ -99,11 +137,12 @@ void PointCloudGLWidget::resizeGL(int w, int h)
|
|||||||
{
|
{
|
||||||
m_projection.setToIdentity();
|
m_projection.setToIdentity();
|
||||||
|
|
||||||
// 使用正交投影代替透视投影,避免"喷射状"效果
|
// 使用正交投影,避免透视变形
|
||||||
float aspect = float(w) / float(h);
|
float aspect = float(w) / float(h);
|
||||||
m_projection.ortho(-m_orthoSize * aspect, m_orthoSize * aspect,
|
float orthoSize = m_viewDistance * 0.5f / m_zoom;
|
||||||
-m_orthoSize, m_orthoSize,
|
m_projection.ortho(-orthoSize * aspect, orthoSize * aspect,
|
||||||
-50000.0f, 50000.0f); // 近平面和远平面
|
-orthoSize, orthoSize,
|
||||||
|
-50000.0f, 50000.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PointCloudGLWidget::paintGL()
|
void PointCloudGLWidget::paintGL()
|
||||||
@@ -114,18 +153,25 @@ void PointCloudGLWidget::paintGL()
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 每帧重新计算正交投影矩阵(确保使用最新的m_orthoSize)
|
// 重新计算正交投影矩阵
|
||||||
m_projection.setToIdentity();
|
m_projection.setToIdentity();
|
||||||
float aspect = float(width()) / float(height());
|
float aspect = float(width()) / float(height());
|
||||||
m_projection.ortho(-m_orthoSize * aspect, m_orthoSize * aspect,
|
float orthoSize = m_viewDistance * 0.5f / m_zoom;
|
||||||
-m_orthoSize, m_orthoSize,
|
m_projection.ortho(-orthoSize * aspect, orthoSize * aspect,
|
||||||
|
-orthoSize, orthoSize,
|
||||||
-50000.0f, 50000.0f);
|
-50000.0f, 50000.0f);
|
||||||
|
|
||||||
// 设置view矩阵
|
// 设置view矩阵 - 轨道相机模式(围绕点云中心旋转)
|
||||||
m_view.setToIdentity();
|
m_view.setToIdentity();
|
||||||
|
// 1. 用户平移偏移
|
||||||
|
m_view.translate(m_panOffset);
|
||||||
|
// 2. 相机后退到观察距离
|
||||||
|
m_view.translate(0.0f, 0.0f, -m_viewDistance);
|
||||||
|
// 3. 应用旋转(围绕原点,即点云中心)
|
||||||
m_view.rotate(m_rotationX, 1.0f, 0.0f, 0.0f);
|
m_view.rotate(m_rotationX, 1.0f, 0.0f, 0.0f);
|
||||||
m_view.rotate(m_rotationY, 0.0f, 1.0f, 0.0f);
|
m_view.rotate(m_rotationY, 0.0f, 1.0f, 0.0f);
|
||||||
m_view.translate(m_translation);
|
// 4. 将点云中心移到原点
|
||||||
|
m_view.translate(-m_cloudCenter);
|
||||||
|
|
||||||
// 设置model矩阵
|
// 设置model矩阵
|
||||||
m_model.setToIdentity();
|
m_model.setToIdentity();
|
||||||
@@ -136,6 +182,9 @@ void PointCloudGLWidget::paintGL()
|
|||||||
// 绑定shader和设置uniform
|
// 绑定shader和设置uniform
|
||||||
m_program->bind();
|
m_program->bind();
|
||||||
m_program->setUniformValue("mvp", mvp);
|
m_program->setUniformValue("mvp", mvp);
|
||||||
|
m_program->setUniformValue("minZ", m_minZ);
|
||||||
|
m_program->setUniformValue("maxZ", m_maxZ);
|
||||||
|
m_program->setUniformValue("colorMode", m_colorMode);
|
||||||
|
|
||||||
// 绑定VAO和绘制
|
// 绑定VAO和绘制
|
||||||
m_vao->bind();
|
m_vao->bind();
|
||||||
@@ -166,10 +215,10 @@ void PointCloudGLWidget::mouseMoveEvent(QMouseEvent *event)
|
|||||||
m_rotationY += delta.x() * 0.5f;
|
m_rotationY += delta.x() * 0.5f;
|
||||||
update();
|
update();
|
||||||
} else if (m_rightButtonPressed) {
|
} else if (m_rightButtonPressed) {
|
||||||
// 右键:平移(根据正交投影视野大小调整平移速度)
|
// 右键:平移(根据观察距离调整平移速度)
|
||||||
float scale = m_orthoSize * 0.002f;
|
float scale = m_viewDistance * 0.002f;
|
||||||
m_translation.setX(m_translation.x() + delta.x() * scale);
|
m_panOffset.setX(m_panOffset.x() + delta.x() * scale);
|
||||||
m_translation.setY(m_translation.y() - delta.y() * scale);
|
m_panOffset.setY(m_panOffset.y() - delta.y() * scale);
|
||||||
update();
|
update();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -185,12 +234,12 @@ void PointCloudGLWidget::mouseReleaseEvent(QMouseEvent *event)
|
|||||||
|
|
||||||
void PointCloudGLWidget::wheelEvent(QWheelEvent *event)
|
void PointCloudGLWidget::wheelEvent(QWheelEvent *event)
|
||||||
{
|
{
|
||||||
// 滚轮:缩放(调整正交投影视野大小)
|
// 滚轮:缩放(调整zoom因子)
|
||||||
float delta = event->angleDelta().y() / 120.0f;
|
float delta = event->angleDelta().y() / 120.0f;
|
||||||
m_orthoSize *= (1.0f - delta * 0.1f);
|
m_zoom *= (1.0f + delta * 0.1f);
|
||||||
m_orthoSize = qMax(100.0f, qMin(m_orthoSize, 10000.0f)); // 范围:100-10000
|
m_zoom = qMax(0.1f, qMin(m_zoom, 10.0f)); // 范围:0.1-10倍
|
||||||
|
|
||||||
update(); // 触发重绘,paintGL会使用新的m_orthoSize
|
update();
|
||||||
}
|
}
|
||||||
|
|
||||||
void PointCloudGLWidget::updatePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
|
void PointCloudGLWidget::updatePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
|
||||||
@@ -199,66 +248,87 @@ void PointCloudGLWidget::updatePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 过滤全零点并转换为顶点数组
|
// 过滤全零点并转换为顶点数组,同时计算包围盒
|
||||||
m_vertices.clear();
|
m_vertices.clear();
|
||||||
|
|
||||||
float minX = FLT_MAX, maxX = -FLT_MAX;
|
float minX = FLT_MAX, maxX = -FLT_MAX;
|
||||||
float minY = FLT_MAX, maxY = -FLT_MAX;
|
float minY = FLT_MAX, maxY = -FLT_MAX;
|
||||||
float minZ = FLT_MAX, maxZ = -FLT_MAX;
|
float minZ = FLT_MAX, maxZ = -FLT_MAX;
|
||||||
|
|
||||||
for (const auto& point : cloud->points) {
|
for (const auto& point : cloud->points) {
|
||||||
if (point.z > 0.01f) {
|
if (point.z > 0.01f) { // 过滤掉无效的零点
|
||||||
|
const float displayZ = -point.z; // Flip front/back axis for viewer convention.
|
||||||
m_vertices.push_back(point.x);
|
m_vertices.push_back(point.x);
|
||||||
m_vertices.push_back(point.y);
|
m_vertices.push_back(-point.y);
|
||||||
m_vertices.push_back(point.z);
|
m_vertices.push_back(displayZ);
|
||||||
|
|
||||||
// 统计坐标范围
|
// 更新包围盒
|
||||||
if (point.x < minX) minX = point.x;
|
if (point.x < minX) minX = point.x;
|
||||||
if (point.x > maxX) maxX = point.x;
|
if (point.x > maxX) maxX = point.x;
|
||||||
if (point.y < minY) minY = point.y;
|
if (point.y < minY) minY = point.y;
|
||||||
if (point.y > maxY) maxY = point.y;
|
if (point.y > maxY) maxY = point.y;
|
||||||
if (point.z < minZ) minZ = point.z;
|
if (displayZ < minZ) minZ = displayZ;
|
||||||
if (point.z > maxZ) maxZ = point.z;
|
if (displayZ > maxZ) maxZ = displayZ;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
m_pointCount = m_vertices.size() / 3;
|
m_pointCount = m_vertices.size() / 3;
|
||||||
|
|
||||||
// 计算点云中心并进行中心化处理
|
// 保存深度范围用于着色
|
||||||
if (m_pointCount > 0) {
|
if (m_pointCount > 0) {
|
||||||
|
m_minZ = minZ;
|
||||||
|
m_maxZ = maxZ;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 只在首帧时自动调整相机位置,之后保持用户交互状态
|
||||||
|
if (m_pointCount > 0 && m_firstFrame) {
|
||||||
|
// 计算点云中心
|
||||||
float centerX = (minX + maxX) / 2.0f;
|
float centerX = (minX + maxX) / 2.0f;
|
||||||
float centerY = (minY + maxY) / 2.0f;
|
float centerY = (minY + maxY) / 2.0f;
|
||||||
float centerZ = (minZ + maxZ) / 2.0f;
|
float centerZ = (minZ + maxZ) / 2.0f;
|
||||||
|
|
||||||
// 第一帧:初始化固定中心点
|
// 计算点云尺寸
|
||||||
if (!m_centerInitialized) {
|
float depthRange = maxZ - minZ;
|
||||||
m_fixedCenter = QVector3D(centerX, centerY, centerZ);
|
float sizeX = maxX - minX;
|
||||||
m_centerInitialized = true;
|
float sizeY = maxY - minY;
|
||||||
qDebug() << "[PointCloudGLWidget] Fixed center initialized:" << m_fixedCenter;
|
float maxSize = std::max({sizeX, sizeY, depthRange});
|
||||||
}
|
|
||||||
|
|
||||||
// 使用固定的中心点进行中心化(避免抖动)
|
// 设置点云中心
|
||||||
for (size_t i = 0; i < m_vertices.size(); i += 3) {
|
m_cloudCenter = QVector3D(centerX, centerY, centerZ);
|
||||||
m_vertices[i] -= m_fixedCenter.x(); // X坐标
|
|
||||||
m_vertices[i + 1] -= m_fixedCenter.y(); // Y坐标
|
|
||||||
m_vertices[i + 2] -= m_fixedCenter.z(); // Z坐标
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 添加调试日志
|
// 计算观察距离,让相机从外部观察点云
|
||||||
static int updateCount = 0;
|
m_viewDistance = maxSize * 1.5f;
|
||||||
if (updateCount < 3 || updateCount % 100 == 0) {
|
|
||||||
// qDebug() << "[PointCloudGLWidget] Update" << updateCount << "- Points:" << m_pointCount
|
// 重置平移偏移和旋转角度
|
||||||
// << "Total cloud size:" << cloud->size();
|
m_panOffset = QVector3D(0.0f, 0.0f, 0.0f);
|
||||||
// qDebug() << " X range:" << minX << "to" << maxX;
|
m_rotationX = 0.0f;
|
||||||
// qDebug() << " Y range:" << minY << "to" << maxY;
|
m_rotationY = 0.0f;
|
||||||
// qDebug() << " Z range:" << minZ << "to" << maxZ;
|
|
||||||
|
// 设置缩放
|
||||||
|
m_zoom = 1.0f;
|
||||||
|
|
||||||
|
qDebug() << "[PointCloudGLWidget] 首帧自动居中 - 点云中心:" << centerX << centerY << centerZ
|
||||||
|
<< "观察距离:" << m_viewDistance;
|
||||||
|
m_firstFrame = false; // 标记首帧已处理
|
||||||
}
|
}
|
||||||
updateCount++;
|
|
||||||
|
|
||||||
updateBuffers();
|
updateBuffers();
|
||||||
update();
|
update();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PointCloudGLWidget::resetView()
|
||||||
|
{
|
||||||
|
// 重置所有视角参数到初始状态
|
||||||
|
m_rotationX = 0.0f;
|
||||||
|
m_rotationY = 0.0f;
|
||||||
|
m_panOffset = QVector3D(0.0f, 0.0f, 0.0f);
|
||||||
|
m_zoom = 1.0f;
|
||||||
|
m_firstFrame = true; // 标记为首帧,下次更新时会重新计算视角
|
||||||
|
|
||||||
|
update();
|
||||||
|
qDebug() << "[PointCloudGLWidget] 视角已重置";
|
||||||
|
}
|
||||||
|
|
||||||
void PointCloudGLWidget::updateBuffers()
|
void PointCloudGLWidget::updateBuffers()
|
||||||
{
|
{
|
||||||
if (m_vertices.empty() || !m_vao || !m_vertexBuffer) {
|
if (m_vertices.empty() || !m_vao || !m_vertexBuffer) {
|
||||||
|
|||||||
@@ -54,3 +54,15 @@ void PointCloudWidget::updatePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr clou
|
|||||||
// 更新OpenGL显示
|
// 更新OpenGL显示
|
||||||
m_glWidget->updatePointCloud(cloud);
|
m_glWidget->updatePointCloud(cloud);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PointCloudWidget::setColorMode(bool enabled)
|
||||||
|
{
|
||||||
|
if (m_glWidget) {
|
||||||
|
m_glWidget->setColorMode(enabled);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool PointCloudWidget::colorMode() const
|
||||||
|
{
|
||||||
|
return m_glWidget ? m_glWidget->colorMode() : false;
|
||||||
|
}
|
||||||
|
|||||||
32
src/main.cpp
32
src/main.cpp
@@ -1,12 +1,15 @@
|
|||||||
#include <QApplication>
|
#include <QApplication>
|
||||||
#include <QDateTime>
|
#include <QDateTime>
|
||||||
#include <QDir>
|
#include <QDir>
|
||||||
|
#include <QStandardPaths>
|
||||||
|
|
||||||
#include "gui/MainWindow.h"
|
#include "gui/MainWindow.h"
|
||||||
#include "core/Logger.h"
|
#include "core/Logger.h"
|
||||||
|
|
||||||
// Custom message handler to redirect qDebug output to Logger
|
// Redirect Qt log output to file logger.
|
||||||
void messageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg)
|
void messageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg)
|
||||||
{
|
{
|
||||||
|
Q_UNUSED(context);
|
||||||
Logger *logger = Logger::instance();
|
Logger *logger = Logger::instance();
|
||||||
|
|
||||||
switch (type) {
|
switch (type) {
|
||||||
@@ -30,29 +33,32 @@ int main(int argc, char *argv[])
|
|||||||
{
|
{
|
||||||
QApplication app(argc, argv);
|
QApplication app(argc, argv);
|
||||||
|
|
||||||
// 设置应用程序信息
|
app.setOrganizationName("Viewer");
|
||||||
app.setOrganizationName("D330Viewer");
|
app.setApplicationName("Viewer");
|
||||||
app.setApplicationName("D330Viewer");
|
app.setApplicationVersion("0.3.3");
|
||||||
app.setApplicationVersion("0.2.0");
|
|
||||||
|
|
||||||
// 初始化Logger(在可执行文件同目录下)
|
// Prefer LocalAppData so MSI installs under Program Files can always write logs.
|
||||||
QString logPath = QCoreApplication::applicationDirPath() + "/d330viewer.log";
|
QString logDir = QStandardPaths::writableLocation(QStandardPaths::AppLocalDataLocation);
|
||||||
|
if (logDir.isEmpty()) {
|
||||||
|
logDir = QCoreApplication::applicationDirPath();
|
||||||
|
}
|
||||||
|
QDir().mkpath(logDir);
|
||||||
|
|
||||||
|
const QString logPath = QDir(logDir).filePath("viewer.log");
|
||||||
Logger::instance()->setLogFile(logPath);
|
Logger::instance()->setLogFile(logPath);
|
||||||
Logger::instance()->setMaxLines(10000); // 保留最新10000行
|
Logger::instance()->setMaxLines(10000);
|
||||||
|
|
||||||
// 安装消息处理器
|
|
||||||
qInstallMessageHandler(messageHandler);
|
qInstallMessageHandler(messageHandler);
|
||||||
|
|
||||||
qDebug() << "D330Viewer started";
|
qDebug() << "Viewer started";
|
||||||
qDebug() << "Log file:" << logPath;
|
qDebug() << "Log file:" << logPath;
|
||||||
|
|
||||||
// 创建并显示主窗口
|
|
||||||
MainWindow mainWindow;
|
MainWindow mainWindow;
|
||||||
mainWindow.show();
|
mainWindow.show();
|
||||||
|
|
||||||
int result = app.exec();
|
const int result = app.exec();
|
||||||
|
|
||||||
qDebug() << "D330Viewer exiting";
|
qDebug() << "Viewer exiting";
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user