kitti_pose_reader.hpp

Go to the documentation of this file.

#pragma once
 
#include <cpp-toolbox/io/dataset/kitti_types.hpp>
#include <cpp-toolbox/io/dataset/kitti_exceptions.hpp>
#include <cpp-toolbox/io/formats/kitti_extended.hpp>
 
#include <Eigen/Dense>
#include <string>
#include <vector>
#include <optional>
 
namespace toolbox::io {
 
template<typename DataType>
class kitti_pose_reader_t {
public:
    kitti_pose_reader_t() = default;
    
    bool load(const std::string& poses_file);
    
    [[nodiscard]] std::size_t size() const noexcept { return poses_.size(); }
    
    [[nodiscard]] bool empty() const noexcept { return poses_.empty(); }
    
    [[nodiscard]] Eigen::Matrix<DataType, 4, 4> get_pose(std::size_t index) const;
    
    [[nodiscard]] std::optional<Eigen::Matrix<DataType, 4, 4>> 
    try_get_pose(std::size_t index) const;
    
    [[nodiscard]] Eigen::Matrix<DataType, 4, 4> get_relative_transform(
        std::size_t from_index, 
        std::size_t to_index) const;
    
    [[nodiscard]] bool is_valid_index(std::size_t index) const noexcept {
        return index < poses_.size();
    }
    
    void clear() noexcept { poses_.clear(); }
    
    [[nodiscard]] const std::vector<Eigen::Matrix<DataType, 4, 4>>& 
    get_all_poses() const noexcept { return poses_; }
    
    [[nodiscard]] DataType compute_trajectory_length() const;
    
    [[nodiscard]] std::pair<Eigen::Vector3<DataType>, Eigen::Vector3<DataType>> 
    get_trajectory_bounds() const;
 
private:
    std::vector<Eigen::Matrix<DataType, 4, 4>> poses_;
};
 
} // namespace toolbox::io
 
// Include implementation
#include <cpp-toolbox/io/dataset/impl/kitti_pose_reader_impl.hpp>