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

src/tracking/SurfelCoverageTracker.h
164 строки5 KB
k k
fix(high): 6 high-severity bugs
13 июл 2026, 23:43
13 июл 2026, 23:43b439ddb
#ifndef SURFELCOVERAGETRACKER_H
#define SURFELCOVERAGETRACKER_H

#include <Eigen/Dense>
#include <cstdint>
#include <memory>
#include <tuple>
#include <unordered_map>
#include <vector>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <QMutex>

struct SurfelCoverageConfig
{
    float surfelRadius = 0.008f;
    float associationDistance = 0.012f;
    float normalAngleThresholdDeg = 35.0f;
    float downsampleLeaf = 0.004f;
    int maxIntegrationPoints = 3000;
    int angularBins = 16;
    float minDistinctAngleDeg = 15.0f;

    float observationWeight = 0.55f;
    float angularCoverageWeight = 0.25f;
    float incidenceWeight = 0.10f;
    float stabilityWeight = 0.10f;
};

struct SurfelCellKey
{
    int x = 0;
    int y = 0;
    int z = 0;

    bool operator==(const SurfelCellKey &other) const
    {
        return x == other.x && y == other.y && z == other.z;
    }
};

struct SurfelCellKeyHash
{
    std::size_t operator()(const SurfelCellKey &key) const noexcept;
};

struct SurfelRecord
{
    Eigen::Vector3f position = Eigen::Vector3f::Zero();
    Eigen::Vector3f normal = Eigen::Vector3f::UnitZ();

    int observations = 0;
    float confidenceSum = 0.0f;
    float incidenceScoreSum = 0.0f;
    float stabilityScoreSum = 0.0f;

    std::vector<float> angularHistogram;
    uint32_t lastSeenFrame = 0;
};

struct SurfelCoverageStats
{
    int surfelCount = 0;
    int goodSurfels = 0;
    int warningSurfels = 0;
    int poorSurfels = 0;
    float meanConfidence = 0.0f;
};

class SurfelCoverageTracker
{
public:
    explicit SurfelCoverageTracker(const SurfelCoverageConfig &config = {});

    void reset();
    void setConfig(const SurfelCoverageConfig &config);
    const SurfelCoverageConfig& config() const { return m_config; }

    void setFrameQualityWeights(int trackingLevel,
                                int alignmentLevel,
                                int depthLevel,
                                int driftLevel);

    void integrateFrame(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
                        const Eigen::Vector3f &sensorOrigin);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr buildCoverageCloud(
        const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud) const;

    SurfelCoverageStats stats() const;

private:
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr preprocessCloud(
        const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud) const;

    pcl::PointCloud<pcl::Normal>::Ptr estimateNormals(
        const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud) const;

    SurfelCellKey makeCellKey(const Eigen::Vector3f &position) const;
    void rebuildSpatialIndex();

    int findMatchingSurfel(const Eigen::Vector3f &position,
                           const Eigen::Vector3f &normal) const;

    bool surfelMatches(const SurfelRecord &surfel,
                       const Eigen::Vector3f &position,
                       const Eigen::Vector3f &normal) const;

    int createSurfel(const Eigen::Vector3f &position,
                     const Eigen::Vector3f &normal);

    void updateSurfel(int surfelIndex,
                      const Eigen::Vector3f &position,
                      const Eigen::Vector3f &normal,
                      const Eigen::Vector3f &sensorOrigin,
                      float frameQualityWeight,
                      uint32_t frameIndex);

    float angleBetweenNormalsDeg(const Eigen::Vector3f &a,
                                 const Eigen::Vector3f &b) const;

    Eigen::Vector3f computeViewDirection(const Eigen::Vector3f &point,
                                         const Eigen::Vector3f &sensorOrigin) const;

    void buildLocalFrame(const Eigen::Vector3f &normal,
                         Eigen::Vector3f &tangent,
                         Eigen::Vector3f &bitangent) const;

    int angularBinForViewDirection(const SurfelRecord &surfel,
                                   const Eigen::Vector3f &viewDir) const;

    float incidenceScore(const Eigen::Vector3f &normal,
                         const Eigen::Vector3f &viewDir) const;

    float frameQualityWeight() const;

    float observationConfidence(const SurfelRecord &surfel) const;
    float angularCoverageConfidence(const SurfelRecord &surfel) const;
    float angularDiversityConfidence(const SurfelRecord &surfel) const;
    float averageIncidenceConfidence(const SurfelRecord &surfel) const;
    float averageStabilityConfidence(const SurfelRecord &surfel) const;

    float finalConfidence(const SurfelRecord &surfel) const;

    std::tuple<std::uint8_t, std::uint8_t, std::uint8_t>
    confidenceToRgb(float confidence) const;

    int findNearestSurfel(const Eigen::Vector3f &position) const;

private:
    mutable QMutex m_mutex;
    SurfelCoverageConfig m_config;
    std::vector<SurfelRecord> m_surfels;
    std::unordered_map<SurfelCellKey, std::vector<int>, SurfelCellKeyHash> m_spatialIndex;

    uint32_t m_frameCounter = 0;

    int m_lastTrackingLevel = 1;
    int m_lastAlignmentLevel = 1;
    int m_lastDepthLevel = 1;
    int m_lastDriftLevel = 1;
};

#endif // SURFELCOVERAGETRACKER_H