Skip to content

Commit 61d52ae

Browse files
nerdneilsfieldnerdneilsfield
committed
feat: add KITTI odometry dataset loader foundation
- Add KITTI data structures and types (frames, frame pairs, semantic variants) - Add KITTI format extensions for poses, labels, and calibration I/O - Implement pose reader component with trajectory analysis - Add base KITTI odometry dataset class with caching support - Organize code: formats in io/formats, datasets in io/dataloader - Support error recovery policies and configurable options Part of KITTI dataset loader implementation.
1 parent 3be7b4e commit 61d52ae

File tree

9 files changed

+1921
-0
lines changed

9 files changed

+1921
-0
lines changed

‎src/include/cpp-toolbox/io/dataloader/impl/kitti_odometry_dataset_impl.hpp‎

Lines changed: 224 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,224 @@
1+
#pragma once
2+
3+
#include <cpp-toolbox/io/dataloader/kitti_odometry_dataset.hpp>
4+
#include <cpp-toolbox/logger/thread_logger.hpp>
5+
6+
#include <filesystem>
7+
#include <algorithm>
8+
9+
namespace toolbox::io {
10+
11+
template<typename DataType>
12+
kitti_odometry_dataset_t<DataType>::kitti_odometry_dataset_t(
13+
const std::string& sequence_path)
14+
: sequence_path_(sequence_path) {
15+
16+
namespace fs = std::filesystem;
17+
18+
// Validate directory structure
19+
if (!validate_kitti_sequence_directory(sequence_path)) {
20+
throw kitti_invalid_sequence(sequence_path);
21+
}
22+
23+
// Extract sequence name
24+
fs::path path(sequence_path);
25+
sequence_name_ = path.filename().string();
26+
27+
// Set paths
28+
velodyne_path_ = (path / "velodyne").string();
29+
30+
// Scan for point cloud files
31+
scan_cloud_files();
32+
33+
if (cloud_files_.empty()) {
34+
throw kitti_invalid_sequence("No point cloud files found in: " + velodyne_path_);
35+
}
36+
37+
// Load poses
38+
// Poses are typically in the parent directory structure
39+
fs::path poses_dir = path.parent_path().parent_path() / "poses";
40+
std::string poses_file = (poses_dir / (sequence_name_ + ".txt")).string();
41+
42+
if (fs::exists(poses_file)) {
43+
load_poses(poses_file);
44+
} else {
45+
LOG_WARN_S << "No poses file found at: " << poses_file;
46+
}
47+
48+
// Load calibration
49+
load_calibration();
50+
51+
LOG_INFO_S << "Loaded KITTI sequence " << sequence_name_
52+
<< " with " << cloud_files_.size() << " frames";
53+
}
54+
55+
template<typename DataType>
56+
void kitti_odometry_dataset_t<DataType>::scan_cloud_files() {
57+
cloud_files_ = list_kitti_cloud_files(velodyne_path_);
58+
59+
// Verify files are properly numbered
60+
for (std::size_t i = 0; i < cloud_files_.size(); ++i) {
61+
int frame_idx = parse_kitti_frame_index(cloud_files_[i]);
62+
if (frame_idx != static_cast<int>(i)) {
63+
LOG_WARN_S << "Frame index mismatch: expected " << i
64+
<< " but got " << frame_idx
65+
<< " for file " << cloud_files_[i];
66+
}
67+
}
68+
}
69+
70+
template<typename DataType>
71+
void kitti_odometry_dataset_t<DataType>::load_poses(const std::string& poses_file) {
72+
if (!pose_reader_.load(poses_file)) {
73+
LOG_ERROR_S << "Failed to load poses from: " << poses_file;
74+
return;
75+
}
76+
77+
// Verify pose count matches frame count
78+
if (pose_reader_.size() != cloud_files_.size()) {
79+
LOG_WARN_S << "Pose count (" << pose_reader_.size()
80+
<< ") does not match frame count (" << cloud_files_.size() << ")";
81+
}
82+
}
83+
84+
template<typename DataType>
85+
void kitti_odometry_dataset_t<DataType>::load_calibration() {
86+
namespace fs = std::filesystem;
87+
88+
fs::path calib_file = fs::path(sequence_path_) / "calib.txt";
89+
90+
if (fs::exists(calib_file)) {
91+
try {
92+
calibration_ = read_kitti_calibration<DataType>(calib_file.string());
93+
LOG_INFO_S << "Loaded calibration from: " << calib_file;
94+
} catch (const std::exception& e) {
95+
LOG_WARN_S << "Failed to load calibration: " << e.what();
96+
}
97+
}
98+
}
99+
100+
template<typename DataType>
101+
std::optional<kitti_odometry_frame_t<DataType>>
102+
kitti_odometry_dataset_t<DataType>::at_impl(std::size_t index) const {
103+
104+
if (index >= cloud_files_.size()) {
105+
if (error_policy_ == error_recovery_policy_t::strict) {
106+
throw kitti_index_out_of_range(index, cloud_files_.size());
107+
}
108+
return std::nullopt;
109+
}
110+
111+
// Check cache first
112+
auto cached = get_from_cache(index);
113+
if (cached) {
114+
return cached;
115+
}
116+
117+
// Load point cloud
118+
auto cloud = load_cloud(cloud_files_[index]);
119+
if (!cloud) {
120+
if (error_policy_ == error_recovery_policy_t::strict) {
121+
throw kitti_corrupted_data("Failed to load cloud at index " +
122+
std::to_string(index));
123+
}
124+
return std::nullopt;
125+
}
126+
127+
// Create frame
128+
frame_type frame;
129+
frame.cloud = std::move(cloud);
130+
frame.frame_index = index;
131+
132+
// Get pose if available
133+
if (index < pose_reader_.size()) {
134+
frame.pose = pose_reader_.get_pose(index);
135+
} else {
136+
// Use identity if no pose available
137+
frame.pose = Eigen::Matrix<DataType, 4, 4>::Identity();
138+
if (error_policy_ != error_recovery_policy_t::best_effort) {
139+
LOG_WARN_S << "No pose available for frame " << index;
140+
}
141+
}
142+
143+
// Set timestamp (frame index as string for now)
144+
frame.timestamp = format_kitti_frame_index(index);
145+
146+
// Update cache before returning
147+
update_cache(index, std::move(frame));
148+
149+
// Return from cache to avoid move issues
150+
return get_from_cache(index);
151+
}
152+
153+
template<typename DataType>
154+
std::unique_ptr<point_cloud_t<DataType>>
155+
kitti_odometry_dataset_t<DataType>::load_cloud(
156+
const std::string& file_path) const {
157+
158+
try {
159+
auto cloud = read_kitti_bin<DataType>(file_path);
160+
161+
if (!cloud) {
162+
LOG_ERROR_S << "Failed to read point cloud from: " << file_path;
163+
return nullptr;
164+
}
165+
166+
// Optionally clear intensity if not needed
167+
if (!load_intensity_) {
168+
cloud->intensity = 0;
169+
}
170+
171+
return cloud;
172+
173+
} catch (const std::exception& e) {
174+
LOG_ERROR_S << "Exception loading point cloud from " << file_path
175+
<< ": " << e.what();
176+
return nullptr;
177+
}
178+
}
179+
180+
template<typename DataType>
181+
void kitti_odometry_dataset_t<DataType>::update_cache(
182+
std::size_t index, frame_type&& frame) const {
183+
184+
// Remove from LRU list if already present
185+
auto it = std::find(lru_list_.begin(), lru_list_.end(), index);
186+
if (it != lru_list_.end()) {
187+
lru_list_.erase(it);
188+
}
189+
190+
// Add to front of LRU list
191+
lru_list_.push_front(index);
192+
193+
// Insert/update in cache
194+
cache_[index] = std::move(frame);
195+
196+
// Evict oldest if cache is full
197+
while (cache_.size() > max_cache_size_ && !lru_list_.empty()) {
198+
auto oldest = lru_list_.back();
199+
cache_.erase(oldest);
200+
lru_list_.pop_back();
201+
}
202+
}
203+
204+
template<typename DataType>
205+
std::optional<kitti_odometry_frame_t<DataType>>
206+
kitti_odometry_dataset_t<DataType>::get_from_cache(std::size_t index) const {
207+
208+
auto it = cache_.find(index);
209+
if (it != cache_.end()) {
210+
// Move to front of LRU list
211+
auto lru_it = std::find(lru_list_.begin(), lru_list_.end(), index);
212+
if (lru_it != lru_list_.end()) {
213+
lru_list_.erase(lru_it);
214+
}
215+
lru_list_.push_front(index);
216+
217+
// Return copy to maintain cache
218+
return it->second;
219+
}
220+
221+
return std::nullopt;
222+
}
223+
224+
} // namespace toolbox::io

0 commit comments

Comments
 (0)