feat: 添加点云去噪及其参数调整
This commit is contained in:
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user