Program Listing for File ManualAlignment.hpp

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

#pragma once

#include <navtk/aspn.hpp>
#include <navtk/inertial/AlignBase.hpp>
#include <navtk/tensors.hpp>

namespace navtk {
namespace inertial {

class ManualAlignment : public AlignBase {
public:
    ManualAlignment(const aspn_xtensor::MeasurementPositionVelocityAttitude& pva,
                    bool wait_for_time               = false,
                    bool wait_for_pos                = false,
                    bool wait_for_vel                = false,
                    bool wait_for_att                = false,
                    const filtering::ImuModel& model = filtering::stim300_model());

    AlignBase::AlignmentStatus process(std::shared_ptr<aspn_xtensor::AspnBase> message) override;

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

    MotionNeeded motion_needed() const override;

private:
    /* The timestamp of the most recent IMU message received */
    aspn_xtensor::TypeTimestamp last_imu_time = aspn_xtensor::to_type_timestamp();

    /* Flag indicating if last_imu_time can be used to time-tag generated NavSolutions */
    bool use_imu_time;

    /* Copy of PVA covariance from sensor messages for use in solution covariance output */
    Matrix cov;

    /* Storage for user-provided constructor parameters */
    std::vector<bool> wait_for;

    /* Log/throw if desired, otherwise update solution time and status */
    AlignBase::AlignmentStatus post_meas_received(
        bool should_throw,
        const aspn_xtensor::TypeTimestamp& meas_time = aspn_xtensor::to_type_timestamp());

    /*
     * Figure out what data is valid and assign into alignment pva/covariance.
     *
     * @param pva Candidate PositionVelocityAttitude.
     * @param set_flags Whether or not to allow the function to change the wait_for flags based on
     * supplied_data.
     * @return bool indicating that all valid data was assigned, effectively only false when the
     * covariance matrix size does not match the number of valid data elements.
     */
    bool assign_from_pva(const aspn_xtensor::MeasurementPositionVelocityAttitude& pva,
                         bool set_flags = true);
};
}  // namespace inertial
}  // namespace navtk