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

686
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,122 +1178,148 @@ 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);
if(jpegData.size() == size8bit) {
// 8位下采样格式：直接显示
QByteArray dataCopy = jpegData;
QtConcurrent::run([this, dataCopy]() {
try {
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 {
const uint16_t* src = reinterpret_cast<const uint16_t*>(dataCopy.constData());
// 方案2：快速百分位数估算（无需排序，采样估算）
// 优点：适应不同环境，画面对比度好，速度快10倍以上
uint16_t minVal = 65535, maxVal = 0;
// 第一遍：快速扫描找到粗略范围（每隔8个像素采样）
for (int i = 0; i < 1224 * 1024; i += 8) {
uint16_t val = src[i];
if(val > 0) {
if(val < minVal) minVal = val;
if(val > maxVal) maxVal = val;
}
}
// 第二遍：使用直方图统计精确百分位数（避免排序）
if(maxVal > minVal) {
const int histSize = 256;
int histogram[histSize] = {0};
float binWidth = (maxVal - minVal) / (float)histSize;
// 构建直方图
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;
}
if(cumsum >= thresh_99) {
maxVal = minVal + i * binWidth;
break;
}
}
}
// 创建8位图像并归一化
QImage image(1224, 1024, QImage::Format_Grayscale8);
uint8_t* dst = image.bits();
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
for (int i = 0; i < 1224 * 1024; i++) {
if(src[i] == 0) {
dst[i] = 0;
} else if(src[i] <= minVal) {
dst[i] = 0;
} else if(src[i] >= maxVal) {
dst[i] = 255;
} else {
dst[i] = static_cast<uint8_t>((src[i] - minVal) * scale);
}
}
QImage imageCopy = image.copy();
// 在主线程更新UI
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 processing exception:" << e.what();
}
});
} else {
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();
QByteArray dataCopy = jpegData;
QtConcurrent::run([this, dataCopy, is16bit]() {
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());
constexpr int kWidth = 1224;
constexpr int kHeight = 1024;
constexpr int kPixels = kWidth * kHeight;
constexpr int kHistSize = 256;
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 < sampleMin) sampleMin = val;
if(val > sampleMax) sampleMax = val;
}
}
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));
int totalPixels = 0;
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(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;
}
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);
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(val >= rangeMax) {
dst[i] = 255;
} else {
dst[i] = static_cast<uint8_t>((val - rangeMin) * scale);
}
}
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 processing exception:" << e.what();
} catch (...) {
qDebug() << "[MainWindow] ERROR: Left IR processing unknown exception";
}
m_leftIRProcessing.deref();
});
}
}
@@ -1195,122 +1351,148 @@ 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);
if(jpegData.size() == size8bit) {
// 8位下采样格式：直接显示
QByteArray dataCopy = jpegData;
QtConcurrent::run([this, dataCopy]() {
try {
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 {
const uint16_t* src = reinterpret_cast<const uint16_t*>(dataCopy.constData());
// 方案2：快速百分位数估算（无需排序，采样估算）
// 优点：适应不同环境，画面对比度好，速度快10倍以上
uint16_t minVal = 65535, maxVal = 0;
// 第一遍：快速扫描找到粗略范围（每隔8个像素采样）
for (int i = 0; i < 1224 * 1024; i += 8) {
uint16_t val = src[i];
if(val > 0) {
if(val < minVal) minVal = val;
if(val > maxVal) maxVal = val;
}
}
// 第二遍：使用直方图统计精确百分位数（避免排序）
if(maxVal > minVal) {
const int histSize = 256;
int histogram[histSize] = {0};
float binWidth = (maxVal - minVal) / (float)histSize;
// 构建直方图
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;
}
if(cumsum >= thresh_99) {
maxVal = minVal + i * binWidth;
break;
}
}
}
// 创建8位图像并归一化
QImage image(1224, 1024, QImage::Format_Grayscale8);
uint8_t* dst = image.bits();
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
for (int i = 0; i < 1224 * 1024; i++) {
if(src[i] == 0) {
dst[i] = 0;
} else if(src[i] <= minVal) {
dst[i] = 0;
} else if(src[i] >= maxVal) {
dst[i] = 255;
} else {
dst[i] = static_cast<uint8_t>((src[i] - minVal) * scale);
}
}
QImage imageCopy = image.copy();
// 在主线程更新UI
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 processing exception:" << e.what();
}
});
} else {
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();
QByteArray dataCopy = jpegData;
QtConcurrent::run([this, dataCopy, is16bit]() {
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());
constexpr int kWidth = 1224;
constexpr int kHeight = 1024;
constexpr int kPixels = kWidth * kHeight;
constexpr int kHistSize = 256;
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 < sampleMin) sampleMin = val;
if(val > sampleMax) sampleMax = val;
}
}
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));
int totalPixels = 0;
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(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;
}
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);
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(val >= rangeMax) {
dst[i] = 255;
} else {
dst[i] = static_cast<uint8_t>((val - rangeMin) * scale);
}
}
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 processing exception:" << e.what();
} catch (...) {
qDebug() << "[MainWindow] ERROR: Right IR processing unknown exception";
}
m_rightIRProcessing.deref();
});
}
}
@@ -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;
}
}