From fed9ced7e9604e7d59d08d17adc2df8d2576ef65 Mon Sep 17 00:00:00 2001 From: Piotr Pioro Date: Mon, 29 Jun 2026 14:57:36 +0200 Subject: [PATCH 1/3] Step 1: classify moving objects as LAS class 7 Points whose indoor RGD bucket was raycast-through (number_of_hits >= threshold, same signal that excludes them from LIO optimization) are now exported in scan_lio_*.laz with LAS classification 7; static points stay class 0. - export_laz.h: writePoint sets point classification; exportLaz takes an optional per-point classification vector (defaults keep existing callers unchanged). - lidar_odometry.cpp: save_result queries params.buckets_indoor per output point and builds the classification vector before exportLaz. - LidarOdometryParams: classify_moving_objects (default on) + moving_object_hits_threshold (default 20), persisted via TOML [moving_objects] section and exposed in the GUI menu. Scope is Step 1 only; preserving the class through Step 2 / insightoBake to the final LAS export is handled on a separate branch. Co-Authored-By: Claude Opus 4.8 --- apps/lidar_odometry_step_1/lidar_odometry.cpp | 35 ++++++++++++++++++- .../lidar_odometry_gui.cpp | 11 ++++++ .../lidar_odometry_utils.h | 5 +++ apps/lidar_odometry_step_1/toml_io.h | 5 ++- core/include/Core/export_laz.h | 10 ++++-- 5 files changed, 61 insertions(+), 5 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index b0e2fb47..f92029fe 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -3,6 +3,7 @@ #include "tbb/tbb.h" #include +#include #include #include @@ -853,7 +854,39 @@ void save_result(std::vector& worker_data, LidarOdometryParams& para timestamps, true, params.save_index_pose); - exportLaz(path.string(), global_pointcloud, intensity, timestamps); + + // Classify moving objects: points whose indoor RGD bucket was hit through (number_of_hits >= + // threshold) are dynamic and get LAS classification 7; everything else stays class 0. + std::vector classifications; + if (params.classify_moving_objects) + { + const Eigen::Vector3d b_indoor( + params.in_out_params_indoor.resolution_X, + params.in_out_params_indoor.resolution_Y, + params.in_out_params_indoor.resolution_Z); + classifications.assign(global_pointcloud.size(), 0); + std::lock_guard lck(params.mutex_buckets_indoor); + for (size_t k = 0; k < global_pointcloud.size(); ++k) + { + const auto key = get_rgd_index_3d(global_pointcloud[k], b_indoor); + const auto it = params.buckets_indoor.find(key); + if (it != params.buckets_indoor.end() && static_cast(it->second.number_of_hits) >= params.moving_object_hits_threshold) + { + classifications[k] = 7; + } + } + } + + exportLaz( + path.string(), + global_pointcloud, + intensity, + timestamps, + 0.0, + 0.0, + 0.0, + nullptr, + params.classify_moving_objects ? &classifications : nullptr); if (params.save_index_pose) { diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index a21a8858..94bd35ee 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -2323,6 +2323,17 @@ void display() ImGui::Separator(); + ImGui::MenuItem("Classify moving objects (LAS class 7)", nullptr, ¶ms.classify_moving_objects); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Export points detected as moving (dynamic) with LAS classification 7; static points stay class 0"); + if (params.classify_moving_objects) + { + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Moving object hits threshold", ¶ms.moving_object_hits_threshold); + } + + ImGui::Separator(); + ImGui::MenuItem("Saving results with index pose", nullptr, ¶ms.save_index_pose); ImGui::Separator(); diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 2f64e95d..c6e6f676 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -201,6 +201,11 @@ struct LidarOdometryParams bool ablation_study_use_view_point_and_normal_vectors = true; bool ablation_study_use_threshold_outer_rgd = false; bool save_index_pose = false; + + // moving objects: points whose RGD bucket has number_of_hits >= moving_object_hits_threshold are + // treated as dynamic/movable and exported with LAS classification 7 (static points stay class 0) + bool classify_moving_objects = true; + int moving_object_hits_threshold = 20; }; inline VQFParams buildVQFParams(const LidarOdometryParams& p) diff --git a/apps/lidar_odometry_step_1/toml_io.h b/apps/lidar_odometry_step_1/toml_io.h index b4001e49..09ce5306 100644 --- a/apps/lidar_odometry_step_1/toml_io.h +++ b/apps/lidar_odometry_step_1/toml_io.h @@ -111,7 +111,9 @@ class TomlIO { "current_output_dir", &LidarOdometryParams::current_output_dir }, { "working_directory_preview", &LidarOdometryParams::working_directory_preview }, { "use_imu_preintegration", &LidarOdometryParams::use_imu_preintegration }, - { "imu_preintegration_method", &LidarOdometryParams::imu_preintegration_method } + { "imu_preintegration_method", &LidarOdometryParams::imu_preintegration_method }, + { "classify_moving_objects", &LidarOdometryParams::classify_moving_objects }, + { "moving_object_hits_threshold", &LidarOdometryParams::moving_object_hits_threshold } }; // Special handling for TaitBryanPose members @@ -205,6 +207,7 @@ class TomlIO "rgd_sf_sigma_fi_deg", "rgd_sf_sigma_ka_deg" } }, { "imu_preintegration", { "use_imu_preintegration", "imu_preintegration_method" } }, + { "moving_objects", { "classify_moving_objects", "moving_object_hits_threshold" } }, { "paths", { "current_output_dir", "working_directory_preview" } }, { "misc", { "clear_color" } } }; diff --git a/core/include/Core/export_laz.h b/core/include/Core/export_laz.h index b3a6ef09..c4e1bfb6 100644 --- a/core/include/Core/export_laz.h +++ b/core/include/Core/export_laz.h @@ -86,13 +86,15 @@ class LazWriter double offset_x, double offset_y, double offset_z, - unsigned short point_source_id = 0) + unsigned short point_source_id = 0, + unsigned char classification = 0) { point_->intensity = intensity; point_->return_number = 1; point_->number_of_returns = 1; point_->gps_time = timestamp * 1e9; point_->point_source_ID = point_source_id; + point_->classification = classification; laszip_F64 coordinates[3]; coordinates[0] = position.x() + offset_x; @@ -164,7 +166,8 @@ inline bool exportLaz( double offset_x = 0.0, double offset_y = 0.0, double offset_alt = 0.0, - const std::vector* point_source_ids = nullptr) + const std::vector* point_source_ids = nullptr, + const std::vector* classifications = nullptr) { LazWriter writer; if (!writer.open(filename, offset_x, offset_y, offset_alt)) @@ -173,7 +176,8 @@ inline bool exportLaz( for (size_t i = 0; i < pointcloud.size(); i++) { const unsigned short psid = (point_source_ids && i < point_source_ids->size()) ? (*point_source_ids)[i] : 0; - if (!writer.writePoint(pointcloud[i], intensity[i], timestamps[i], offset_x, offset_y, offset_alt, psid)) + const unsigned char classification = (classifications && i < classifications->size()) ? (*classifications)[i] : 0; + if (!writer.writePoint(pointcloud[i], intensity[i], timestamps[i], offset_x, offset_y, offset_alt, psid, classification)) return false; } From d3eea6bc859f44a97a28aa430096b79beb4aeff5 Mon Sep 17 00:00:00 2001 From: Piotr Pioro Date: Mon, 29 Jun 2026 15:03:45 +0200 Subject: [PATCH 2/3] Ignore 3rdpartyBinary/untwine_env (out-of-band conda env) The untwine + libpdal-e57 conda env is created locally for COPC/E57 conversion and must not be tracked; without this, git add -A pulls in thousands of env files. Co-Authored-By: Claude Opus 4.8 --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index fd91924d..77445377 100644 --- a/.gitignore +++ b/.gitignore @@ -72,3 +72,6 @@ imgui.ini /.gitmodules /build2 /build3 + +# conda env for COPC/E57 conversion (untwine + libpdal-e57), created out-of-band +/3rdpartyBinary/untwine_env/ From 66fe695bb1bbffcbfd9d49444b3b0996a0e19ed4 Mon Sep 17 00:00:00 2001 From: Piotr Pioro Date: Mon, 29 Jun 2026 15:40:26 +0200 Subject: [PATCH 3/3] Fix moving-object classification across sliding windows MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The first version queried params.buckets_indoor in save_result(), but the RGD grids are transient: process_worker_step_update_rgd_after() clears buckets_indoor/outdoor on every sliding-window shift (~5 m), so at save time only the last window survived — points were marked class 7 only at the very end of the trajectory. It also ignored the outdoor grid and used first-pose-relative coords for the bucket key (world coords were needed). Now compute_step_2() accumulates the keys of every bucket that reached the moving-object hit threshold into params.moving_buckets_indoor/outdoor, snapshotting before each grid clear (segment peak) and once after the loop (last window). save_result() classifies each output point by looking up its world-coordinate bucket key (indoor OR outdoor) in those sets, mirroring the points_to_vector output filter so the per-point vector stays aligned. Co-Authored-By: Claude Opus 4.8 --- apps/lidar_odometry_step_1/lidar_odometry.cpp | 28 +++++++++++++------ .../lidar_odometry_utils.h | 5 ++++ .../lidar_odometry_utils_optimizers.cpp | 28 +++++++++++++++++++ 3 files changed, 52 insertions(+), 9 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index f92029fe..7dc281e2 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -855,8 +855,13 @@ void save_result(std::vector& worker_data, LidarOdometryParams& para true, params.save_index_pose); - // Classify moving objects: points whose indoor RGD bucket was hit through (number_of_hits >= - // threshold) are dynamic and get LAS classification 7; everything else stays class 0. + // Classify moving objects: a point is dynamic (LAS class 7) if its indoor OR outdoor RGD bucket + // reached the moving-object hit threshold anywhere along the trajectory. compute_step_2 accumulated + // those bucket keys into params.moving_buckets_* (the live grids are transient per sliding window, + // so they can't be queried here). The key must use WORLD coordinates (trajectory[index_pose]*point), + // matching how the grids were built - not the first-pose-relative coords written to the .laz. This + // loop mirrors the threshould_output_filter of points_to_vector so it stays index-aligned with + // global_pointcloud. std::vector classifications; if (params.classify_moving_objects) { @@ -864,15 +869,20 @@ void save_result(std::vector& worker_data, LidarOdometryParams& para params.in_out_params_indoor.resolution_X, params.in_out_params_indoor.resolution_Y, params.in_out_params_indoor.resolution_Z); - classifications.assign(global_pointcloud.size(), 0); - std::lock_guard lck(params.mutex_buckets_indoor); - for (size_t k = 0; k < global_pointcloud.size(); ++k) + const Eigen::Vector3d b_outdoor( + params.in_out_params_outdoor.resolution_X, + params.in_out_params_outdoor.resolution_Y, + params.in_out_params_outdoor.resolution_Z); + const auto& traj = worker_data_concatenated[i].intermediate_trajectory; + classifications.reserve(global_pointcloud.size()); + for (const auto& org_p : original_points_to_save) { - const auto key = get_rgd_index_3d(global_pointcloud[k], b_indoor); - const auto it = params.buckets_indoor.find(key); - if (it != params.buckets_indoor.end() && static_cast(it->second.number_of_hits) >= params.moving_object_hits_threshold) + if (org_p.point.norm() > params.threshould_output_filter) { - classifications[k] = 7; + const Eigen::Vector3d world = traj[org_p.index_pose] * org_p.point; + const bool moving = params.moving_buckets_indoor.count(get_rgd_index_3d(world, b_indoor)) != 0 || + params.moving_buckets_outdoor.count(get_rgd_index_3d(world, b_outdoor)) != 0; + classifications.push_back(moving ? 7 : 0); } } } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index c6e6f676..dd284a7e 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -206,6 +206,11 @@ struct LidarOdometryParams // treated as dynamic/movable and exported with LAS classification 7 (static points stay class 0) bool classify_moving_objects = true; int moving_object_hits_threshold = 20; + // Union of moving bucket keys (indexed by get_rgd_index_3d) accumulated across the whole trajectory + // during compute_step_2. The RGD grids are transient (cleared each sliding window), so we snapshot + // the moving buckets before each clear; these sets persist until save_result() classifies points. + ankerl::unordered_dense::set moving_buckets_indoor; + ankerl::unordered_dense::set moving_buckets_outdoor; }; inline VQFParams buildVQFParams(const LidarOdometryParams& p) 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 1611bdc0..f8d86362 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2743,6 +2743,22 @@ bool process_worker_step_lidar_odometry_core( return true; } +// The RGD grids are transient (cleared each sliding window), so we record the keys of all buckets that +// reached the moving-object hit threshold into persistent sets on params. Their union over the whole +// trajectory is later used by save_result() to assign LAS classification 7. Bucket keys are the map keys +// (get_rgd_index_3d), deterministic for a given resolution and world coordinates. +static void snapshot_moving_buckets(LidarOdometryParams& params) +{ + const int threshold = params.moving_object_hits_threshold; + std::scoped_lock lock(params.mutex_buckets_indoor, params.mutex_buckets_outdoor); + for (const auto& [key, bucket] : params.buckets_indoor) + if (static_cast(bucket.number_of_hits) >= threshold) + params.moving_buckets_indoor.insert(key); + for (const auto& [key, bucket] : params.buckets_outdoor) + if (static_cast(bucket.number_of_hits) >= threshold) + params.moving_buckets_outdoor.insert(key); +} + bool process_worker_step_update_rgd_after( double& acc_distance, LidarOdometryParams& params, @@ -2755,6 +2771,10 @@ bool process_worker_step_update_rgd_after( { spdlog::stopwatch stopwatch_update; + // Capture moving buckets at the segment peak, before the grids are cleared for the next window. + if (params.classify_moving_objects) + snapshot_moving_buckets(params); + if (params.reference_points.size() == 0) { params.buckets_indoor.clear(); @@ -2860,6 +2880,10 @@ bool compute_step_2( LookupStats lookup_stats; std::vector points_global; + // Fresh run: drop moving-bucket keys accumulated by a previous compute_step_2(). + params.moving_buckets_indoor.clear(); + params.moving_buckets_outdoor.clear(); + if (initialize_lidar_odometry(worker_data, params, ts_failure, loProgress, pause, debugMsg, lookup_stats)) { for (int i = 0; i < worker_data.size(); i++) @@ -3060,6 +3084,10 @@ bool compute_step_2( HDMAP_ZONE_END(after_iter); } + // Last sliding window is never cleared after the loop: capture its moving buckets too. + if (params.classify_moving_objects) + snapshot_moving_buckets(params); + for (int i = 0; i < worker_data.size(); i++) worker_data[i].intermediate_trajectory_motion_model = worker_data[i].intermediate_trajectory;