Program Listing for File SmartFactorIMU.h

↰ Return to documentation for file (include/mola_kernel/factors/SmartFactorIMU.h)

/* -------------------------------------------------------------------------
 *   A Modular Optimization framework for Localization and mApping  (MOLA)
 * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#pragma once

#include <mola_kernel/factors/FactorBase.h>

#include <deque>

namespace mola
{
class BackEndBase;

class SmartFactorIMU : public FactorBase
{
    DEFINE_SERIALIZABLE(SmartFactorIMU, mola)

   public:
    SmartFactorIMU() = default;

    // TODO: Add configurable noise model & IMU pose:
    SmartFactorIMU(BackEndBase* slam_backend);

    std::size_t edge_count() const override;
    mola::id_t  edge_indices(const std::size_t i) const override;

    void integrateMeasurement(
        double accx, double accy, double accz, double wx, double wy, double wz,
        double dt);

    void createIMUFactor(mola::id_t prev_pose_kf, mola::id_t new_pose_kf);

    enum NewState
    {
        NONE = 0,
        MEASURE,
        FACTOR
    };

    NewState new_state_{NewState::NONE};

    double     ax_, ay_, az_, wx_, wy_, wz_, dt_;
    mola::id_t prev_pose_kf_, new_pose_kf_;

   private:
    BackEndBase* slam_backend_{nullptr};
};

}  // namespace mola