From 09fb29822df4e2a96c107a168621ee57a1990c82 Mon Sep 17 00:00:00 2001 From: Adam Dabrowski Date: Mon, 5 Oct 2026 12:27:53 +0200 Subject: [PATCH 1/2] Lidar odometry: bounded memory for long recordings, results unchanged lidar_odometry_step_1 loads every raw point-cloud file before step 1 starts, and step 2's map buffer (points_global) only shrinks at a sliding-window reset. A long recording from a slow-moving platform (~50 min, ~29,000 frames) needs more than 60 GB of RAM and the run is killed part-way. Two parameters in the [performance] section of LidarOdometryParams, saved with every run and documented in doc/virtual_memory.md: - lazy_load_raw_clouds (default true): each raw file is loaded when step 1 reaches its time range and freed once step 1 has passed it. Files are loaded and sorted exactly as before, so step 1 sees the same points in the same order. The raw data viewer sets it to false and keeps every file. - points_global_spill_directory (default "", in memory): when set, step 2's map buffer is kept in a temporary file there, owned by the compute_step_2 call, and streamed back in order at a reset with the ray-cast decision of its whole size. An I/O error ends step 2 with an error (no abort) and the files are removed on every exit; the final close is checked. Lazy loading reads each file twice and refuses a file that changed in between (size, modification time before and after the read, point count, time range). The step 1 GUI, its console mode and the precision-forestry app now act on a failed step 1 or step 2: the console reports "no results saved" and exits 1, and the GUI neither marks the step done nor saves a partial result. Also: the two 101^3 normal-vector histograms are cleared by resetting only the bins written in the previous iteration; the raw clouds are released after a successful step 1 (release_raw_clouds: run_lidar_odometry, the step 1 GUI and console, the precision-forestry app), since nothing later reads them; both parameters are exposed in the Python bindings. Heap trimming is glibc-only; file seeks are 64-bit (_fseeki64 / fseeko, range-checked where off_t is narrower). Every trajectory, scan and pose file is byte-identical to the unpatched build on a recording window, with the defaults, lazy loading off, the spill directory set, and resets forced by a 20 m window; only metadata differs. (When the per-frame time limit real_time_threshold_seconds is reached, the iteration count depends on machine speed with or without this change.) Peak memory there: 2.2 GB unpatched, 1.3 GB with the defaults, 0.75 GB with the spill directory set. Co-Authored-By: Claude Opus 5.5 (1M context) --- ..._data_and_drop_here-precision_forestry.cpp | 33 ++- apps/lidar_odometry_step_1/lidar_odometry.cpp | 217 ++++++++++++++++-- apps/lidar_odometry_step_1/lidar_odometry.h | 5 +- .../lidar_odometry_gui.cpp | 45 +++- .../lidar_odometry_utils.cpp | 40 +++- .../lidar_odometry_utils.h | 34 +++ .../lidar_odometry_utils_optimizers.cpp | 214 ++++++++++++++++- apps/lidar_odometry_step_1/toml_io.h | 4 +- .../mandeye_raw_data_viewer.cpp | 1 + doc/virtual_memory.md | 33 ++- pybind/lidar_odometry_binding.cpp | 2 + 11 files changed, 580 insertions(+), 48 deletions(-) diff --git a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp index 9d47f5b7..49821da1 100644 --- a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp +++ b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp @@ -427,8 +427,13 @@ void step1(const std::atomic& loPause, std::string input_folder_name) { working_directory = fs::path(input_file_names[0]).parent_path().string(); calculate_trajectory(trajectory, imu_data, params, full_debug_messages); - compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); - step_1_done = true; + if (compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause)) + { + release_raw_clouds(pointsPerFile, params); // nothing after step 1 reads them + step_1_done = true; + } + else + std::cerr << "Lidar odometry step 1 failed (see the messages above)" << std::endl; } else { @@ -1525,6 +1530,12 @@ void openData() start = std::chrono::system_clock::now(); step2(loPause); + if (!step_2_done) + { + loRunning = false; + std::cerr << "Lidar odometry step 2 failed: no results saved" << std::endl; + return; + } end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; @@ -1549,7 +1560,7 @@ void openData() loRunning = false; } -void step1( +bool step1( const std::string& folder, LidarOdometryParams& params, std::vector>& pointsPerFile, @@ -1574,16 +1585,20 @@ void step1( { working_directory = fs::path(input_file_names[0]).parent_path().string(); calculate_trajectory(trajectory, imu_data, params, full_debug_messages); - compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); + if (!compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause)) + return false; + release_raw_clouds(pointsPerFile, params); // nothing after step 1 reads them std::cout << "step_1_done" << std::endl; + return true; } + return false; } -void step2(std::vector& worker_data, LidarOdometryParams& params, const std::atomic& loPause) +bool step2(std::vector& worker_data, LidarOdometryParams& params, const std::atomic& loPause) { double ts_failure = 0.0; std::atomic loProgress; - compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages); + return compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages); } void save_results( @@ -2321,6 +2336,12 @@ int main(int argc, char* argv[]) start = std::chrono::system_clock::now(); step2(loPause); + if (!step_2_done) + { + loRunning = false; + std::cerr << "Lidar odometry step 2 failed: no results saved" << std::endl; + return; + } end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 7a1f0653..cfa0770f 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -5,9 +5,99 @@ #include #include +#if defined(__GLIBC__) +#include +#endif + +#include namespace fs = std::filesystem; +// ---- raw clouds loaded per file (LidarOdometryParams::lazy_load_raw_clouds) ----------------------------------------- +// Eagerly, load_data holds every file's raw cloud before step 1 (~56 B per point: ~30 GB for 500 M points). Step 1 +// reads them chunk by chunk in time order, each chunk taking [t0, t1) from every file by binary search. Lazily, the +// first pass loads each file once (one per thread at a time) only to record its time range; step 1 then loads a file +// when its range overlaps the chunk and frees it once the chunk start has passed its last point. A file is loaded by +// the same call and sorted by the same comparator as the eager load, so every chunk receives the same points in the +// same order. The loader lives in LidarOdometryParams::raw_cloud_loader, so each pipeline owns its own. +struct RawCloudLoader +{ + std::function(size_t)> load; + std::vector files; + std::vector file_size; + std::vector file_time; + std::vector t_first, t_last; + std::vector n; + std::vector loaded, done; +}; + +static void release_freed_heap() +{ +#if defined(__GLIBC__) + malloc_trim(0); // a freed cloud of ~20 MB sits below glibc's grown mmap threshold and stays in the heap otherwise +#endif +} + +static void write_calibration_validation(const fs::path& wdp, const std::vector& data, const LidarOdometryParams& params) +{ + if (!fs::exists(wdp)) + { + std::cout << "Creating folder: " << wdp << "\n"; + fs::create_directory(wdp); + } + + fs::path outFile = wdp / "calibrationValidation.asc"; + std::ofstream testPointcloud(outFile); + if (!testPointcloud) + std::cerr << "Failed to open file: " << outFile << "\n"; + else + for (size_t j = 0; j < std::min(data.size(), (size_t)params.calibration_validation_points); ++j) + { + const auto& p = data[j]; + testPointcloud << p.point.x() << "\t" << p.point.y() << "\t" << p.point.z() << "\t" << p.intensity << "\t" << (int)p.lidarid + << "\n"; + } +} + +// Load file i if step 1 needs it and it is not in memory. Lazy loading reads every file twice (the range pass in +// load_data, then here), so a file that changed in between is refused rather than silently processed. +static bool lazy_ensure(RawCloudLoader* L, std::vector>& ppf, size_t i) +{ + if (!L || L->loaded[i] || L->done[i] || L->n[i] == 0) + return true; + const auto unchanged = [&]() + { + std::error_code ec; + const auto size = fs::file_size(L->files[i], ec); + const auto time = fs::last_write_time(L->files[i], ec); + return !ec && size == L->file_size[i] && time == L->file_time[i]; + }; + if (!unchanged()) + { + std::cerr << "lazy_load_raw_clouds: '" << L->files[i] << "' changed or vanished since it was first read\n"; + return false; + } + ppf[i] = L->load(i); + // checked again after the read, so a file replaced while it was being opened is refused too + if (!unchanged() || ppf[i].size() != L->n[i] || ppf[i].front().timestamp != L->t_first[i] || ppf[i].back().timestamp != L->t_last[i]) + { + std::cerr << "lazy_load_raw_clouds: '" << L->files[i] << "' reads differently than when it was first read\n"; + std::vector().swap(ppf[i]); + return false; + } + L->loaded[i] = 1; + return true; +} + +static void lazy_release(RawCloudLoader* L, std::vector>& ppf, size_t i) +{ + if (!L || !L->loaded[i]) + return; + std::vector().swap(ppf[i]); + L->loaded[i] = 0; + L->done[i] = 1; +} + bool load_data( std::vector& input_file_names, LidarOdometryParams& params, @@ -239,6 +329,60 @@ bool load_data( fs::path wdp_cache = fs::path(working_directory) / "cache"; params.working_directory_cache = wdp_cache.string(); + params.raw_cloud_loader.reset(); + if (params.lazy_load_raw_clouds) + { + params.raw_cloud_loader = std::make_shared(); + auto& L = *params.raw_cloud_loader; + // a reload (the GUI opening another folder) must not keep the previous session's clouds + std::vector>(minSize).swap(pointsPerFile); + const double inner = params.filter_threshold_xy_inner, outer = params.filter_threshold_xy_outer; + L.files.assign(laz_files.begin(), laz_files.begin() + minSize); + L.file_size.assign(minSize, 0); + L.file_time.assign(minSize, fs::file_time_type{}); + const std::vector files = L.files; + L.load = [files, inner, outer, combinedCalibration](size_t i) + { + auto data = load_point_cloud(files[i].c_str(), true, inner, outer, combinedCalibration); + std::sort( + data.begin(), + data.end(), + [](const Point3Di& a, const Point3Di& b) + { + return a.timestamp < b.timestamp; + }); + return data; + }; + L.t_first.assign(minSize, 0.0); + L.t_last.assign(minSize, 0.0); + L.n.assign(minSize, 0); + L.loaded.assign(minSize, 0); + L.done.assign(minSize, 0); + tbb::parallel_for( + size_t(1), // upstream clears the first file's cloud after loading: it is never used + minSize, + [&](size_t i) + { + std::error_code ec; + L.file_size[i] = fs::file_size(L.files[i], ec); + L.file_time[i] = fs::last_write_time(L.files[i], ec); + auto data = L.load(i); + L.n[i] = data.size(); + if (!data.empty()) + { + L.t_first[i] = data.front().timestamp; + L.t_last[i] = data.back().timestamp; + } + }); + if (params.save_calibration_validation && minSize > 0) // as the eager load: the first file, then dropped + write_calibration_validation(wdp, L.load(0), params); + size_t totalPoints = 0; + for (auto k : L.n) + totalPoints += k; + std::cout << "TOTAL: " << totalPoints << " (lazy: ranges recorded, clouds loaded on demand)\n..loading finished.\n\n"; + return true; + } + // --- Parallel load of LAZ files tbb::parallel_for( size_t(0), @@ -260,25 +404,7 @@ bool load_data( // Optional calibration validation (first file only) if ((i == 0) && params.save_calibration_validation) - { - if (!fs::exists(wdp)) - { - std::cout << "Creating folder: " << wdp << "\n"; - fs::create_directory(wdp); - } - - fs::path outFile = wdp / "calibrationValidation.asc"; - std::ofstream testPointcloud(outFile); - if (!testPointcloud) - std::cerr << "Failed to open file: " << outFile << "\n"; - else - for (size_t j = 0; j < std::min(data.size(), (size_t)params.calibration_validation_points); ++j) - { - const auto& p = data[j]; - testPointcloud << p.point.x() << "\t" << p.point.y() << "\t" << p.point.z() << "\t" << p.intensity << "\t" - << (int)p.lidarid << "\n"; - } - } + write_calibration_validation(wdp, data, params); // Store results pointsPerFile[i] = std::move(data); @@ -469,7 +595,7 @@ void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, LidarOdometryPa } bool compute_step_1( - const std::vector>& pointsPerFile, + std::vector>& pointsPerFile, LidarOdometryParams& params, Trajectory& trajectory, std::vector& worker_data, @@ -482,8 +608,27 @@ bool compute_step_1( // Step 1: Pre-reserve to avoid vector reallocations params.initial_points.reserve(params.threshold_initial_points); - for (const auto& pp : pointsPerFile) + auto& ppf = pointsPerFile; + RawCloudLoader* loader = params.raw_cloud_loader.get(); + if (loader) { + if (ppf.size() != loader->n.size()) + { + std::cerr << "lazy_load_raw_clouds: the cloud vector does not belong to the last load_data\n"; + return false; + } + // the flags describe THIS vector: a re-run reads every file again, and a file counts as loaded only if it is + for (size_t i = 0; i < ppf.size(); i++) + { + loader->loaded[i] = !ppf[i].empty(); + loader->done[i] = 0; + } + } + for (size_t fi = 0; fi < pointsPerFile.size(); fi++) + { + if (!lazy_ensure(loader, ppf, fi)) + return false; + const auto& pp = pointsPerFile[fi]; // number_of_points += pp.size(); for (const auto& p : pp) { @@ -573,6 +718,25 @@ bool compute_step_1( std::vector points; + if (loader) + { + auto& L = *loader; + const double t0 = wd.intermediate_trajectory_timestamps[0].first; + const double t1 = wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1].first; + for (size_t index = 0; index < pointsPerFile.size(); index++) + { + if (L.n[index] == 0) + continue; + if (L.t_last[index] < t0) // every later chunk starts later still: never read again + lazy_release(loader, ppf, index); + else if (L.t_first[index] < t1) // may hold points in [t0, t1): load; the search below decides + { + if (!lazy_ensure(loader, ppf, index)) + return false; + } + } + } + for (size_t index = 0; index < pointsPerFile.size(); index++) { auto lower = std::lower_bound( @@ -1077,6 +1241,13 @@ std::string save_results_automatic( return outwd.string(); } +void release_raw_clouds(std::vector>& pointsPerFile, LidarOdometryParams& params) +{ + std::vector>().swap(pointsPerFile); + params.raw_cloud_loader.reset(); + release_freed_heap(); +} + std::vector run_lidar_odometry(const std::string& input_dir, LidarOdometryParams& params) { HDMAP_ZONE_SCOPE("run_lidar_odometry"); @@ -1107,6 +1278,10 @@ std::vector run_lidar_odometry(const std::string& input_dir, LidarOd std::cout << "Calculation failed at step 1 of lidar odometry, exiting." << std::endl; return worker_data; } + // Step 1 has cached every chunk's points to disk (worker_data[*].*_cache_file_name) and nothing after it reads + // the raw clouds: release them instead of holding them (~56 B x every raw point; 27 GB on a 480 M-point capture) + // through step 2. Output-identical: no later reader exists. + release_raw_clouds(pointsPerFile, params); double ts_failure = 0.0; std::atomic loProgress; diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h index 8a64aa11..c5883812 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.h +++ b/apps/lidar_odometry_step_1/lidar_odometry.h @@ -22,12 +22,15 @@ bool load_data( Imu& imu_data, bool debugMsg); void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, LidarOdometryParams& params, bool debugMsg); +// pointsPerFile is read, and with lazy_load_raw_clouds loaded and freed file by file bool compute_step_1( - const std::vector>& pointsPerFile, + std::vector>& pointsPerFile, LidarOdometryParams& params, Trajectory& trajectory, std::vector& worker_data, const std::atomic& pause); +// after a successful step 1: free the raw clouds (and the lazy loader) — nothing after step 1 reads them +void release_raw_clouds(std::vector>& pointsPerFile, LidarOdometryParams& params); void run_consistency(std::vector& worker_data, const LidarOdometryParams& params); void filter_reference_buckets(LidarOdometryParams& params); void load_reference_point_clouds(const std::vector& input_file_names, LidarOdometryParams& params); diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 73a55901..5fea0cf5 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -640,8 +640,13 @@ void step1(const std::atomic& loPause) { working_directory = fs::path(input_file_names[0]).parent_path().string(); calculate_trajectory(trajectory, imu_data, params, full_debug_messages); - compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); - step_1_done = true; + if (compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause)) + { + release_raw_clouds(pointsPerFile, params); // nothing after step 1 reads them + step_1_done = true; + } + else + std::cerr << "Lidar odometry step 1 failed (see the messages above)" << std::endl; } else { @@ -1786,6 +1791,12 @@ void openData() start = std::chrono::system_clock::now(); step2(loPause); + if (!step_2_done) + { + loRunning = false; + std::cerr << "Lidar odometry step 2 failed: no results saved" << std::endl; + return; + } end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; @@ -1810,7 +1821,7 @@ void openData() loRunning = false; } -void step1( +bool step1( const std::string& folder, LidarOdometryParams& params, std::vector>& pointsPerFile, @@ -1835,16 +1846,20 @@ void step1( { working_directory = fs::path(input_file_names[0]).parent_path().string(); calculate_trajectory(trajectory, imu_data, params, full_debug_messages); - compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); + if (!compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause)) + return false; + release_raw_clouds(pointsPerFile, params); // nothing after step 1 reads them std::cout << "step_1_done" << std::endl; + return true; } + return false; } -void step2(std::vector& worker_data, LidarOdometryParams& params, const std::atomic& loPause) +bool step2(std::vector& worker_data, LidarOdometryParams& params, const std::atomic& loPause) { double ts_failure = 0.0; std::atomic loProgress; - compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages); + return compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages); } void save_results( @@ -2721,9 +2736,12 @@ int main(int argc, char* argv[]) start = std::chrono::system_clock::now(); std::atomic loPause{ false }; - step1(path.string(), params, pointsPerFile, imu_data, working_directory, trajectory, worker_data, loPause); - - step2(worker_data, params, loPause); + if (!step1(path.string(), params, pointsPerFile, imu_data, working_directory, trajectory, worker_data, loPause) || + !step2(worker_data, params, loPause)) + { + std::cerr << "Lidar odometry failed: no results saved" << std::endl; + return 1; + } end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; @@ -2757,9 +2775,12 @@ int main(int argc, char* argv[]) start = std::chrono::system_clock::now(); std::atomic loPause{ false }; - step1(argv[1], params, pointsPerFile, imu_data, working_directory, trajectory, worker_data, loPause); - - step2(worker_data, params, loPause); + if (!step1(argv[1], params, pointsPerFile, imu_data, working_directory, trajectory, worker_data, loPause) || + !step2(worker_data, params, loPause)) + { + std::cerr << "Lidar odometry failed: no results saved" << std::endl; + return 1; + } end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 95c373e0..2a376f95 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -70,6 +70,19 @@ void update_rgd( const std::vector& points_global, const Eigen::Vector3d& viewport, size_t* lookup_count) +{ + update_rgd_ray(rgd_params, buckets, points_global, viewport, lookup_count, points_global.size() < 100000); +} + +// update_rgd with the ray-cast decision made by the caller: a buffer streamed in chunks takes the decision its +// WHOLE size would have taken, so the buckets are the ones the whole buffer at once would have produced. +void update_rgd_ray( + const NDT::GridParameters& rgd_params, + NDTBucketMapType& buckets, + const std::vector& points_global, + const Eigen::Vector3d& viewport, + size_t* lookup_count, + bool ray_cast) { Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); @@ -187,7 +200,7 @@ void update_rgd( // Eigen::Vector3d direction = points_global[i].point - viewport; // direction.normalize(); // double bucket_norm = b.norm(); - if (points_global.size() < 100000) + if (ray_cast) { Eigen::Vector3d direction = viewport - points_global[i].point; double distance = direction.norm(); @@ -232,15 +245,36 @@ void update_rgd_hierarchy( const NDT::GridParameters& rgd_params_outdoor, NDTBucketMapType& buckets_outdoor, LookupStats& stats) +{ + update_rgd_hierarchy_ray( + rgd_params_indoor, + buckets_indoor, + points_global, + viewport, + rgd_params_outdoor, + buckets_outdoor, + stats, + points_global.size() < 100000); +} + +void update_rgd_hierarchy_ray( + const NDT::GridParameters& rgd_params_indoor, + NDTBucketMapType& buckets_indoor, + const std::vector& points_global, + const Eigen::Vector3d& viewport, + const NDT::GridParameters& rgd_params_outdoor, + NDTBucketMapType& buckets_outdoor, + LookupStats& stats, + bool ray_cast) { tbb::parallel_invoke( [&]() { - update_rgd(rgd_params_indoor, buckets_indoor, points_global, viewport, &stats.indoor_lookups); + update_rgd_ray(rgd_params_indoor, buckets_indoor, points_global, viewport, &stats.indoor_lookups, ray_cast); }, [&]() { - update_rgd(rgd_params_outdoor, buckets_outdoor, points_global, viewport, &stats.outdoor_lookups); + update_rgd_ray(rgd_params_outdoor, buckets_outdoor, points_global, viewport, &stats.outdoor_lookups, ray_cast); }); } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 47402fc2..2361b600 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -45,6 +46,9 @@ inline std::string get_software_version() return HDMAPPING_VERSION_STRING; } +struct RawCloudLoader; // lidar_odometry.cpp: raw point clouds loaded per file during step 1 +class PointsGlobalSpill; // lidar_odometry_utils_optimizers.cpp: step 2's map buffer kept in a file + struct LidarOdometryParams { // version information - automatically generated from CMake build system @@ -56,6 +60,18 @@ struct LidarOdometryParams bool useMultithread = true; double real_time_threshold_seconds = 10.0; // for realtime use: threshold_nr_poses * 0.005, where 0.005 is related with IMU frequency + // memory (results are unchanged by either option; see doc/virtual_memory.md) + // load each raw point-cloud file when step 1 reaches its time range and free it once step 1 has passed it, + // instead of holding every file in memory before step 1 starts + bool lazy_load_raw_clouds = true; + // non-empty: keep step 2's map buffer (every frame's points between sliding-window resets) in a temporary file + // in this directory instead of in memory; the file is removed when step 2 ends + std::string points_global_spill_directory = ""; + + // runtime state of the two options above, not parameters (not saved to the parameter file) + std::shared_ptr raw_cloud_loader; + PointsGlobalSpill* points_global_spill = nullptr; + // filter points double filter_threshold_xy_inner = 0.3; // filtering points during load double filter_threshold_xy_outer = 70.0; // filtering points during load @@ -270,6 +286,24 @@ void update_rgd_hierarchy( NDTBucketMapType& buckets_outdoor, LookupStats& stats); +// the same two with the ray-cast decision (upstream: buffer size < 100000) made by the caller +void update_rgd_ray( + const NDT::GridParameters& rgd_params, + NDTBucketMapType& buckets, + const std::vector& points_global, + const Eigen::Vector3d& viewport, + size_t* lookup_count, + bool ray_cast); +void update_rgd_hierarchy_ray( + const NDT::GridParameters& rgd_params_indoor, + NDTBucketMapType& buckets_indoor, + const std::vector& points_global, + const Eigen::Vector3d& viewport, + const NDT::GridParameters& rgd_params_outdoor, + NDTBucketMapType& buckets_outdoor, + LookupStats& stats, + bool ray_cast); + void update_rgd_spherical_coordinates( const NDT::GridParameters& rgd_params, NDTBucketMapType& buckets, diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp index e86efcab..eae5e088 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -10,6 +10,144 @@ #include #include +#include +#include +#include +#include +#include +#include +#include +// ---- points_global spill (LidarOdometryParams::points_global_spill_directory) ------------------------------------- +// Between sliding-window resets the map buffer `points_global` only grows (every frame's points) and is read only AT a +// reset: its second half becomes the new buffer and is folded into the buckets point by point, in order. Kept in a +// file instead of RAM, and streamed back in the same order with the ray-cast decision of its whole size, the buckets +// and so the trajectory are the ones the in-memory buffer gives; the RAM is one chunk. Empty directory = in memory. +// +// One spill file per compute_step_2 call, owned by a PointsGlobalSpill on that call's stack and reached through +// LidarOdometryParams::points_global_spill: created on entry, its final close checked on success, removed on every exit. +// A spill I/O failure ends the computation (compute_step_2 returns false); the PointsGlobalSpill that owns the files +// removes them while the exception unwinds. +[[noreturn]] static void spill_fail(const std::string& what) +{ + throw std::runtime_error("points_global spill: " + what); +} + +static void seek64(std::FILE* f, std::uint64_t offset) +{ +#if defined(_WIN32) + const bool ok = _fseeki64(f, static_cast<__int64>(offset), SEEK_SET) == 0; +#else + // off_t is 32-bit on a 32-bit POSIX build without large-file support: refuse an offset it cannot hold + if (offset > static_cast(std::numeric_limits::max())) + spill_fail("offset beyond this platform's off_t (build with _FILE_OFFSET_BITS=64)"); + const bool ok = fseeko(f, static_cast(offset), SEEK_SET) == 0; +#endif + if (!ok) + spill_fail("seek failed"); +} + +class PointsGlobalSpill +{ +public: + explicit PointsGlobalSpill(LidarOdometryParams& params) + : params_(params) + { + if (params.points_global_spill_directory.empty()) + return; + dir_ = params.points_global_spill_directory; + std::random_device rd; + const auto now = std::chrono::steady_clock::now().time_since_epoch().count(); + tag_ = std::to_string((static_cast(rd()) << 32 ^ rd()) ^ static_cast(now)); + f_ = std::fopen(path().c_str(), "wb"); + if (!f_) + spill_fail("cannot create '" + path() + "' (points_global_spill_directory)"); + spdlog::info("points_global spilled to '{}'", path()); + params_.points_global_spill = this; + } + ~PointsGlobalSpill() + { + if (params_.points_global_spill == this) + params_.points_global_spill = nullptr; + if (in_) + std::fclose(in_); + if (f_) + std::fclose(f_); + if (!pending_remove_.empty()) // a reset that did not finish: the previous generation's file too + std::remove(pending_remove_.c_str()); + if (!dir_.empty()) + std::remove(path().c_str()); + } + PointsGlobalSpill(const PointsGlobalSpill&) = delete; + PointsGlobalSpill& operator=(const PointsGlobalSpill&) = delete; + + void append(const Point3Di& p) + { + if (std::fwrite(&p, sizeof(Point3Di), 1, f_) != 1) + spill_fail("write failed on '" + path() + "'"); + n_++; + } + + // The reset, spilled: records [n/2, n) become the new buffer and are folded into the buckets in order. + template + void reset(Update update) + { + std::FILE* g = f_; + f_ = nullptr; // closed below whatever fclose returns: the destructor must not close it again + if (std::fclose(g) != 0) + spill_fail("close failed on '" + path() + "'"); + const std::string old_path = path(); + pending_remove_ = old_path; + in_ = std::fopen(old_path.c_str(), "rb"); + const std::uint64_t first = n_ / 2, keep = n_ - n_ / 2; + gen_++; + f_ = std::fopen(path().c_str(), "wb"); + if (!in_ || !f_) + spill_fail("reset could not open '" + old_path + "' / '" + path() + "'"); + seek64(in_, first * sizeof(Point3Di)); + const bool ray_cast = keep < 100000; + std::vector chunk; + const std::uint64_t CH = std::uint64_t(1) << 21; + for (std::uint64_t done = 0; done < keep;) + { + const size_t m = static_cast(std::min(CH, keep - done)); + chunk.resize(m); + if (std::fread(chunk.data(), sizeof(Point3Di), m, in_) != m || std::fwrite(chunk.data(), sizeof(Point3Di), m, f_) != m) + spill_fail("reset read/write failed on '" + old_path + "'"); + update(chunk, ray_cast); + done += m; + } + std::fclose(in_); + in_ = nullptr; + if (std::remove(old_path.c_str()) != 0) + spdlog::warn("points_global spill: could not remove '{}'", old_path); + pending_remove_.clear(); + n_ = keep; + } + + // a successful run: the final close is checked, so a failed flush is not hidden behind a reported success + void finish() + { + if (f_ && std::fclose(f_) != 0) + { + f_ = nullptr; + spill_fail("close failed on '" + path() + "'"); + } + f_ = nullptr; + } + +private: + std::string path() const + { + return dir_ + "/points_global_" + tag_ + "_" + std::to_string(gen_) + ".bin"; + } + LidarOdometryParams& params_; + std::string dir_, tag_, pending_remove_; + std::FILE* f_ = nullptr; + std::FILE* in_ = nullptr; + std::uint64_t n_ = 0; + int gen_ = 0; +}; + const double DEG_TO_RAD = M_PI / 180.0f; const double RAD_TO_DEG = 180.0f / M_PI; @@ -2067,6 +2205,7 @@ bool process_worker_step_lidar_odometry_core( constexpr double outdoor_range_squared = 5.0 * 5.0; // 25.0 const double max_distance_squared = params.max_distance_lidar * params.max_distance_lidar; + std::vector touched_bins_indoor, touched_bins_outdoor; const auto build_normal_vector_histograms = [&]() { auto process_chunk = [&](size_t chunk) @@ -2138,14 +2277,27 @@ bool process_worker_step_lidar_odometry_core( process_chunk(c); } - std::fill(table_buckets_nv_indoor.begin(), table_buckets_nv_indoor.end(), 0.0); - std::fill(table_buckets_nv_outdoor.begin(), table_buckets_nv_outdoor.end(), 0.0); + // Zero only the bins the previous build wrote (the tables are this frame's, zero-initialised) instead of all + // 2 x 101^3 doubles every iteration: the same table contents (integer counts summed exactly), ~16 MB of + // memory writes per iteration fewer. + for (const int b : touched_bins_indoor) + table_buckets_nv_indoor[b] = 0.0; + for (const int b : touched_bins_outdoor) + table_buckets_nv_outdoor[b] = 0.0; + touched_bins_indoor.clear(); + touched_bins_outdoor.clear(); for (size_t c = 0; c < num_hist_chunks; ++c) { for (const auto& [bin, count] : chunk_hist_indoor[c]) + { table_buckets_nv_indoor[bin] += static_cast(count); + touched_bins_indoor.push_back(bin); + } for (const auto& [bin, count] : chunk_hist_outdoor[c]) + { table_buckets_nv_outdoor[bin] += static_cast(count); + touched_bins_outdoor.push_back(bin); + } } }; @@ -2239,6 +2391,31 @@ bool process_worker_step_update_rgd_after( params.buckets_outdoor.clear(); } + if (PointsGlobalSpill* spill = params.points_global_spill) + { + acc_distance = 0; + std::scoped_lock lock(params.mutex_buckets_indoor, params.mutex_buckets_outdoor); + // the upstream decimate() below discards its result: the buffer is never decimated, so neither is this + const Eigen::Vector3d viewport = worker_data.intermediate_trajectory[0].translation(); + spill->reset( + [&](const std::vector& chunk, bool ray_cast) + { + if (params.ablation_study_use_hierarchical_rgd) + update_rgd_hierarchy_ray( + params.in_out_params_indoor, + params.buckets_indoor, + chunk, + viewport, + params.in_out_params_outdoor, + params.buckets_outdoor, + lookup_stats, + ray_cast); + else + update_rgd_ray( + params.in_out_params_indoor, params.buckets_indoor, chunk, viewport, &lookup_stats.indoor_lookups, ray_cast); + }); + return true; + } std::vector points_global_new; points_global_new.reserve(points_global.size() / 2 + 1); for (int k = points_global.size() / 2; k < points_global.size(); k++) @@ -2346,6 +2523,14 @@ bool process_worker_step_update_rgd_after( return true; } +static bool compute_step_2_impl( + std::vector& worker_data, + LidarOdometryParams& params, + double& ts_failure, + std::atomic& loProgress, + const std::atomic& pause, + bool debugMsg); + bool compute_step_2( std::vector& worker_data, LidarOdometryParams& params, @@ -2353,6 +2538,24 @@ bool compute_step_2( std::atomic& loProgress, const std::atomic& pause, bool debugMsg) +{ + try + { + return compute_step_2_impl(worker_data, params, ts_failure, loProgress, pause, debugMsg); + } catch (const std::runtime_error& e) // a points_global spill I/O failure; its files are already removed + { + spdlog::error("{}", e.what()); + return false; + } +} + +static bool compute_step_2_impl( + std::vector& worker_data, + LidarOdometryParams& params, + double& ts_failure, + std::atomic& loProgress, + const std::atomic& pause, + bool debugMsg) { // exit(1); bool debug = false; @@ -2366,6 +2569,7 @@ bool compute_step_2( double total_optimization_time_seconds = 0.0; LookupStats lookup_stats; std::vector points_global; + PointsGlobalSpill spill(params); // a spill directory set: points_global lives in this call's file, removed at the end spdlog::stopwatch stopwatch_worker; if (initialize_lidar_odometry(worker_data, params, ts_failure, loProgress, pause, debugMsg, lookup_stats)) @@ -2565,7 +2769,10 @@ bool compute_step_2( { Point3Di pp = intermediate_points[j]; pp.point = worker_data[i].intermediate_trajectory[intermediate_points[j].index_pose] * pp.point; - points_global.push_back(pp); + if (PointsGlobalSpill* sp = params.points_global_spill) + sp->append(pp); + else + points_global.push_back(pp); } HDMAP_ZONE_END(transform_pts); @@ -2603,6 +2810,7 @@ bool compute_step_2( const double avg_iteration_ms = (total_iterations > 0) ? (total_optimization_time_seconds * 1000.0 / total_iterations) : 0.0; spdlog::info("avg_iteration_time: {:.3f}ms", avg_iteration_ms); spdlog::debug("lookup_stats: indoor={} outdoor_lookups={}", lookup_stats.indoor_lookups, lookup_stats.outdoor_lookups); + spill.finish(); return true; } else diff --git a/apps/lidar_odometry_step_1/toml_io.h b/apps/lidar_odometry_step_1/toml_io.h index b4001e49..5d677582 100644 --- a/apps/lidar_odometry_step_1/toml_io.h +++ b/apps/lidar_odometry_step_1/toml_io.h @@ -29,6 +29,8 @@ class TomlIO { "useMultithread", &LidarOdometryParams::useMultithread }, { "real_time_threshold_seconds", &LidarOdometryParams::real_time_threshold_seconds }, + { "lazy_load_raw_clouds", &LidarOdometryParams::lazy_load_raw_clouds }, + { "points_global_spill_directory", &LidarOdometryParams::points_global_spill_directory }, { "filter_threshold_xy_inner", &LidarOdometryParams::filter_threshold_xy_inner }, { "filter_threshold_xy_outer", &LidarOdometryParams::filter_threshold_xy_outer }, { "decimation", &LidarOdometryParams::decimation }, @@ -123,7 +125,7 @@ class TomlIO std::map> CATEGORIES = { { "version_info", { "software_version", "config_version", "build_date" } }, - { "performance", { "useMultithread", "real_time_threshold_seconds" } }, + { "performance", { "useMultithread", "real_time_threshold_seconds", "lazy_load_raw_clouds", "points_global_spill_directory" } }, { "filter_points", { "filter_threshold_xy_inner", "filter_threshold_xy_outer", diff --git a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp index aee200bd..32eafac8 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -1018,6 +1018,7 @@ void loadFiles(std::vector input_file_names) { LidarOdometryParams params; // dummy for load_data function params.save_calibration_validation = false; + params.lazy_load_raw_clouds = false; // the viewer shows every cloud params.filter_threshold_xy_inner = filter_threshold_xy_inner; params.filter_threshold_xy_outer = filter_threshold_xy_outer; diff --git a/doc/virtual_memory.md b/doc/virtual_memory.md index 82cc0c69..5f580ebd 100644 --- a/doc/virtual_memory.md +++ b/doc/virtual_memory.md @@ -1,5 +1,36 @@ # Consideration -Implementation loads all data in memory, there is no out-of-core solution utilized in the project. +Most of the implementation keeps its data in memory. Lidar odometry (step 1) has two parameters that bound its memory +use on long recordings (below); for the rest, virtual memory on a fast SSD lets you work with large projects. + +# Lidar odometry on long recordings +Two parameters in the `[performance]` section of the lidar odometry parameter file (`LidarOdometryParams`) keep the +memory of `lidar_odometry_step_1` bounded. Neither changes the results: the trajectory, scans and poses are the same +with or without them. + +| Parameter | Default | What it does | +|---|---|---| +| `lazy_load_raw_clouds` | `true` | Each raw point-cloud file is loaded when step 1 reaches its time range and freed once step 1 has passed it, instead of loading every file before step 1 starts. | +| `points_global_spill_directory` | `""` (in memory) | When set, the map buffer of step 2 (every frame's points between two sliding-window resets) is kept in a temporary file in this directory instead of in memory. The file is removed when step 2 ends. | + +For a long recording from a slow-moving platform (many frames inside one sliding window), set +`points_global_spill_directory` to a local SSD with room for the buffer (tens of GB on a recording of about an hour): + +```toml +[performance] +lazy_load_raw_clouds = true +points_global_spill_directory = "/path/to/fast/scratch" +``` + +Notes: +- With `lazy_load_raw_clouds`, every raw file is read twice: once when the data is loaded (to record its time range) + and again during step 1. The input files must not change during the run; if one does, step 1 stops with an error. +- The spill directory must exist. During a sliding-window reset the old buffer and the retained half exist side by + side, so allow about 1.5 times the buffer size. A write error ends step 2 with an error, and the spill files are + removed. +- Results are identical to running without these parameters, as long as the per-frame time limit + (`real_time_threshold_seconds`) is not reached: when it is, the number of iterations depends on machine speed, with + or without them. + With modern operating systems, you can work with large projects, utilizing virtual memory with large and fast SSD. The setup of optimal paging is system-dependent. diff --git a/pybind/lidar_odometry_binding.cpp b/pybind/lidar_odometry_binding.cpp index 87c4aa02..de880427 100644 --- a/pybind/lidar_odometry_binding.cpp +++ b/pybind/lidar_odometry_binding.cpp @@ -66,6 +66,8 @@ PYBIND11_MODULE(lidar_odometry_py, m) .def_readwrite("consecutive_distance", &LidarOdometryParams::consecutive_distance) .def_readwrite("nr_iter", &LidarOdometryParams::nr_iter) .def_readwrite("useMultithread", &LidarOdometryParams::useMultithread) + .def_readwrite("lazy_load_raw_clouds", &LidarOdometryParams::lazy_load_raw_clouds) + .def_readwrite("points_global_spill_directory", &LidarOdometryParams::points_global_spill_directory) .def_readwrite("reference_points", &LidarOdometryParams::reference_points) .def_readwrite("decimation", &LidarOdometryParams::decimation) .def_readwrite("working_directory_preview", &LidarOdometryParams::working_directory_preview) From 134ff9bf9ed338ff3c89f1d61b099c0b8569da69 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 6 Oct 2026 18:23:31 +0200 Subject: [PATCH 2/2] Address PR #555 review: extract PointsGlobalSpill and RawCloudLoader Move both out of lidar_odometry_utils_optimizers.cpp/lidar_odometry.cpp into their own header/source pairs, and build PointsGlobalSpill's spill file path with std::filesystem instead of string concatenation to avoid Windows path-separator issues. Logic is unchanged. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01G9Cmw8PaVx6XGFJhAh7G1f --- apps/lidar_odometry_step_1/CMakeLists.txt | 2 + apps/lidar_odometry_step_1/lidar_odometry.cpp | 58 +------- .../lidar_odometry_utils.h | 4 +- .../lidar_odometry_utils_optimizers.cpp | 139 +----------------- .../points_global_spill.cpp | 78 ++++++++++ .../points_global_spill.h | 89 +++++++++++ .../raw_cloud_loader.cpp | 40 +++++ apps/lidar_odometry_step_1/raw_cloud_loader.h | 32 ++++ .../CMakeLists.txt | 1 + .../CMakeLists.txt | 1 + apps/mandeye_raw_data_viewer/CMakeLists.txt | 2 + pybind/CMakeLists.txt | 2 + 12 files changed, 251 insertions(+), 197 deletions(-) create mode 100644 apps/lidar_odometry_step_1/points_global_spill.cpp create mode 100644 apps/lidar_odometry_step_1/points_global_spill.h create mode 100644 apps/lidar_odometry_step_1/raw_cloud_loader.cpp create mode 100644 apps/lidar_odometry_step_1/raw_cloud_loader.h diff --git a/apps/lidar_odometry_step_1/CMakeLists.txt b/apps/lidar_odometry_step_1/CMakeLists.txt index 3c5ef37a..60a1a956 100644 --- a/apps/lidar_odometry_step_1/CMakeLists.txt +++ b/apps/lidar_odometry_step_1/CMakeLists.txt @@ -21,6 +21,8 @@ set(SOURCES lidar_odometry_utils.h lidar_odometry_utils.cpp lidar_odometry_utils_optimizers.cpp lidar_odometry.cpp lidar_odometry.h + points_global_spill.h points_global_spill.cpp + raw_cloud_loader.h raw_cloud_loader.cpp toml_io.h toml_io.cpp "../../core/src/utils.cpp" ) diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index cfa0770f..1659bf02 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -1,5 +1,6 @@ #include "lidar_odometry.h" #include "hdmapping_profiler.hpp" +#include "raw_cloud_loader.h" #include "tbb/tbb.h" #include @@ -13,24 +14,6 @@ namespace fs = std::filesystem; -// ---- raw clouds loaded per file (LidarOdometryParams::lazy_load_raw_clouds) ----------------------------------------- -// Eagerly, load_data holds every file's raw cloud before step 1 (~56 B per point: ~30 GB for 500 M points). Step 1 -// reads them chunk by chunk in time order, each chunk taking [t0, t1) from every file by binary search. Lazily, the -// first pass loads each file once (one per thread at a time) only to record its time range; step 1 then loads a file -// when its range overlaps the chunk and frees it once the chunk start has passed its last point. A file is loaded by -// the same call and sorted by the same comparator as the eager load, so every chunk receives the same points in the -// same order. The loader lives in LidarOdometryParams::raw_cloud_loader, so each pipeline owns its own. -struct RawCloudLoader -{ - std::function(size_t)> load; - std::vector files; - std::vector file_size; - std::vector file_time; - std::vector t_first, t_last; - std::vector n; - std::vector loaded, done; -}; - static void release_freed_heap() { #if defined(__GLIBC__) @@ -59,45 +42,6 @@ static void write_calibration_validation(const fs::path& wdp, const std::vector< } } -// Load file i if step 1 needs it and it is not in memory. Lazy loading reads every file twice (the range pass in -// load_data, then here), so a file that changed in between is refused rather than silently processed. -static bool lazy_ensure(RawCloudLoader* L, std::vector>& ppf, size_t i) -{ - if (!L || L->loaded[i] || L->done[i] || L->n[i] == 0) - return true; - const auto unchanged = [&]() - { - std::error_code ec; - const auto size = fs::file_size(L->files[i], ec); - const auto time = fs::last_write_time(L->files[i], ec); - return !ec && size == L->file_size[i] && time == L->file_time[i]; - }; - if (!unchanged()) - { - std::cerr << "lazy_load_raw_clouds: '" << L->files[i] << "' changed or vanished since it was first read\n"; - return false; - } - ppf[i] = L->load(i); - // checked again after the read, so a file replaced while it was being opened is refused too - if (!unchanged() || ppf[i].size() != L->n[i] || ppf[i].front().timestamp != L->t_first[i] || ppf[i].back().timestamp != L->t_last[i]) - { - std::cerr << "lazy_load_raw_clouds: '" << L->files[i] << "' reads differently than when it was first read\n"; - std::vector().swap(ppf[i]); - return false; - } - L->loaded[i] = 1; - return true; -} - -static void lazy_release(RawCloudLoader* L, std::vector>& ppf, size_t i) -{ - if (!L || !L->loaded[i]) - return; - std::vector().swap(ppf[i]); - L->loaded[i] = 0; - L->done[i] = 1; -} - bool load_data( std::vector& input_file_names, LidarOdometryParams& params, diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 2361b600..27baae5c 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -46,8 +46,8 @@ inline std::string get_software_version() return HDMAPPING_VERSION_STRING; } -struct RawCloudLoader; // lidar_odometry.cpp: raw point clouds loaded per file during step 1 -class PointsGlobalSpill; // lidar_odometry_utils_optimizers.cpp: step 2's map buffer kept in a file +struct RawCloudLoader; // raw_cloud_loader.h: raw point clouds loaded per file during step 1 +class PointsGlobalSpill; // points_global_spill.h: step 2's map buffer kept in a file struct LidarOdometryParams { diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp index eae5e088..7d4f5d98 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1,5 +1,6 @@ #include "hdmapping_profiler.hpp" #include "lidar_odometry_utils.h" +#include "points_global_spill.h" #include #include #include @@ -10,144 +11,6 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -// ---- points_global spill (LidarOdometryParams::points_global_spill_directory) ------------------------------------- -// Between sliding-window resets the map buffer `points_global` only grows (every frame's points) and is read only AT a -// reset: its second half becomes the new buffer and is folded into the buckets point by point, in order. Kept in a -// file instead of RAM, and streamed back in the same order with the ray-cast decision of its whole size, the buckets -// and so the trajectory are the ones the in-memory buffer gives; the RAM is one chunk. Empty directory = in memory. -// -// One spill file per compute_step_2 call, owned by a PointsGlobalSpill on that call's stack and reached through -// LidarOdometryParams::points_global_spill: created on entry, its final close checked on success, removed on every exit. -// A spill I/O failure ends the computation (compute_step_2 returns false); the PointsGlobalSpill that owns the files -// removes them while the exception unwinds. -[[noreturn]] static void spill_fail(const std::string& what) -{ - throw std::runtime_error("points_global spill: " + what); -} - -static void seek64(std::FILE* f, std::uint64_t offset) -{ -#if defined(_WIN32) - const bool ok = _fseeki64(f, static_cast<__int64>(offset), SEEK_SET) == 0; -#else - // off_t is 32-bit on a 32-bit POSIX build without large-file support: refuse an offset it cannot hold - if (offset > static_cast(std::numeric_limits::max())) - spill_fail("offset beyond this platform's off_t (build with _FILE_OFFSET_BITS=64)"); - const bool ok = fseeko(f, static_cast(offset), SEEK_SET) == 0; -#endif - if (!ok) - spill_fail("seek failed"); -} - -class PointsGlobalSpill -{ -public: - explicit PointsGlobalSpill(LidarOdometryParams& params) - : params_(params) - { - if (params.points_global_spill_directory.empty()) - return; - dir_ = params.points_global_spill_directory; - std::random_device rd; - const auto now = std::chrono::steady_clock::now().time_since_epoch().count(); - tag_ = std::to_string((static_cast(rd()) << 32 ^ rd()) ^ static_cast(now)); - f_ = std::fopen(path().c_str(), "wb"); - if (!f_) - spill_fail("cannot create '" + path() + "' (points_global_spill_directory)"); - spdlog::info("points_global spilled to '{}'", path()); - params_.points_global_spill = this; - } - ~PointsGlobalSpill() - { - if (params_.points_global_spill == this) - params_.points_global_spill = nullptr; - if (in_) - std::fclose(in_); - if (f_) - std::fclose(f_); - if (!pending_remove_.empty()) // a reset that did not finish: the previous generation's file too - std::remove(pending_remove_.c_str()); - if (!dir_.empty()) - std::remove(path().c_str()); - } - PointsGlobalSpill(const PointsGlobalSpill&) = delete; - PointsGlobalSpill& operator=(const PointsGlobalSpill&) = delete; - - void append(const Point3Di& p) - { - if (std::fwrite(&p, sizeof(Point3Di), 1, f_) != 1) - spill_fail("write failed on '" + path() + "'"); - n_++; - } - - // The reset, spilled: records [n/2, n) become the new buffer and are folded into the buckets in order. - template - void reset(Update update) - { - std::FILE* g = f_; - f_ = nullptr; // closed below whatever fclose returns: the destructor must not close it again - if (std::fclose(g) != 0) - spill_fail("close failed on '" + path() + "'"); - const std::string old_path = path(); - pending_remove_ = old_path; - in_ = std::fopen(old_path.c_str(), "rb"); - const std::uint64_t first = n_ / 2, keep = n_ - n_ / 2; - gen_++; - f_ = std::fopen(path().c_str(), "wb"); - if (!in_ || !f_) - spill_fail("reset could not open '" + old_path + "' / '" + path() + "'"); - seek64(in_, first * sizeof(Point3Di)); - const bool ray_cast = keep < 100000; - std::vector chunk; - const std::uint64_t CH = std::uint64_t(1) << 21; - for (std::uint64_t done = 0; done < keep;) - { - const size_t m = static_cast(std::min(CH, keep - done)); - chunk.resize(m); - if (std::fread(chunk.data(), sizeof(Point3Di), m, in_) != m || std::fwrite(chunk.data(), sizeof(Point3Di), m, f_) != m) - spill_fail("reset read/write failed on '" + old_path + "'"); - update(chunk, ray_cast); - done += m; - } - std::fclose(in_); - in_ = nullptr; - if (std::remove(old_path.c_str()) != 0) - spdlog::warn("points_global spill: could not remove '{}'", old_path); - pending_remove_.clear(); - n_ = keep; - } - - // a successful run: the final close is checked, so a failed flush is not hidden behind a reported success - void finish() - { - if (f_ && std::fclose(f_) != 0) - { - f_ = nullptr; - spill_fail("close failed on '" + path() + "'"); - } - f_ = nullptr; - } - -private: - std::string path() const - { - return dir_ + "/points_global_" + tag_ + "_" + std::to_string(gen_) + ".bin"; - } - LidarOdometryParams& params_; - std::string dir_, tag_, pending_remove_; - std::FILE* f_ = nullptr; - std::FILE* in_ = nullptr; - std::uint64_t n_ = 0; - int gen_ = 0; -}; - const double DEG_TO_RAD = M_PI / 180.0f; const double RAD_TO_DEG = 180.0f / M_PI; diff --git a/apps/lidar_odometry_step_1/points_global_spill.cpp b/apps/lidar_odometry_step_1/points_global_spill.cpp new file mode 100644 index 00000000..d38fd9ca --- /dev/null +++ b/apps/lidar_odometry_step_1/points_global_spill.cpp @@ -0,0 +1,78 @@ +#include "points_global_spill.h" + +#include +#include +#include +#include + +PointsGlobalSpill::PointsGlobalSpill(LidarOdometryParams& params) + : params_(params) +{ + if (params.points_global_spill_directory.empty()) + return; + dir_ = params.points_global_spill_directory; + std::random_device rd; + const auto now = std::chrono::steady_clock::now().time_since_epoch().count(); + tag_ = std::to_string((static_cast(rd()) << 32 ^ rd()) ^ static_cast(now)); + f_ = std::fopen(path().string().c_str(), "wb"); + if (!f_) + fail("cannot create '" + path().string() + "' (points_global_spill_directory)"); + spdlog::info("points_global spilled to '{}'", path().string()); + params_.points_global_spill = this; +} + +PointsGlobalSpill::~PointsGlobalSpill() +{ + if (params_.points_global_spill == this) + params_.points_global_spill = nullptr; + if (in_) + std::fclose(in_); + if (f_) + std::fclose(f_); + std::error_code ec; + if (!pending_remove_.empty()) // a reset that did not finish: the previous generation's file too + std::filesystem::remove(pending_remove_, ec); + if (!dir_.empty()) + std::filesystem::remove(path(), ec); +} + +void PointsGlobalSpill::append(const Point3Di& p) +{ + if (std::fwrite(&p, sizeof(Point3Di), 1, f_) != 1) + fail("write failed on '" + path().string() + "'"); + n_++; +} + +void PointsGlobalSpill::finish() +{ + if (f_ && std::fclose(f_) != 0) + { + f_ = nullptr; + fail("close failed on '" + path().string() + "'"); + } + f_ = nullptr; +} + +void PointsGlobalSpill::fail(const std::string& what) +{ + throw std::runtime_error("points_global spill: " + what); +} + +void PointsGlobalSpill::seek64(std::FILE* f, std::uint64_t offset) +{ +#if defined(_WIN32) + const bool ok = _fseeki64(f, static_cast<__int64>(offset), SEEK_SET) == 0; +#else + // off_t is 32-bit on a 32-bit POSIX build without large-file support: refuse an offset it cannot hold + if (offset > static_cast(std::numeric_limits::max())) + fail("offset beyond this platform's off_t (build with _FILE_OFFSET_BITS=64)"); + const bool ok = fseeko(f, static_cast(offset), SEEK_SET) == 0; +#endif + if (!ok) + fail("seek failed"); +} + +std::filesystem::path PointsGlobalSpill::path() const +{ + return dir_ / ("points_global_" + tag_ + "_" + std::to_string(gen_) + ".bin"); +} \ No newline at end of file diff --git a/apps/lidar_odometry_step_1/points_global_spill.h b/apps/lidar_odometry_step_1/points_global_spill.h new file mode 100644 index 00000000..0f48458e --- /dev/null +++ b/apps/lidar_odometry_step_1/points_global_spill.h @@ -0,0 +1,89 @@ +#pragma once + +#include "lidar_odometry_utils.h" + +#include + +#include +#include +#include +#include + +//! Between sliding-window resets the map buffer `points_global` only grows (every frame's points) and is read only AT a +//! reset: its second half becomes the new buffer and is folded into the buckets point by point, in order. Kept in a +//! file instead of RAM, and streamed back in the same order with the ray-cast decision of its whole size, the buckets +//! and so the trajectory are the ones the in-memory buffer gives; the RAM is one chunk. Empty directory = in memory. +//! +//! One spill file per compute_step_2 call, owned by a PointsGlobalSpill on that call's stack and reached through +//! LidarOdometryParams::points_global_spill: created on entry, its final close checked on success, removed on every exit. +//! A spill I/O failure ends the computation (compute_step_2 returns false); the PointsGlobalSpill that owns the files +//! removes them while the exception unwinds. +class PointsGlobalSpill +{ +public: + explicit PointsGlobalSpill(LidarOdometryParams& params); + ~PointsGlobalSpill(); + PointsGlobalSpill(const PointsGlobalSpill&) = delete; + PointsGlobalSpill& operator=(const PointsGlobalSpill&) = delete; + + void append(const Point3Di& p); + + // The reset, spilled: records [n/2, n) become the new buffer and are folded into the buckets in order. + template + void reset(Update update); + + // a successful run: the final close is checked, so a failed flush is not hidden behind a reported success + void finish(); + +private: + [[noreturn]] static void fail(const std::string& what); + static void seek64(std::FILE* f, std::uint64_t offset); + std::filesystem::path path() const; + + LidarOdometryParams& params_; + std::filesystem::path dir_; + std::filesystem::path pending_remove_; + std::string tag_; + std::FILE* f_ = nullptr; + std::FILE* in_ = nullptr; + std::uint64_t n_ = 0; + int gen_ = 0; +}; + +template +void PointsGlobalSpill::reset(Update update) +{ + std::FILE* g = f_; + f_ = nullptr; // closed below whatever fclose returns: the destructor must not close it again + if (std::fclose(g) != 0) + fail("close failed on '" + path().string() + "'"); + const std::filesystem::path old_path = path(); + pending_remove_ = old_path; + in_ = std::fopen(old_path.string().c_str(), "rb"); + const std::uint64_t first = n_ / 2, keep = n_ - n_ / 2; + gen_++; + f_ = std::fopen(path().string().c_str(), "wb"); + if (!in_ || !f_) + fail("reset could not open '" + old_path.string() + "' / '" + path().string() + "'"); + seek64(in_, first * sizeof(Point3Di)); + const bool ray_cast = keep < 100000; + std::vector chunk; + const std::uint64_t CH = std::uint64_t(1) << 21; + for (std::uint64_t done = 0; done < keep;) + { + const size_t m = static_cast(std::min(CH, keep - done)); + chunk.resize(m); + if (std::fread(chunk.data(), sizeof(Point3Di), m, in_) != m || std::fwrite(chunk.data(), sizeof(Point3Di), m, f_) != m) + fail("reset read/write failed on '" + old_path.string() + "'"); + update(chunk, ray_cast); + done += m; + } + std::fclose(in_); + in_ = nullptr; + std::error_code ec; + std::filesystem::remove(old_path, ec); + if (ec) + spdlog::warn("points_global spill: could not remove '{}': {}", old_path.string(), ec.message()); + pending_remove_.clear(); + n_ = keep; +} \ No newline at end of file diff --git a/apps/lidar_odometry_step_1/raw_cloud_loader.cpp b/apps/lidar_odometry_step_1/raw_cloud_loader.cpp new file mode 100644 index 00000000..9a1e6bcc --- /dev/null +++ b/apps/lidar_odometry_step_1/raw_cloud_loader.cpp @@ -0,0 +1,40 @@ +#include "raw_cloud_loader.h" + +#include + +bool lazy_ensure(RawCloudLoader* L, std::vector>& ppf, size_t i) +{ + if (!L || L->loaded[i] || L->done[i] || L->n[i] == 0) + return true; + const auto unchanged = [&]() + { + std::error_code ec; + const auto size = fs::file_size(L->files[i], ec); + const auto time = fs::last_write_time(L->files[i], ec); + return !ec && size == L->file_size[i] && time == L->file_time[i]; + }; + if (!unchanged()) + { + std::cerr << "lazy_load_raw_clouds: '" << L->files[i] << "' changed or vanished since it was first read\n"; + return false; + } + ppf[i] = L->load(i); + // checked again after the read, so a file replaced while it was being opened is refused too + if (!unchanged() || ppf[i].size() != L->n[i] || ppf[i].front().timestamp != L->t_first[i] || ppf[i].back().timestamp != L->t_last[i]) + { + std::cerr << "lazy_load_raw_clouds: '" << L->files[i] << "' reads differently than when it was first read\n"; + std::vector().swap(ppf[i]); + return false; + } + L->loaded[i] = 1; + return true; +} + +void lazy_release(RawCloudLoader* L, std::vector>& ppf, size_t i) +{ + if (!L || !L->loaded[i]) + return; + std::vector().swap(ppf[i]); + L->loaded[i] = 0; + L->done[i] = 1; +} \ No newline at end of file diff --git a/apps/lidar_odometry_step_1/raw_cloud_loader.h b/apps/lidar_odometry_step_1/raw_cloud_loader.h new file mode 100644 index 00000000..9bb039d4 --- /dev/null +++ b/apps/lidar_odometry_step_1/raw_cloud_loader.h @@ -0,0 +1,32 @@ +#pragma once + +#include "lidar_odometry_utils.h" + +#include +#include +#include + +//! Raw point clouds loaded per file during step 1 (LidarOdometryParams::lazy_load_raw_clouds). +//! +//! Eagerly, load_data holds every file's raw cloud before step 1 (~56 B per point: ~30 GB for 500 M points). Step 1 +//! reads them chunk by chunk in time order, each chunk taking [t0, t1) from every file by binary search. Lazily, the +//! first pass loads each file once (one per thread at a time) only to record its time range; step 1 then loads a file +//! when its range overlaps the chunk and frees it once the chunk start has passed its last point. A file is loaded by +//! the same call and sorted by the same comparator as the eager load, so every chunk receives the same points in the +//! same order. The loader lives in LidarOdometryParams::raw_cloud_loader, so each pipeline owns its own. +struct RawCloudLoader +{ + std::function(size_t)> load; + std::vector files; + std::vector file_size; + std::vector file_time; + std::vector t_first, t_last; + std::vector n; + std::vector loaded, done; +}; + +//! Load file i if step 1 needs it and it is not in memory. Lazy loading reads every file twice (the range pass in +//! load_data, then here), so a file that changed in between is refused rather than silently processed. +bool lazy_ensure(RawCloudLoader* L, std::vector>& ppf, size_t i); + +void lazy_release(RawCloudLoader* L, std::vector>& ppf, size_t i); \ No newline at end of file diff --git a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt index dfc89566..e2f719b4 100644 --- a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt +++ b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt @@ -7,6 +7,7 @@ add_executable( livox_mid_360_intrinsic_calibration.cpp ../lidar_odometry_step_1/lidar_odometry_utils.cpp ../lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp + ../lidar_odometry_step_1/points_global_spill.cpp ) target_include_directories( diff --git a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt index 04feb1b4..d308cd1e 100644 --- a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt +++ b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt @@ -7,6 +7,7 @@ set(SOURCES mandeye_mission_recorder_calibration.cpp ../lidar_odometry_step_1/lidar_odometry_utils.cpp ../lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp + ../lidar_odometry_step_1/points_global_spill.cpp "../../core/src/utils.cpp" ) diff --git a/apps/mandeye_raw_data_viewer/CMakeLists.txt b/apps/mandeye_raw_data_viewer/CMakeLists.txt index ad213454..7477ecdf 100644 --- a/apps/mandeye_raw_data_viewer/CMakeLists.txt +++ b/apps/mandeye_raw_data_viewer/CMakeLists.txt @@ -9,6 +9,8 @@ set(SOURCES ../lidar_odometry_step_1/lidar_odometry.cpp ../lidar_odometry_step_1/lidar_odometry_utils.cpp ../lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp + ../lidar_odometry_step_1/points_global_spill.cpp + ../lidar_odometry_step_1/raw_cloud_loader.cpp "../../core/src/utils.cpp" ) diff --git a/pybind/CMakeLists.txt b/pybind/CMakeLists.txt index 7f2df2b5..243de801 100644 --- a/pybind/CMakeLists.txt +++ b/pybind/CMakeLists.txt @@ -74,6 +74,8 @@ target_sources(lidar_odometry_py ${CMAKE_SOURCE_DIR}/apps/lidar_odometry_step_1/lidar_odometry.cpp ${CMAKE_SOURCE_DIR}/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp ${CMAKE_SOURCE_DIR}/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp + ${CMAKE_SOURCE_DIR}/apps/lidar_odometry_step_1/points_global_spill.cpp + ${CMAKE_SOURCE_DIR}/apps/lidar_odometry_step_1/raw_cloud_loader.cpp ${CMAKE_SOURCE_DIR}/apps/lidar_odometry_step_1/toml_io.cpp )