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