Lidar odometry: bounded memory for long recordings

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"
 )

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<bool>& 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<double> elapsed_seconds = end - start;
@@ -1549,7 +1560,7 @@ void openData()
         loRunning = false;
 }
 
-void step1(
+bool step1(
     const std::string& folder,
     LidarOdometryParams& params,
     std::vector<std::vector<Point3Di>>& 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<WorkerData>& worker_data, LidarOdometryParams& params, const std::atomic<bool>& loPause)
+bool step2(std::vector<WorkerData>& worker_data, LidarOdometryParams& params, const std::atomic<bool>& loPause)
 {
     double ts_failure = 0.0;
     std::atomic<float> 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<double> elapsed_seconds = end - start;

apps/lidar_odometry_step_1/lidar_odometry.cpp

@@ -1,13 +1,47 @@
 #include "lidar_odometry.h"
 #include "hdmapping_profiler.hpp"
+#include "raw_cloud_loader.h"
 #include "tbb/tbb.h"
 #include <mutex>
 
 #include <Core/system_info.hpp>
 #include <Fusion.h>
+#if defined(__GLIBC__)
+#include <malloc.h>
+#endif
+
+#include <functional>
 
 namespace fs = std::filesystem;
 
+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<Point3Di>& 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";
+        }
+}
+
 bool load_data(
     std::vector<std::string>& input_file_names,
     LidarOdometryParams& params,
@@ -239,6 +273,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<RawCloudLoader>();
+            auto& L = *params.raw_cloud_loader;
+            // a reload (the GUI opening another folder) must not keep the previous session's clouds
+            std::vector<std::vector<Point3Di>>(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<std::string> 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 +348,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 +539,7 @@ void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, LidarOdometryPa
 }
 
 bool compute_step_1(
-    const std::vector<std::vector<Point3Di>>& pointsPerFile,
+    std::vector<std::vector<Point3Di>>& pointsPerFile,
     LidarOdometryParams& params,
     Trajectory& trajectory,
     std::vector<WorkerData>& worker_data,
@@ -482,8 +552,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 +662,25 @@ bool compute_step_1(
 
         std::vector<Point3Di> 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 +1185,13 @@ std::string save_results_automatic(
     return outwd.string();
 }
 
+void release_raw_clouds(std::vector<std::vector<Point3Di>>& pointsPerFile, LidarOdometryParams& params)
+{
+    std::vector<std::vector<Point3Di>>().swap(pointsPerFile);
+    params.raw_cloud_loader.reset();
+    release_freed_heap();
+}
+
 std::vector<WorkerData> run_lidar_odometry(const std::string& input_dir, LidarOdometryParams& params)
 {
     HDMAP_ZONE_SCOPE("run_lidar_odometry");
@@ -1107,6 +1222,10 @@ std::vector<WorkerData> 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<float> loProgress;

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<std::vector<Point3Di>>& pointsPerFile,
+    std::vector<std::vector<Point3Di>>& pointsPerFile,
     LidarOdometryParams& params,
     Trajectory& trajectory,
     std::vector<WorkerData>& worker_data,
     const std::atomic<bool>& 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<std::vector<Point3Di>>& pointsPerFile, LidarOdometryParams& params);
 void run_consistency(std::vector<WorkerData>& worker_data, const LidarOdometryParams& params);
 void filter_reference_buckets(LidarOdometryParams& params);
 void load_reference_point_clouds(const std::vector<std::string>& input_file_names, LidarOdometryParams& params);