gloamxun/d330viewer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 5 Wiki Activity

feat: 添加点云去噪及其参数调整

Browse Source
This commit is contained in:
YusenZhao
2026-03-04 15:59:39 +08:00
parent c2b525d948
commit a6e2e3280a
10 changed files with 2536 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
CMakeLists.txt
View File

@@ -34,7 +34,7 @@ find_package(Qt6 REQUIRED COMPONENTS
)
# 查找PCL
find_package(PCL REQUIRED COMPONENTS common io visualization)
find_package(PCL REQUIRED COMPONENTS common io visualization filters)
if(PCL_FOUND)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
@@ -101,6 +101,52 @@ add_executable(${PROJECT_NAME} WIN32
${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}
Qt6::Core
@@ -141,6 +187,15 @@ install(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/platforms/
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安装程序 ====================
set(CPACK_PACKAGE_NAME "Viewer")
set(CPACK_PACKAGE_VENDOR "Lorenzo Zhao")

45
README.md
View File

@@ -181,6 +181,51 @@ C:\Program Files\D330Viewer\
- 性能监控（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`
## 项目结构
```

3
cmos0/KK.txt Normal file
View 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
View File

File diff suppressed because it is too large Load Diff

5
cmos0/kc.txt Normal file
View File

@@ -0,0 +1,5 @@
-1.2009005e-01
1.1928703e-01
9.6197371e-05
-1.4896083e-04
0.0000000e+00

48
include/core/PointCloudProcessor.h
View File

@@ -1,8 +1,10 @@
#ifndef POINTCLOUDPROCESSOR_H
#ifndef POINTCLOUDPROCESSOR_H
#define POINTCLOUDPROCESSOR_H
#include <QObject>
#include <QByteArray>
#include <atomic>
#include <mutex>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <CL/cl.h>
@@ -15,44 +17,38 @@ public:
explicit PointCloudProcessor(QObject *parent = nullptr);
~PointCloudProcessor();
// 初始化OpenCL
bool initializeOpenCL();
// 设置相机内参
void setCameraIntrinsics(float fx, float fy, float cx, float cy);
// 设置Z缩放因子
void setZScaleFactor(float scale);
// 将深度数据转换为点云（使用OpenCL GPU加速）
void processDepthData(const QByteArray &depthData, uint32_t blockId);
// 处理已经计算好的点云数据（x,y,z格式）
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:
void pointCloudReady(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, uint32_t blockId);
void errorOccurred(const QString &error);
private:
// 清理OpenCL资源
pcl::PointCloud<pcl::PointXYZ>::Ptr applyDenoise(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input);
void loadLowerCalibration();
void cleanupOpenCL();
// 相机内参
float m_fx;
float m_fy;
float m_cx;
float m_cy;
// Z缩放因子
float m_zScale;
// 图像尺寸
int m_imageWidth;
int m_imageHeight;
int m_totalPoints;
// OpenCL资源
cl_platform_id m_platform;
cl_device_id m_device;
cl_context m_context;
@@ -62,6 +58,30 @@ private:
cl_mem m_depthBuffer;
cl_mem m_xyzBuffer;
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

704
src/core/PointCloudProcessor.cpp
View File

@@ -1,12 +1,93 @@
#include "core/PointCloudProcessor.h"
#include <QDebug>
#include <QCoreApplication>
#include <QDir>
#include <QFile>
#include <QRegularExpression>
#include <QStringList>
#include <vector>
#include <cmath>
#include <cstdint>
#include <algorithm>
#include <unordered_map>
#include <pcl/common/point_tests.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#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)
: QObject(parent)
, m_fx(1432.8957f)
@@ -26,7 +107,26 @@ PointCloudProcessor::PointCloudProcessor(QObject *parent)
, m_depthBuffer(nullptr)
, m_xyzBuffer(nullptr)
, 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()
@@ -40,6 +140,14 @@ void PointCloudProcessor::setCameraIntrinsics(float fx, float fy, float cx, floa
m_fy = fy;
m_cx = cx;
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)
@@ -47,6 +155,557 @@ void PointCloudProcessor::setZScaleFactor(float 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()
{
if (m_clInitialized) {
@@ -247,6 +906,9 @@ void PointCloudProcessor::processDepthData(const QByteArray &depthData, uint32_t
// 注释掉频繁的日志输出
// qDebug() << "[PointCloud] Block" << blockId << "processed successfully";
if (m_denoiseEnabled.load(std::memory_order_relaxed)) {
cloud = applyDenoise(cloud);
}
emit pointCloudReady(cloud, blockId);
}
@@ -282,8 +944,9 @@ void PointCloudProcessor::processPointCloudData(const QByteArray &cloudData, uin
// 从int16_t数组读取点云数据
const int16_t* cloudShort = reinterpret_cast<const int16_t*>(cloudData.constData());
float inv_fx = 1.0f / m_fx;
float inv_fy = 1.0f / m_fy;
// 与下位机 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) {
// Z-only格式：标准针孔模型反投影
@@ -292,8 +955,21 @@ void PointCloudProcessor::processPointCloudData(const QByteArray &cloudData, uin
int col = i % m_imageWidth;
float z = static_cast<float>(cloudShort[i]) * m_zScale;
cloud->points[i].x = (col - m_cx) * z * inv_fx;
cloud->points[i].y = (row - m_cy) * z * inv_fy;
// 旧公式保留，便于快速回退：
// 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;
}
} else {
@@ -303,14 +979,30 @@ void PointCloudProcessor::processPointCloudData(const QByteArray &cloudData, uin
int col = i % m_imageWidth;
float 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;
// 旧公式保留，便于快速回退：
// 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,"
// << m_totalPoints << "points";
if (m_denoiseEnabled.load(std::memory_order_relaxed)) {
cloud = applyDenoise(cloud);
}
emit pointCloudReady(cloud, blockId);
}

550
src/gui/MainWindow.cpp
View File

@@ -63,11 +63,19 @@ MainWindow::MainWindow(QWidget *parent)
, m_rgbFrameCount(0)
, m_totalRgbFrameCount(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_rgbProcessing.storeRelaxed(0); // 初始化RGB处理标志
m_leftIRProcessing.storeRelaxed(0); // 初始化左红外处理标志
m_rightIRProcessing.storeRelaxed(0); // 初始化右红外处理标志
m_pointCloudProcessing.storeRelaxed(0); // 初始化点云处理标志
m_pointCloudDropCounter.storeRelaxed(0); // 初始化点云丢帧计数
m_leftIREnabled.storeRelaxed(0); // 初始化左红外启用标志（默认禁用）
m_rightIREnabled.storeRelaxed(0); // 初始化右红外启用标志（默认禁用）
m_rgbEnabled.storeRelaxed(0); // 初始化RGB启用标志（默认禁用）
@@ -232,6 +240,14 @@ void MainWindow::setupUI()
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);
// 单目/双目模式切换按钮
@@ -433,6 +449,53 @@ void MainWindow::setupUI()
m_pointCloudFormatCombo->setCurrentIndex(2);
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->addStretch();
@@ -692,6 +755,7 @@ void MainWindow::setupConnections()
connect(m_leftIRToggle, &QPushButton::toggled, this, [this](bool checked) {
if(checked) {
m_leftIREnabled.storeRelaxed(1); // 标记启用
m_leftIrDisplayRangeInited = false; // 重新开启时重置显示动态范围
m_networkManager->sendEnableLeftIR();
qDebug() << "启用左红外传输";
} else {
@@ -709,6 +773,7 @@ void MainWindow::setupConnections()
connect(m_rightIRToggle, &QPushButton::toggled, this, [this](bool checked) {
if(checked) {
m_rightIREnabled.storeRelaxed(1); // 标记启用
m_rightIrDisplayRangeInited = false; // 重新开启时重置显示动态范围
m_networkManager->sendEnableRightIR();
qDebug() << "启用右红外传输";
} else {
@@ -748,6 +813,71 @@ void MainWindow::setupConnections()
}
});
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]() {
m_monocularBtn->setChecked(true);
@@ -1048,107 +1178,134 @@ void MainWindow::onLeftImageReceived(const QByteArray &jpegData, uint32_t blockI
// 使用后台线程处理红外数据，避免阻塞UI
if(m_leftImageDisplay && jpegData.size() > 0) {
// 检查数据大小：8位下采样(612x512)或16位原始(1224x1024)
size_t size8bit = 612 * 512;
size_t size16bit = 1224 * 1024 * sizeof(uint16_t);
const size_t size8bit = 612 * 512;
const size_t size16bit = 1224 * 1024 * sizeof(uint16_t);
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();
if(jpegData.size() == size8bit) {
// 8位下采样格式：直接显示
QByteArray dataCopy = jpegData;
QtConcurrent::run([this, dataCopy]() {
QtConcurrent::run([this, dataCopy, is16bit]() {
try {
QImage imageCopy;
if(!is16bit) {
QImage image(reinterpret_cast<const uchar*>(dataCopy.constData()),
612, 512, 612, QImage::Format_Grayscale8);
QImage imageCopy = image.copy();
QMetaObject::invokeMethod(this, [this, imageCopy]() {
if(m_leftImageDisplay) {
QPixmap pixmap = QPixmap::fromImage(imageCopy);
m_leftImageDisplay->setPixmap(pixmap.scaled(
m_leftImageDisplay->size(), Qt::KeepAspectRatio, Qt::FastTransformation));
}
}, Qt::QueuedConnection);
} catch (const std::exception &e) {
qDebug() << "[MainWindow] ERROR: Left IR 8bit processing exception:" << e.what();
}
});
} else if(jpegData.size() == size16bit) {
// 16位原始格式：需要归一化处理
QByteArray dataCopy = jpegData;
// 在后台线程处理
QtConcurrent::run([this, dataCopy]() {
try {
imageCopy = image.copy();
} else {
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：快速百分位数估算（无需排序，采样估算）
// 优点：适应不同环境，画面对比度好，速度快10倍以上
uint16_t minVal = 65535, maxVal = 0;
// 第一遍：快速扫描找到粗略范围（每隔8个像素采样）
for (int i = 0; i < 1224 * 1024; i += 8) {
uint16_t val = src[i];
uint16_t sampleMin = 65535;
uint16_t sampleMax = 0;
for(int i = 0; i < kPixels; i += 8) {
const uint16_t val = src[i];
if(val > 0) {
if(val < minVal) minVal = val;
if(val > maxVal) maxVal = val;
if(val < sampleMin) sampleMin = val;
if(val > sampleMax) sampleMax = val;
}
}
// 第二遍：使用直方图统计精确百分位数（避免排序）
if(maxVal > minVal) {
const int histSize = 256;
int histogram[histSize] = {0};
float binWidth = (maxVal - minVal) / (float)histSize;
float rangeMin = 0.0f;
float rangeMax = 65535.0f;
if(sampleMax > sampleMin) {
int histogram[kHistSize] = {0};
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;
for (int i = 0; i < histSize; i++) totalPixels += histogram[i];
int thresh_1 = totalPixels * 0.01;
int thresh_99 = totalPixels * 0.99;
int cumsum = 0;
for (int i = 0; i < histSize; i++) {
cumsum += histogram[i];
if(cumsum >= thresh_1 && minVal == 65535) {
minVal = minVal + i * binWidth;
for(int i = 0; i < kPixels; ++i) {
const uint16_t val = src[i];
if(val > 0) {
int bin = static_cast<int>((val - sampleMin) / binWidth);
if(bin < 0) bin = 0;
if(bin >= kHistSize) bin = kHistSize - 1;
histogram[bin]++;
totalPixels++;
}
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;
}
}
float rawMin = sampleMin + p1Bin * binWidth;
float rawMax = sampleMin + p99Bin * binWidth;
if(rawMax <= rawMin + 1.0f) {
rawMax = rawMin + 1.0f;
}
// 创建8位图像并归一化
QImage image(1224, 1024, QImage::Format_Grayscale8);
uint8_t* dst = image.bits();
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
if(!m_leftIrDisplayRangeInited) {
m_leftIrDisplayMin = rawMin;
m_leftIrDisplayMax = rawMax;
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++) {
if(src[i] == 0) {
m_leftIrDisplayMin = prevMin * 0.85f + minClamped * 0.15f;
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;
} else if(src[i] <= minVal) {
dst[i] = 0;
} else if(src[i] >= maxVal) {
} else if(val >= rangeMax) {
dst[i] = 255;
} 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]() {
if(m_leftImageDisplay) {
QPixmap pixmap = QPixmap::fromImage(imageCopy);
@@ -1158,12 +1315,11 @@ void MainWindow::onLeftImageReceived(const QByteArray &jpegData, uint32_t blockI
}, Qt::QueuedConnection);
} catch (const std::exception &e) {
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()
<< "(expected 8bit:" << size8bit << "or 16bit:" << size16bit << ")";
}
}
}
@@ -1195,107 +1351,134 @@ void MainWindow::onRightImageReceived(const QByteArray &jpegData, uint32_t block
// 使用后台线程处理红外数据，避免阻塞UI
if(m_rightImageDisplay && jpegData.size() > 0) {
// 检查数据大小：8位下采样(612x512)或16位原始(1224x1024)
size_t size8bit = 612 * 512;
size_t size16bit = 1224 * 1024 * sizeof(uint16_t);
const size_t size8bit = 612 * 512;
const size_t size16bit = 1224 * 1024 * sizeof(uint16_t);
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();
if(jpegData.size() == size8bit) {
// 8位下采样格式：直接显示
QByteArray dataCopy = jpegData;
QtConcurrent::run([this, dataCopy]() {
QtConcurrent::run([this, dataCopy, is16bit]() {
try {
QImage imageCopy;
if(!is16bit) {
QImage image(reinterpret_cast<const uchar*>(dataCopy.constData()),
612, 512, 612, QImage::Format_Grayscale8);
QImage imageCopy = image.copy();
QMetaObject::invokeMethod(this, [this, imageCopy]() {
if(m_rightImageDisplay) {
QPixmap pixmap = QPixmap::fromImage(imageCopy);
m_rightImageDisplay->setPixmap(pixmap.scaled(
m_rightImageDisplay->size(), Qt::KeepAspectRatio, Qt::FastTransformation));
}
}, Qt::QueuedConnection);
} catch (const std::exception &e) {
qDebug() << "[MainWindow] ERROR: Right IR 8bit processing exception:" << e.what();
}
});
} else if(jpegData.size() == size16bit) {
// 16位原始格式：需要归一化处理
QByteArray dataCopy = jpegData;
// 在后台线程处理
QtConcurrent::run([this, dataCopy]() {
try {
imageCopy = image.copy();
} else {
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：快速百分位数估算（无需排序，采样估算）
// 优点：适应不同环境，画面对比度好，速度快10倍以上
uint16_t minVal = 65535, maxVal = 0;
// 第一遍：快速扫描找到粗略范围（每隔8个像素采样）
for (int i = 0; i < 1224 * 1024; i += 8) {
uint16_t val = src[i];
uint16_t sampleMin = 65535;
uint16_t sampleMax = 0;
for(int i = 0; i < kPixels; i += 8) {
const uint16_t val = src[i];
if(val > 0) {
if(val < minVal) minVal = val;
if(val > maxVal) maxVal = val;
if(val < sampleMin) sampleMin = val;
if(val > sampleMax) sampleMax = val;
}
}
// 第二遍：使用直方图统计精确百分位数（避免排序）
if(maxVal > minVal) {
const int histSize = 256;
int histogram[histSize] = {0};
float binWidth = (maxVal - minVal) / (float)histSize;
float rangeMin = 0.0f;
float rangeMax = 65535.0f;
if(sampleMax > sampleMin) {
int histogram[kHistSize] = {0};
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;
for (int i = 0; i < histSize; i++) totalPixels += histogram[i];
int thresh_1 = totalPixels * 0.01;
int thresh_99 = totalPixels * 0.99;
int cumsum = 0;
for (int i = 0; i < histSize; i++) {
cumsum += histogram[i];
if(cumsum >= thresh_1 && minVal == 65535) {
minVal = minVal + i * binWidth;
for(int i = 0; i < kPixels; ++i) {
const uint16_t val = src[i];
if(val > 0) {
int bin = static_cast<int>((val - sampleMin) / binWidth);
if(bin < 0) bin = 0;
if(bin >= kHistSize) bin = kHistSize - 1;
histogram[bin]++;
totalPixels++;
}
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;
}
}
float rawMin = sampleMin + p1Bin * binWidth;
float rawMax = sampleMin + p99Bin * binWidth;
if(rawMax <= rawMin + 1.0f) {
rawMax = rawMin + 1.0f;
}
// 创建8位图像并归一化
QImage image(1224, 1024, QImage::Format_Grayscale8);
uint8_t* dst = image.bits();
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
if(!m_rightIrDisplayRangeInited) {
m_rightIrDisplayMin = rawMin;
m_rightIrDisplayMax = rawMax;
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++) {
if(src[i] == 0) {
m_rightIrDisplayMin = prevMin * 0.85f + minClamped * 0.15f;
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;
} else if(src[i] <= minVal) {
dst[i] = 0;
} else if(src[i] >= maxVal) {
} else if(val >= rangeMax) {
dst[i] = 255;
} 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]() {
if(m_rightImageDisplay) {
QPixmap pixmap = QPixmap::fromImage(imageCopy);
@@ -1305,12 +1488,11 @@ void MainWindow::onRightImageReceived(const QByteArray &jpegData, uint32_t block
}, Qt::QueuedConnection);
} catch (const std::exception &e) {
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()
<< "(expected 8bit:" << size8bit << "or 16bit:" << size16bit << ")";
}
}
}
@@ -1407,21 +1589,57 @@ void MainWindow::onRgbImageReceived(const QByteArray &jpegData, uint32_t blockId
void MainWindow::onDepthDataReceived(const QByteArray &depthData, uint32_t blockId)
{
// 实时处理每一帧
// 注释掉频繁的日志输出
// qDebug() << "Depth data received: Block" << blockId << "Size:" << depthData.size() << "bytes";
// 点云处理忙时直接丢弃新帧，避免任务堆积拖垮线程池和UI响应。
if(m_pointCloudProcessing.loadAcquire() > 0) {
int dropped = m_pointCloudDropCounter.fetchAndAddRelaxed(1) + 1;
if((dropped % 60) == 0) {
qDebug() << "[MainWindow] Point cloud(depth) busy, dropped frames:" << dropped;
}
return;
}
// 调用PointCloudProcessor进行OpenCL计算
m_pointCloudProcessor->processDepthData(depthData, blockId);
m_pointCloudProcessing.ref();
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)
{
// qDebug() << "[MainWindow] Point cloud data received: Block" << blockId << "Size:" << cloudData.size() << "bytes";
// 使用QtConcurrent在后台线程处理点云数据
QtConcurrent::run([this, cloudData, blockId]() {
m_pointCloudProcessor->processPointCloudData(cloudData, blockId);
// 点云处理忙时直接丢弃新帧，避免任务堆积拖垮线程池和UI响应。
if(m_pointCloudProcessing.loadAcquire() > 0) {
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();
});
}

15
src/gui/MainWindow.h
View File

@@ -141,6 +141,7 @@ private:
QPushButton *m_rightIRToggle;
QPushButton *m_rgbToggle;
QPushButton *m_pointCloudColorToggle; // 点云颜色开关
QPushButton *m_pointCloudDenoiseToggle; // Point cloud denoise toggle
// 单目/双目模式切换按钮
QPushButton *m_monocularBtn;
@@ -157,6 +158,12 @@ private:
QPushButton *m_browseSavePathBtn;
class QComboBox *m_depthFormatCombo;
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;
@@ -212,6 +219,14 @@ private:
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帧跳过计数器
// 相机启用状态标志（防止关闭后闪烁）

7
src/gui/PointCloudGLWidget.cpp
View File

@@ -257,17 +257,18 @@ void PointCloudGLWidget::updatePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
for (const auto& point : cloud->points) {
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.y);
m_vertices.push_back(point.z);
m_vertices.push_back(displayZ);
// 更新包围盒
if (point.x < minX) minX = point.x;
if (point.x > maxX) maxX = point.x;
if (point.y < minY) minY = point.y;
if (point.y > maxY) maxY = point.y;
if (point.z < minZ) minZ = point.z;
if (point.z > maxZ) maxZ = point.z;
if (displayZ < minZ) minZ = displayZ;
if (displayZ > maxZ) maxZ = displayZ;
}
}