Program Listing for File FactorStereoProjectionPose.h

↰ Return to documentation for file (include/mola_kernel/factors/FactorStereoProjectionPose.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
{
struct StereoObservation
{
    double x_left{0}, x_right{0}, y{0};
};

class FactorStereoProjectionPose : public FactorBase
{
    DEFINE_SERIALIZABLE(FactorStereoProjectionPose, mola)

   public:
    FactorStereoProjectionPose() = default;

    FactorStereoProjectionPose(
        double sigma_xleft, double sigma_xright, double sigma_y,
        const StereoObservation& st, const id_t observing_kf,
        const id_t observed_landmark, const id_t camera_params,
        const mrpt::math::TPose3D& cameraPoseOnRobot = mrpt::math::TPose3D())
        : sigma_xleft_(sigma_xleft),
          sigma_xright_(sigma_xright),
          sigma_y_(sigma_y),
          observation_(st),
          observing_kf_(observing_kf),
          observed_landmark_(observed_landmark),
          camera_params_id_(camera_params),
          cameraPoseOnRobot_(cameraPoseOnRobot)
    {
    }

    double sigma_xleft_, sigma_xright_, sigma_y_;

    StereoObservation   observation_;
    mola::id_t          observing_kf_;
    mola::id_t          observed_landmark_;
    mola::id_t          camera_params_id_;
    mrpt::math::TPose3D cameraPoseOnRobot_;

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

}  // namespace mola