Program Listing for File SmartFactorStereoProjectionPose.h
↰ Return to documentation for file (include/mola_kernel/factors/SmartFactorStereoProjectionPose.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/img/TPixelCoord.h>
#include <mrpt/math/TPose3D.h>
#include <deque>
namespace mola
{
class BackEndBase;
class SmartFactorStereoProjectionPose : public FactorBase
{
DEFINE_SERIALIZABLE(SmartFactorStereoProjectionPose, mola)
public:
struct StereoObservation
{
double x_left{0}, x_right{0}, y{0};
};
SmartFactorStereoProjectionPose() = default;
SmartFactorStereoProjectionPose(
double sigma_xleft, double sigma_xright, double sigma_y,
BackEndBase* slam_backend,
const mrpt::math::TPose3D& cameraPoseOnRobot = mrpt::math::TPose3D());
double sigma_xleft_, sigma_xright_, sigma_y_;
std::size_t edge_count() const override;
mola::id_t edge_indices(const std::size_t i) const override;
mrpt::math::TPose3D cameraPoseOnRobot_;
void addObservation(
const StereoObservation& st, const id_t observing_kf,
const id_t camera_params);
struct obs_tuple_t
{
obs_tuple_t(const StereoObservation& pt, id_t obs_kf, id_t cam_params)
: pixel_coords(pt), observing_kf(obs_kf), camera_params(cam_params)
{
}
StereoObservation pixel_coords;
id_t observing_kf;
id_t camera_params;
};
using obs_tuple_list_t = std::deque<obs_tuple_t>;
const obs_tuple_list_t& allObservations() const
{
return all_observations_;
}
private:
BackEndBase* slam_backend_{nullptr};
obs_tuple_list_t all_observations_;
};
} // namespace mola