Program Listing for File KittiOdometryDataset.h

↰ Return to documentation for file (include/mola_input_kitti_dataset/KittiOdometryDataset.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/interfaces/Dataset_UI.h>
#include <mola_kernel/interfaces/OfflineDatasetSource.h>
#include <mola_kernel/interfaces/RawDataSourceBase.h>
#include <mrpt/core/Clock.h>
#include <mrpt/img/TCamera.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/obs/obs_frwds.h>

#include <array>

// fwrd decls:
namespace mrpt::obs
{
class CObservationPointCloud;
}

namespace mola
{
class KittiOdometryDataset : public RawDataSourceBase,
                             public OfflineDatasetSource,
                             public Dataset_UI
{
    DEFINE_MRPT_OBJECT(KittiOdometryDataset, mola)

   public:
    KittiOdometryDataset();
    ~KittiOdometryDataset() override = default;

    // See docs in base class
    void spinOnce() override;
    bool hasGroundTruthTrajectory() const override
    {
        return !groundTruthTrajectory_.empty();
    }
    trajectory_t getGroundTruthTrajectory() const override
    {
        return groundTruthTrajectory_;
    }

    std::shared_ptr<mrpt::obs::CObservation> getPointCloud(
        timestep_t step) const;
    std::shared_ptr<mrpt::obs::CObservationImage> getImage(
        const unsigned int cam_idx, timestep_t step) const;

    // See docs in base class:
    size_t datasetSize() const override;

    mrpt::obs::CSensoryFrame::Ptr datasetGetObservations(
        size_t timestep) const override;

    // Virtual interface of Dataset_UI (see docs in derived class)
    size_t datasetUI_size() const override { return datasetSize(); }
    size_t datasetUI_lastQueriedTimestep() const override
    {
        auto lck = mrpt::lockHelper(dataset_ui_mtx_);
        return last_used_tim_index_;
    }
    double datasetUI_playback_speed() const override
    {
        auto lck = mrpt::lockHelper(dataset_ui_mtx_);
        return time_warp_scale_;
    }
    void datasetUI_playback_speed(double speed) override
    {
        auto lck         = mrpt::lockHelper(dataset_ui_mtx_);
        time_warp_scale_ = speed;
    }
    bool datasetUI_paused() const override
    {
        auto lck = mrpt::lockHelper(dataset_ui_mtx_);
        return paused_;
    }
    void datasetUI_paused(bool paused) override
    {
        auto lck = mrpt::lockHelper(dataset_ui_mtx_);
        paused_  = paused;
    }
    void datasetUI_teleport(size_t timestep) override
    {
        auto lck       = mrpt::lockHelper(dataset_ui_mtx_);
        teleport_here_ = timestep;
    }

    double VERTICAL_ANGLE_OFFSET = mrpt::DEG2RAD(0.205);

   protected:
    // See docs in base class
    void initialize_rds(const Yaml& cfg) override;

   private:
    bool                initialized_ = false;
    std::string         base_dir_;
    std::string         sequence_;
    bool                clouds_as_organized_points_ = false;
    unsigned int        range_matrix_column_count_  = 2000;
    unsigned int        range_matrix_row_count_     = 64;
    timestep_t          replay_next_tim_index_{0};
    bool                publish_lidar_{true};
    bool                publish_ground_truth_{true};
    std::array<bool, 4> publish_image_{{true, true, true, true}};
    std::array<mrpt::img::TCamera, 4>  cam_intrinsics_;
    std::array<mrpt::math::TPose3D, 4> cam_poses_;

    std::optional<mrpt::Clock::time_point> last_play_wallclock_time_;
    double                                 last_dataset_time_ = 0;

    std::array<std::vector<std::string>, 4> lst_image_;
    std::vector<std::string>                lst_velodyne_;
    mrpt::math::CMatrixDouble               groundTruthPoses_;
    trajectory_t                            groundTruthTrajectory_;
    mutable std::map<timestep_t, mrpt::obs::CObservation::Ptr>
        read_ahead_lidar_obs_;
    mutable std::map<timestep_t, std::array<mrpt::obs::CObservation::Ptr, 4>>
        read_ahead_image_obs_;

    std::vector<double> lst_timestamps_;
    double              replay_time_{.0};
    std::string         seq_dir_;

    mutable timestep_t    last_used_tim_index_ = 0;
    bool                  paused_              = false;
    double                time_warp_scale_     = 1.0;
    std::optional<size_t> teleport_here_;
    mutable std::mutex    dataset_ui_mtx_;

    void load_img(const unsigned int cam_idx, const timestep_t step) const;
    void load_lidar(timestep_t step) const;

    void autoUnloadOldEntries() const;
};

}  // namespace mola