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