localization.hpp

Go to the documentation of this file.

#pragma once
 
#include "foundation/types.hpp"
#include "libstp/map/Geometry.hpp"
#include "libstp/map/WorldMap.hpp"
#include "hal/odometry.hpp"
#include "threading/thread_manager.hpp"
 
#include <Eigen/Core>
#include <cstdint>
#include <memory>
#include <mutex>
#include <optional>
#include <random>
#include <string>
#include <vector>
 
namespace libstp::localization {
 
class LocalizationRecorder;
struct RecorderConfig;

struct Observation {
    enum class SurfaceKind : uint8_t {
        Line = 0,
        Wall = 1,
    };
 
    struct SurfaceMeasurement {
        SurfaceKind kind{SurfaceKind::Line};
        libstp::map::SensorOffset sensor{};
        bool detected{true};
        double sigma_cm{1.0};
    };
 
    libstp::foundation::Pose pose{};
    Eigen::Vector3d sigma{1e-3, 1e-3, 1e-3};
    std::vector<SurfaceMeasurement> surface_measurements;
};
 
struct LocalizationConfig {
    int tick_period_ms{10};  
    int particle_count{128};
    double process_translation_noise_m{0.002};
    double process_translation_noise_per_m{0.02};
    double process_heading_noise_rad{0.01};
    double process_heading_noise_per_rad{0.05};
    double observation_injection_ratio{0.35};
    double resample_effective_sample_ratio{0.5};
    uint32_t rng_seed{0x5EED1234u};
};

class Localization {
public:
    Localization(std::shared_ptr<libstp::odometry::IOdometry> odometry,
                 LocalizationConfig config = {},
                 std::optional<libstp::map::WorldMap> tableMap = std::nullopt);
    ~Localization();
 
    Localization(const Localization&) = delete;
    Localization& operator=(const Localization&) = delete;
    Localization(Localization&&) = delete;
    Localization& operator=(Localization&&) = delete;

    [[nodiscard]] libstp::foundation::Pose getPose() const;

    void observe(const Observation& obs);

    void start();

    void stop();

    bool enableRecording(RecorderConfig cfg);
 
private:
    struct Particle {
        libstp::foundation::Pose pose{};
        double weight{1.0};
    };
 
    void initializeLocked(const libstp::foundation::Pose& anchorPose);
    void initializeParticlesLocked(const libstp::foundation::Pose& anchorPose,
                                   const Eigen::Vector3d& sigma);
    void predictLocked(const libstp::foundation::Pose& odomDelta);
    void integrateObservationLocked(const Observation& obs);
    [[nodiscard]] double computeParticleLogWeight(const Particle& particle,
                                                  const Observation& obs) const;
    void normalizeWeightsLocked();
    void resampleLocked();
    void updateEstimateLocked();
    void tickLoop(libstp::threading::stop_token stop);
 
    std::shared_ptr<libstp::odometry::IOdometry> m_odometry;
    LocalizationConfig m_config;
 
    mutable std::mutex m_mutex;
    libstp::foundation::Pose m_worldPose{};
    libstp::foundation::Pose m_lastOdom{};
    std::vector<Particle> m_particles;
    bool m_initialized{false};
    std::mt19937 m_rng;
    std::optional<libstp::map::WorldMap> m_tableMap;
 
    // Post-run debug recorder. nullptr unless enableRecording() was called.
    // Snapshots are produced under m_mutex from tickLoop; the recorder's
    // own writer thread does the I/O.
    std::unique_ptr<LocalizationRecorder> m_recorder;
    // Pending observation snapshot for the next recorded frame. observe()
    // appends, tickLoop drains. Mirrors the brief's contract: resync points
    // are never lost to downsampling.
    std::vector<Observation::SurfaceMeasurement> m_pendingObservations;
    std::optional<libstp::foundation::Pose> m_pendingObservationPose;
    std::optional<Eigen::Vector3d> m_pendingObservationSigma;
    bool m_pendingObservationArrived{false};
    bool m_pendingResampled{false};
    std::optional<libstp::foundation::Pose> m_lastOdomDelta;
 
    // Background worker routed through ThreadManager: dropping this handle
    // requests stop on the libstp::threading::jthread and joins it. The destructor relies
    // on that RAII contract; ``stop()`` calls it explicitly to keep the
    // start()/stop() API idempotent.
    libstp::threading::ThreadManager::DaemonHandle m_daemon;
};
 
}  // namespace libstp::localization