Program Listing for File AlignBase.hpp

↰ Return to documentation for file (src/navtk/inertial/AlignBase.hpp)

#pragma once

#include <navtk/aspn.hpp>
#include <navtk/filtering/containers/ImuModel.hpp>
#include <navtk/filtering/containers/NavSolution.hpp>
#include <navtk/inertial/ImuErrors.hpp>
#include <navtk/tensors.hpp>

namespace navtk {
namespace inertial {

enum class MotionNeeded {
    NO_MOTION,
    MOTION_NEEDED,
    ANY_MOTION
};

class AlignBase {
public:
    enum class AlignmentStatus {
        ALIGNING_COARSE,
        ALIGNING_FINE,
        ALIGNED_GOOD
    };

    enum class CovarianceFormat {
        PINSON15NEDBLOCK,
        PINSON21NEDBLOCK,
    };

public:
    virtual ~AlignBase() = default;

    AlignBase(bool supports_static,
              bool supports_dynamic,
              const filtering::ImuModel& model = filtering::stim300_model());

    AlignBase(const AlignBase& other) = default;

    AlignBase(AlignBase&& other) = default;

    AlignBase& operator=(const AlignBase& other) = default;

    AlignBase& operator=(AlignBase&& other) = default;

    virtual AlignmentStatus process(std::shared_ptr<aspn_xtensor::AspnBase> message) = 0;

    bool requires_dynamic();

    AlignmentStatus check_alignment_status();

    virtual std::pair<bool, filtering::NavSolution> get_computed_alignment() const;

    virtual std::pair<bool, Matrix> get_computed_covariance(
        const CovarianceFormat format = CovarianceFormat::PINSON15NEDBLOCK) const;

    virtual std::pair<bool, ImuErrors> get_imu_errors() const;

    virtual MotionNeeded motion_needed() const = 0;

protected:
    Matrix bias_stats_from_model(
        const CovarianceFormat format = CovarianceFormat::PINSON15NEDBLOCK) const;

    AlignmentStatus alignment_status = AlignmentStatus::ALIGNING_COARSE;

    std::pair<bool, filtering::NavSolution> computed_alignment;

    std::vector<aspn_xtensor::MeasurementImu> align_buffer;

    std::vector<aspn_xtensor::MeasurementPosition> gps_buffer;

    bool supports_static;

    bool supports_dynamic;

    filtering::ImuModel model;
};

}  // namespace inertial
}  // namespace navtk