src/tracking/SurfelCoverageTracker.cpp - master - klischa/AstraScanner2

src/tracking/SurfelCoverageTracker.cpp
512 строк17 KB
k k
refactor: remove MainWindow dead code + fix VoxelGrid warnings
14 июл 2026, 23:40
14 июл 2026, 23:401459aca
#include "SurfelCoverageTracker.h"

#include <algorithm>
#include <array>
#include <cmath>
#include <limits>
#include <numeric>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/search/kdtree.h>
#include <QMutexLocker>

namespace {

float levelToWeight(int level)
{
    switch (level) {
    case 0: return 1.0f;
    case 1: return 0.55f;
    case 2:
    default: return 0.20f;
    }
}

Eigen::Vector3f anyPerpendicular(const Eigen::Vector3f &normal)
{
    if (std::abs(normal.z()) < 0.9f) {
        return normal.cross(Eigen::Vector3f::UnitZ()).normalized();
    }
    return normal.cross(Eigen::Vector3f::UnitX()).normalized();
}

} // namespace

std::size_t SurfelCellKeyHash::operator()(const SurfelCellKey &key) const noexcept
{
    std::size_t hx = std::hash<int>{}(key.x);
    std::size_t hy = std::hash<int>{}(key.y);
    std::size_t hz = std::hash<int>{}(key.z);
    return hx ^ (hy << 1) ^ (hz << 2);
}

SurfelCoverageTracker::SurfelCoverageTracker(const SurfelCoverageConfig &config)
    : m_config(config)
{
}

void SurfelCoverageTracker::reset()
{
    QMutexLocker locker(&m_mutex);
    m_surfels.clear();
    m_spatialIndex.clear();
    m_frameCounter = 0;
}

void SurfelCoverageTracker::setConfig(const SurfelCoverageConfig &config)
{
    QMutexLocker locker(&m_mutex);
    m_config = config;
}

void SurfelCoverageTracker::setFrameQualityWeights(int trackingLevel,
                                                   int alignmentLevel,
                                                   int depthLevel,
                                                   int driftLevel)
{
    QMutexLocker locker(&m_mutex);
    m_lastTrackingLevel = trackingLevel;
    m_lastAlignmentLevel = alignmentLevel;
    m_lastDepthLevel = depthLevel;
    m_lastDriftLevel = driftLevel;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr SurfelCoverageTracker::preprocessCloud(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud) const
{
    auto filtered = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    if (!cloud || cloud->empty()) {
        return filtered;
    }

    filtered->reserve(cloud->size());
    for (const auto &pt : cloud->points) {
        if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
            continue;
        }
        filtered->push_back(pt);
    }

    if (filtered->empty()) {
        return filtered;
    }

    pcl::VoxelGrid<pcl::PointXYZRGB> voxel;
    voxel.setLeafSize(m_config.downsampleLeaf, m_config.downsampleLeaf, m_config.downsampleLeaf);
    voxel.setSaveLeafLayout(false);
    voxel.setInputCloud(filtered);
    auto downsampled = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    try {
        voxel.filter(*downsampled);
    } catch (...) {
        return filtered;
    }

    if (downsampled->size() <= static_cast<std::size_t>(m_config.maxIntegrationPoints)) {
        return downsampled;
    }

    auto limited = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    const std::size_t step = std::max<std::size_t>(1, downsampled->size() / static_cast<std::size_t>(m_config.maxIntegrationPoints));
    limited->reserve(m_config.maxIntegrationPoints);
    for (std::size_t i = 0; i < downsampled->size() && limited->size() < static_cast<std::size_t>(m_config.maxIntegrationPoints); i += step) {
        limited->push_back((*downsampled)[i]);
    }
    return limited;
}

pcl::PointCloud<pcl::Normal>::Ptr SurfelCoverageTracker::estimateNormals(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud) const
{
    auto normals = pcl::make_shared<pcl::PointCloud<pcl::Normal>>();
    if (!cloud || cloud->empty()) {
        return normals;
    }

    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
    ne.setInputCloud(cloud);
    ne.setSearchMethod(tree);
    ne.setRadiusSearch(std::max(0.01f, m_config.surfelRadius * 2.0f));
    ne.compute(*normals);
    return normals;
}

SurfelCellKey SurfelCoverageTracker::makeCellKey(const Eigen::Vector3f &position) const
{
    const float cellSize = std::max(0.001f, m_config.surfelRadius);
    return {
        static_cast<int>(std::floor(position.x() / cellSize)),
        static_cast<int>(std::floor(position.y() / cellSize)),
        static_cast<int>(std::floor(position.z() / cellSize))
    };
}

void SurfelCoverageTracker::rebuildSpatialIndex()
{
    m_spatialIndex.clear();
    for (int i = 0; i < static_cast<int>(m_surfels.size()); ++i) {
        m_spatialIndex[makeCellKey(m_surfels[i].position)].push_back(i);
    }
}

constexpr float kPi = 3.14159265358979323846f;

float SurfelCoverageTracker::angleBetweenNormalsDeg(const Eigen::Vector3f &a,
                                                    const Eigen::Vector3f &b) const
{
    const float dot = std::clamp(a.normalized().dot(b.normalized()), -1.0f, 1.0f);
    return std::acos(dot) * 180.0f / static_cast<float>(kPi);
}

bool SurfelCoverageTracker::surfelMatches(const SurfelRecord &surfel,
                                          const Eigen::Vector3f &position,
                                          const Eigen::Vector3f &normal) const
{
    const float distance = (surfel.position - position).norm();
    if (!std::isfinite(distance) || distance > m_config.associationDistance) {
        return false;
    }

    const float angle = angleBetweenNormalsDeg(surfel.normal, normal);
    return std::isfinite(angle) && angle <= m_config.normalAngleThresholdDeg;
}

int SurfelCoverageTracker::findMatchingSurfel(const Eigen::Vector3f &position,
                                              const Eigen::Vector3f &normal) const
{
    const SurfelCellKey center = makeCellKey(position);
    int bestIndex = -1;
    float bestDistance = std::numeric_limits<float>::infinity();

    for (int dx = -1; dx <= 1; ++dx) {
        for (int dy = -1; dy <= 1; ++dy) {
            for (int dz = -1; dz <= 1; ++dz) {
                const SurfelCellKey key{center.x + dx, center.y + dy, center.z + dz};
                auto it = m_spatialIndex.find(key);
                if (it == m_spatialIndex.end()) {
                    continue;
                }
                for (int idx : it->second) {
                    const auto &surfel = m_surfels[idx];
                    if (!surfelMatches(surfel, position, normal)) {
                        continue;
                    }
                    const float distance = (surfel.position - position).norm();
                    if (distance < bestDistance) {
                        bestDistance = distance;
                        bestIndex = idx;
                    }
                }
            }
        }
    }

    return bestIndex;
}

int SurfelCoverageTracker::createSurfel(const Eigen::Vector3f &position,
                                        const Eigen::Vector3f &normal)
{
    SurfelRecord surfel;
    surfel.position = position;
    surfel.normal = normal.normalized();
    surfel.angularHistogram.assign(std::max(4, m_config.angularBins), 0.0f);
    m_surfels.push_back(surfel);
    const int index = static_cast<int>(m_surfels.size()) - 1;
    m_spatialIndex[makeCellKey(position)].push_back(index);
    return index;
}

Eigen::Vector3f SurfelCoverageTracker::computeViewDirection(const Eigen::Vector3f &point,
                                                            const Eigen::Vector3f &sensorOrigin) const
{
    Eigen::Vector3f dir = sensorOrigin - point;
    const float n = dir.norm();
    if (!std::isfinite(n) || n < 1e-6f) {
        return Eigen::Vector3f::UnitZ();
    }
    return dir / n;
}

void SurfelCoverageTracker::buildLocalFrame(const Eigen::Vector3f &normal,
                                            Eigen::Vector3f &tangent,
                                            Eigen::Vector3f &bitangent) const
{
    tangent = anyPerpendicular(normal);
    bitangent = normal.cross(tangent).normalized();
}

int SurfelCoverageTracker::angularBinForViewDirection(const SurfelRecord &surfel,
                                                      const Eigen::Vector3f &viewDir) const
{
    Eigen::Vector3f tangent, bitangent;
    buildLocalFrame(surfel.normal, tangent, bitangent);

    const float x = viewDir.dot(tangent);
    const float y = viewDir.dot(bitangent);
    float angle = std::atan2(y, x);
    if (angle < 0.0f) {
        angle += 2.0f * static_cast<float>(kPi);
    }

    const int bins = std::max(4, m_config.angularBins);
    const float binSize = 2.0f * static_cast<float>(kPi) / static_cast<float>(bins);
    return std::clamp(static_cast<int>(angle / binSize), 0, bins - 1);
}

float SurfelCoverageTracker::incidenceScore(const Eigen::Vector3f &normal,
                                            const Eigen::Vector3f &viewDir) const
{
    return std::clamp(std::abs(normal.normalized().dot(-viewDir.normalized())), 0.0f, 1.0f);
}

float SurfelCoverageTracker::frameQualityWeight() const
{
    return std::clamp(
        0.35f * levelToWeight(m_lastTrackingLevel) +
        0.30f * levelToWeight(m_lastAlignmentLevel) +
        0.20f * levelToWeight(m_lastDepthLevel) +
        0.15f * levelToWeight(m_lastDriftLevel),
        0.05f, 1.0f);
}

void SurfelCoverageTracker::updateSurfel(int surfelIndex,
                                         const Eigen::Vector3f &position,
                                         const Eigen::Vector3f &normal,
                                         const Eigen::Vector3f &sensorOrigin,
                                         float frameQualityWeight,
                                         uint32_t frameIndex)
{
    auto &surfel = m_surfels[surfelIndex];
    const Eigen::Vector3f oldPosition = surfel.position;

    const float alpha = 0.18f;
    surfel.position = (1.0f - alpha) * surfel.position + alpha * position;
    surfel.normal = ((1.0f - alpha) * surfel.normal + alpha * normal).normalized();

    surfel.observations += 1;
    surfel.confidenceSum += frameQualityWeight;

    const Eigen::Vector3f viewDir = computeViewDirection(position, sensorOrigin);
    surfel.incidenceScoreSum += incidenceScore(surfel.normal, viewDir) * frameQualityWeight;

    const float stability = std::clamp(1.0f - ((position - oldPosition).norm() / std::max(0.001f, m_config.associationDistance)), 0.0f, 1.0f);
    surfel.stabilityScoreSum += stability * frameQualityWeight;

    const int bin = angularBinForViewDirection(surfel, viewDir);
    if (bin >= 0 && bin < static_cast<int>(surfel.angularHistogram.size())) {
        surfel.angularHistogram[bin] += frameQualityWeight;
    }

    surfel.lastSeenFrame = frameIndex;
}

float SurfelCoverageTracker::observationConfidence(const SurfelRecord &surfel) const
{
    return std::clamp(surfel.confidenceSum / 4.0f, 0.0f, 1.0f);
}

float SurfelCoverageTracker::angularCoverageConfidence(const SurfelRecord &surfel) const
{
    if (surfel.angularHistogram.empty()) {
        return 0.0f;
    }
    int filled = 0;
    for (float w : surfel.angularHistogram) {
        if (w > 0.01f) {
            ++filled;
        }
    }
    return static_cast<float>(filled) / static_cast<float>(surfel.angularHistogram.size());
}

float SurfelCoverageTracker::angularDiversityConfidence(const SurfelRecord &surfel) const
{
    if (surfel.angularHistogram.empty()) {
        return 0.0f;
    }
    const float total = std::accumulate(surfel.angularHistogram.begin(), surfel.angularHistogram.end(), 0.0f);
    if (total <= 1e-6f) {
        return 0.0f;
    }
    const float maxBin = *std::max_element(surfel.angularHistogram.begin(), surfel.angularHistogram.end());
    return std::clamp(1.0f - maxBin / total, 0.0f, 1.0f);
}

float SurfelCoverageTracker::averageIncidenceConfidence(const SurfelRecord &surfel) const
{
    if (surfel.confidenceSum <= 1e-6f) {
        return 0.0f;
    }
    return std::clamp(surfel.incidenceScoreSum / surfel.confidenceSum, 0.0f, 1.0f);
}

float SurfelCoverageTracker::averageStabilityConfidence(const SurfelRecord &surfel) const
{
    if (surfel.confidenceSum <= 1e-6f) {
        return 0.0f;
    }
    return std::clamp(surfel.stabilityScoreSum / surfel.confidenceSum, 0.0f, 1.0f);
}

float SurfelCoverageTracker::finalConfidence(const SurfelRecord &surfel) const
{
    const float obs = observationConfidence(surfel);
    const float coverage = angularCoverageConfidence(surfel);
    const float diversity = angularDiversityConfidence(surfel);
    const float incidence = averageIncidenceConfidence(surfel);
    const float stability = averageStabilityConfidence(surfel);

    const float angularComponent = 0.65f * coverage + 0.35f * diversity;
    return std::clamp(
        m_config.observationWeight * obs +
        m_config.angularCoverageWeight * angularComponent +
        m_config.incidenceWeight * incidence +
        m_config.stabilityWeight * stability,
        0.0f, 1.0f);
}

std::tuple<std::uint8_t, std::uint8_t, std::uint8_t>
SurfelCoverageTracker::confidenceToRgb(float confidence) const
{
    confidence = std::clamp(confidence, 0.0f, 1.0f);
    if (confidence < 0.5f) {
        const float t = confidence / 0.5f;
        return {
            static_cast<std::uint8_t>(220),
            static_cast<std::uint8_t>(70.0f + t * (210.0f - 70.0f)),
            static_cast<std::uint8_t>(70)
        };
    }

    const float t = (confidence - 0.5f) / 0.5f;
    return {
        static_cast<std::uint8_t>(220.0f + t * (70.0f - 220.0f)),
        static_cast<std::uint8_t>(210.0f + t * (190.0f - 210.0f)),
        static_cast<std::uint8_t>(70.0f + t * (90.0f - 70.0f))
    };
}

int SurfelCoverageTracker::findNearestSurfel(const Eigen::Vector3f &position) const
{
    const SurfelCellKey center = makeCellKey(position);
    int bestIndex = -1;
    float bestDistance = std::numeric_limits<float>::infinity();

    for (int dx = -1; dx <= 1; ++dx) {
        for (int dy = -1; dy <= 1; ++dy) {
            for (int dz = -1; dz <= 1; ++dz) {
                const SurfelCellKey key{center.x + dx, center.y + dy, center.z + dz};
                auto it = m_spatialIndex.find(key);
                if (it == m_spatialIndex.end()) {
                    continue;
                }
                for (int idx : it->second) {
                    const float distance = (m_surfels[idx].position - position).norm();
                    if (distance < bestDistance) {
                        bestDistance = distance;
                        bestIndex = idx;
                    }
                }
            }
        }
    }

    return bestIndex;
}

void SurfelCoverageTracker::integrateFrame(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
                                           const Eigen::Vector3f &sensorOrigin)
{
    QMutexLocker locker(&m_mutex);
    auto processed = preprocessCloud(cloud);
    if (!processed || processed->empty()) {
        return;
    }

    auto normals = estimateNormals(processed);
    if (!normals || normals->size() != processed->size()) {
        return;
    }

    ++m_frameCounter;
    const float qualityWeight = frameQualityWeight();

    for (std::size_t i = 0; i < processed->size(); ++i) {
        const auto &pt = processed->points[i];
        const auto &n = normals->points[i];
        if (!std::isfinite(n.normal_x) || !std::isfinite(n.normal_y) || !std::isfinite(n.normal_z)) {
            continue;
        }

        Eigen::Vector3f pos(pt.x, pt.y, pt.z);
        Eigen::Vector3f normal(n.normal_x, n.normal_y, n.normal_z);
        if (normal.norm() < 1e-6f) {
            continue;
        }
        normal.normalize();

        int surfelIndex = findMatchingSurfel(pos, normal);
        if (surfelIndex < 0) {
            surfelIndex = createSurfel(pos, normal);
        }
        updateSurfel(surfelIndex, pos, normal, sensorOrigin, qualityWeight, m_frameCounter);
    }

    rebuildSpatialIndex();
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr SurfelCoverageTracker::buildCoverageCloud(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud) const
{
    QMutexLocker locker(&m_mutex);
    if (!cloud || cloud->empty()) {
        return cloud;
    }

    auto colored = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>(*cloud);
    for (auto &pt : colored->points) {
        if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
            continue;
        }

        const int surfelIndex = findNearestSurfel(Eigen::Vector3f(pt.x, pt.y, pt.z));
        if (surfelIndex < 0) {
            pt.r = 255; pt.g = 170; pt.b = 0;
            continue;
        }

        const float confidence = finalConfidence(m_surfels[surfelIndex]);
        auto [r, g, b] = confidenceToRgb(confidence);
        pt.r = r;
        pt.g = g;
        pt.b = b;
    }
    return colored;
}

SurfelCoverageStats SurfelCoverageTracker::stats() const
{
    QMutexLocker locker(&m_mutex);
    SurfelCoverageStats s;
    s.surfelCount = static_cast<int>(m_surfels.size());
    if (m_surfels.empty()) {
        return s;
    }

    float sum = 0.0f;
    for (const auto &surfel : m_surfels) {
        const float c = finalConfidence(surfel);
        sum += c;
        if (c >= 0.75f) {
            ++s.goodSurfels;
        } else if (c >= 0.45f) {
            ++s.warningSurfels;
        } else {
            ++s.poorSurfels;
        }
    }
    s.meanConfidence = sum / static_cast<float>(m_surfels.size());
    return s;
}