kitti_extended.hpp

Go to the documentation of this file.

#pragma once
 
#include <cpp-toolbox/io/formats/kitti.hpp>
#include <cpp-toolbox/types/point.hpp>
 
#include <Eigen/Dense>
#include <filesystem>
#include <string>
#include <vector>
#include <cstdint>
#include <optional>
 
namespace toolbox::io {
 
// ==================== Label I/O ====================
 
std::vector<uint32_t> read_kitti_labels(const std::string& file_path);
 
bool write_kitti_labels(const std::string& file_path,
                       const std::vector<uint32_t>& labels);
 
// ==================== Pose I/O ====================
 
template<typename DataType>
Eigen::Matrix<DataType, 4, 4> parse_kitti_pose_line(const std::string& line);
 
template<typename DataType>
std::vector<Eigen::Matrix<DataType, 4, 4>> read_kitti_poses(const std::string& file_path);
 
template<typename DataType>
std::string format_kitti_pose(const Eigen::Matrix<DataType, 4, 4>& pose);
 
template<typename DataType>
bool write_kitti_poses(const std::string& file_path,
                      const std::vector<Eigen::Matrix<DataType, 4, 4>>& poses);
 
// ==================== Calibration I/O ====================
 
template<typename DataType>
struct kitti_calibration_t {
    Eigen::Matrix<DataType, 4, 4> Tr_velo_to_cam;
    
    Eigen::Matrix<DataType, 3, 4> P0, P1, P2, P3;
    
    Eigen::Matrix<DataType, 3, 3> R0_rect;
    
    std::optional<Eigen::Matrix<DataType, 4, 4>> Tr_imu_to_velo;
};
 
template<typename DataType>
kitti_calibration_t<DataType> read_kitti_calibration(const std::string& file_path);
 
template<typename DataType>
bool write_kitti_calibration(const std::string& file_path,
                            const kitti_calibration_t<DataType>& calib);
 
// ==================== Utility Functions ====================
 
template<typename DataType>
Eigen::Matrix<DataType, 4, 4> compute_relative_transform(
    const Eigen::Matrix<DataType, 4, 4>& from_pose,
    const Eigen::Matrix<DataType, 4, 4>& to_pose);
 
template<typename DataType>
std::unique_ptr<point_cloud_t<DataType>> transform_point_cloud(
    const point_cloud_t<DataType>& cloud,
    const Eigen::Matrix<DataType, 4, 4>& transform);
 
template<typename DataType = float>
std::unique_ptr<point_cloud_t<DataType>> read_kitti_with_labels(
    const std::string& bin_path,
    const std::string& label_path,
    std::vector<uint32_t>& labels);
 
std::vector<std::string> list_kitti_cloud_files(const std::string& velodyne_path);
 
std::vector<std::string> list_kitti_label_files(const std::string& labels_path);
 
int parse_kitti_frame_index(const std::string& filename);
 
std::string format_kitti_frame_index(std::size_t index, int digits = 6);
 
struct kitti_sequence_info_t {
    std::size_t num_frames = 0;      
    bool has_labels = false;         
    bool has_calibration = false;    
    std::string sequence_name;       
    std::filesystem::path path;      
};
 
kitti_sequence_info_t get_kitti_sequence_info(const std::string& sequence_path);
 
bool validate_kitti_sequence_directory(const std::string& sequence_path);
 
// ==================== Label Definitions ====================
 
namespace kitti_semantic_labels {
    // Static classes
    constexpr uint16_t UNLABELED = 0;
    constexpr uint16_t OUTLIER = 1;
    constexpr uint16_t CAR = 10;
    constexpr uint16_t BICYCLE = 11;
    constexpr uint16_t BUS = 13;
    constexpr uint16_t MOTORCYCLE = 15;
    constexpr uint16_t TRUCK = 18;
    constexpr uint16_t OTHER_VEHICLE = 20;
    constexpr uint16_t PERSON = 30;
    constexpr uint16_t BICYCLIST = 31;
    constexpr uint16_t MOTORCYCLIST = 32;
    constexpr uint16_t ROAD = 40;
    constexpr uint16_t PARKING = 44;
    constexpr uint16_t SIDEWALK = 48;
    constexpr uint16_t OTHER_GROUND = 49;
    constexpr uint16_t BUILDING = 50;
    constexpr uint16_t FENCE = 51;
    constexpr uint16_t OTHER_STRUCTURE = 52;
    constexpr uint16_t VEGETATION = 70;
    constexpr uint16_t TRUNK = 71;
    constexpr uint16_t TERRAIN = 72;
    constexpr uint16_t POLE = 80;
    constexpr uint16_t TRAFFIC_SIGN = 81;
    
    inline bool is_static(uint16_t label) {
        return label == ROAD || label == PARKING || label == SIDEWALK ||
               label == OTHER_GROUND || label == BUILDING || label == FENCE ||
               label == OTHER_STRUCTURE || label == VEGETATION || label == TRUNK ||
               label == TERRAIN || label == POLE || label == TRAFFIC_SIGN;
    }
    
    inline bool is_dynamic(uint16_t label) {
        return label == CAR || label == BICYCLE || label == BUS ||
               label == MOTORCYCLE || label == TRUCK || label == OTHER_VEHICLE ||
               label == PERSON || label == BICYCLIST || label == MOTORCYCLIST;
    }
    
    std::string get_label_name(uint16_t label);
    
    std::map<uint16_t, std::string> get_label_map();
} // namespace kitti_semantic_labels
 
inline uint16_t get_kitti_label_id(uint32_t full_label) {
    return static_cast<uint16_t>(full_label & 0xFFFF);
}
 
inline uint16_t get_kitti_instance_id(uint32_t full_label) {
    return static_cast<uint16_t>(full_label >> 16);
}
 
inline uint32_t make_kitti_full_label(uint16_t label_id, uint16_t instance_id) {
    return (static_cast<uint32_t>(instance_id) << 16) | label_id;
}
 
} // namespace toolbox::io
 
// Include implementation
#include <cpp-toolbox/io/formats/detail/kitti_extended_impl.hpp>