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/ diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index b0e2fb47..7dc281e2 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,49 @@ 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: 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) + { + 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); + 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) + { + if (org_p.point.norm() > params.threshould_output_filter) + { + 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); + } + } + } + + 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..dd284a7e 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -201,6 +201,16 @@ 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; + // 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; 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; }