Program Listing for File FactorRelativePose3.h

↰ Return to documentation for file (include/mola_kernel/factors/FactorRelativePose3.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 <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/TPose3D.h>

#include <optional>

namespace mola
{
class FactorRelativePose3 : public FactorBase
{
    DEFINE_SERIALIZABLE(FactorRelativePose3, mola)

   public:
    FactorRelativePose3() = default;

    FactorRelativePose3(
        id_t kf_from, id_t kf_to, const mrpt::math::TPose3D& rel_pose)
        : from_kf_(kf_from), to_kf_{kf_to}, rel_pose_{rel_pose}
    {
    }

    id_t                from_kf_{INVALID_ID}, to_kf_{INVALID_ID};
    mrpt::math::TPose3D rel_pose_;

    std::optional<mrpt::math::CMatrixDouble66> noise_model_;

    double noise_model_diag_xyz_{0.01};
    double noise_model_diag_rot_{mrpt::DEG2RAD(0.5)};

    std::size_t edge_count() const override { return 2; }
    mola::id_t  edge_indices(const std::size_t i) const override;
};

}  // namespace mola