Program Listing for File RotationIntegrator.h

↰ Return to documentation for file (include/mola_imu_preintegration/RotationIntegrator.h)

/* -------------------------------------------------------------------------
 *   A Modular Optimization framework for Localization and mApping  (MOLA)
 *
 * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
 * Licensed under the GNU GPL v3 for non-commercial applications.
 *
 * This file is part of MOLA.
 * MOLA is free software: you can redistribute it and/or modify it under the
 * terms of the GNU General Public License as published by the Free Software
 * Foundation, either version 3 of the License, or (at your option) any later
 * version.
 *
 * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
 * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along with
 * MOLA. If not, see <https://www.gnu.org/licenses/>.
 * ------------------------------------------------------------------------- */
#pragma once

#include <mola_imu_preintegration/RotationIntegrationParams.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/core/optional_ref.h>

namespace mola
{
class RotationIntegrator
{
   public:
    RotationIntegrator()  = default;
    ~RotationIntegrator() = default;

    struct IntegrationState
    {
        IntegrationState()  = default;
        ~IntegrationState() = default;

        double deltaTij_ = 0;

        mrpt::math::CMatrixDouble33 deltaRij_ =
            mrpt::math::CMatrixDouble33::Identity();

        // (TODO)
        // mrpt::math::CMatrixDouble33 delRdelBiasOmega_ =
        // mrpt::math::CMatrixDouble33::Zero();
    };

    void initialize(const mrpt::containers::yaml& cfg);

    void reset_integration();

    const IntegrationState& current_integration_state() const { return state_; }

    void integrate_measurement(const mrpt::math::TVector3D& w, double dt);

    RotationIntegrationParams params_;

   private:
    IntegrationState state_;
};

mrpt::math::CMatrixDouble33 incremental_rotation(
    const mrpt::math::TVector3D& w, const RotationIntegrationParams& params,
    double dt,
    const mrpt::optional_ref<mrpt::math::CMatrixDouble33>&
        D_incrR_integratedOmega = std::nullopt);

}  // namespace mola