kitti_odometry_dataset.hpp

Go to the documentation of this file.

#pragma once
 
#include <cpp-toolbox/io/dataset/dataset.hpp>
#include <cpp-toolbox/io/dataset/kitti_types.hpp>
#include <cpp-toolbox/io/dataset/kitti_pose_reader.hpp>
#include <cpp-toolbox/io/dataset/kitti_exceptions.hpp>
#include <cpp-toolbox/io/formats/kitti.hpp>
#include <cpp-toolbox/io/formats/kitti_extended.hpp>
 
#include <memory>
#include <string>
#include <vector>
#include <map>
#include <optional>
#include <filesystem>
 
namespace toolbox::io {
 
template<typename DataType = float>
class kitti_odometry_dataset_t : 
    public dataset_t<kitti_odometry_dataset_t<DataType>, 
                     kitti_odometry_frame_t<DataType>> {
public:
    using frame_type = kitti_odometry_frame_t<DataType>;
    using base_type = dataset_t<kitti_odometry_dataset_t<DataType>, frame_type>;
    
    explicit kitti_odometry_dataset_t(const std::string& sequence_path);
    
    void set_cache_size(std::size_t max_cached_frames) {
        max_cache_size_ = max_cached_frames;
        // If cache is larger than new size, evict oldest entries
        while (cache_.size() > max_cache_size_) {
            auto oldest = lru_list_.back();
            cache_.erase(oldest);
            lru_list_.pop_back();
        }
    }
    
    void enable_intensity(bool enable) { load_intensity_ = enable; }
    
    void set_error_policy(error_recovery_policy_t policy) { 
        error_policy_ = policy; 
    }
    
    // ==================== Dataset Interface ====================
    
    [[nodiscard]] std::size_t size_impl() const { return cloud_files_.size(); }
    
    [[nodiscard]] std::optional<frame_type> at_impl(std::size_t index) const;
    
    // ==================== Additional Functionality ====================
    
    [[nodiscard]] std::string get_cloud_file(std::size_t index) const {
        if (index >= cloud_files_.size()) {
            throw kitti_index_out_of_range(index, cloud_files_.size());
        }
        return cloud_files_[index];
    }
    
    [[nodiscard]] Eigen::Matrix<DataType, 4, 4> get_pose(std::size_t index) const {
        return pose_reader_.get_pose(index);
    }
    
    [[nodiscard]] bool has_calibration() const { 
        return calibration_.has_value(); 
    }
    
    [[nodiscard]] const kitti_calibration_t<DataType>& get_calibration() const {
        if (!calibration_) {
            throw std::runtime_error("No calibration data available");
        }
        return *calibration_;
    }
    
    [[nodiscard]] const std::string& get_sequence_name() const { 
        return sequence_name_; 
    }
    
    [[nodiscard]] const std::string& get_sequence_path() const { 
        return sequence_path_; 
    }
    
    void clear_cache() const {
        cache_.clear();
        lru_list_.clear();
    }
 
private:
    // ==================== Private Methods ====================
    
    void scan_cloud_files();
    
    void load_poses(const std::string& poses_path);
    
    void load_calibration();
    
    std::unique_ptr<point_cloud_t<DataType>> load_cloud(
        const std::string& file_path) const;
    
    void update_cache(std::size_t index, frame_type&& frame) const;
    
    std::optional<frame_type> get_from_cache(std::size_t index) const;
    
    // ==================== Member Variables ====================
    
    std::string sequence_path_;
    
    std::string velodyne_path_;
    
    std::string sequence_name_;
    
    kitti_pose_reader_t<DataType> pose_reader_;
    
    std::optional<kitti_calibration_t<DataType>> calibration_;
    
    std::vector<std::string> cloud_files_;
    
    mutable std::map<std::size_t, frame_type> cache_;
    
    mutable std::list<std::size_t> lru_list_;
    
    std::size_t max_cache_size_ = 100;
    
    bool load_intensity_ = true;
    error_recovery_policy_t error_policy_ = error_recovery_policy_t::lenient;
};
 
} // namespace toolbox::io
 
// Include implementation
#include <cpp-toolbox/io/dataloader/impl/kitti_odometry_dataset_impl.hpp>