Alignment separation#2
Open
sevahul wants to merge 12 commits into
Open
Conversation
added 11 commits
April 23, 2026 20:15
- split mapOptimization monolith into dedicated source files for core, map, scan, GPS, loop, and publishing paths - add shared mapOptimization header and update build sources - fix post-split linker and warning issues for multi-TU build - update architecture and developer docs for new layout
…igurable kNN + map params - add XYZRGB colored debug cloud publisher (surf_debug_colored) with per-point rejection reason codes (neighbor_count, knn_distance, plane_invalid, low_weight, accepted, not_optimized) - add legend topic (surf_debug_legend) alongside colored cloud - add matched surface features publisher (matched_surface_features) - add surfKnnMinDistance param to replace hardcoded 1.0m kNN gate - add localMapTruncationRadius param separate from surroundingKeyframeSearchRadius for independent voxel map pruning - populate laserCloudSurfDebugCode per-point in surfOptimization() with OMP reduction-based stage counters - remove per-stage cloud publishers (input/knn_pass/plane_valid); keep only combined colored cloud and matched features
There was a problem hiding this comment.
Pull request overview
This PR extracts scan-to-map alignment into a dedicated ScanAligner object and refactors the mapOptimization node into multiple translation units, alongside expanding diagnostics/telemetry and adding scan “turn-quality” metadata into CloudInfo.
Changes:
- Introduces
ScanAligner(newscanAlignment/module) and wires scan-to-map optimization through it. - Splits the monolithic map optimization implementation into
mapOptimization_*compilation units with a newmain.cppentry point. - Adds/extends diagnostics + telemetry outputs, GPS/deskew gating metadata (
scan_admission_ok, angular speed/span, scan duration), and updates build/config/docs accordingly.
Reviewed changes
Copilot reviewed 27 out of 28 changed files in this pull request and generated 4 comments.
Show a summary per file
| File | Description |
|---|---|
| src/scanAlignment/ScanAligner.cpp | New scan alignment implementation (surf matching + LM optimization) extracted from map optimization. |
| include/scanAlignment/ScanAligner.hpp | New ScanAligner API + alignment metrics/debug codes for visualization. |
| src/mapOptimization/mapOptimization_core.cpp | Refactored node constructor + LiDAR callback orchestration; instantiates ScanAligner. |
| src/mapOptimization/mapOptimization_scan.cpp | Refactored scan-to-map pipeline to call ScanAligner and record per-stage metrics. |
| src/mapOptimization/mapOptimization_publish.cpp | Refactored TF/odometry/frame publishing; adds surf debug colored publishing. |
| src/mapOptimization/mapOptimization_map.cpp | Refactored local map management (voxel hash map + rolling map updates). |
| src/mapOptimization/mapOptimization_loop.cpp | Refactored loop closure implementation into its own TU. |
| src/mapOptimization/mapOptimization_graph.cpp | Refactored factor graph orchestration + improved GTSAM exception logging context. |
| src/mapOptimization/mapOptimization_gps.cpp | Refactored GPS datum/anchor/factor fusion + richer diagnostics. |
| src/mapOptimization/main.cpp | New executable entry point for the split mapOptimization node. |
| src/liorf_diagnostics.cpp | Moves diagnostics implementation to a .cpp and expands outputs (timing, events, warnings, telemetry). |
| include/liorf_diagnostics.h | Header now declares diagnostics API (implementation moved to .cpp). |
| src/imuPreintegration.cpp | Adds exception handling and richer context logs around GTSAM optimizer failures. |
| src/imageProjection.cpp | Adds scan turn-quality summary fields and gating behavior into CloudInfo. |
| msg/CloudInfo.msg | Adds scan admission + angular speed/span + scan duration fields. |
| include/utility.h | Adds new params (prediction/gating/GPS alignment), fixes ODR via inline, and initializes QoS profile safely. |
| include/Scancontext.cpp | Hardens xy2theta() with atan2 and adds <cmath>. |
| CMakeLists.txt | Updates build to compile split sources; adds diagnostics library + fastcdr dependency; optional GCC warning suppression. |
| package.xml | Adds fastcdr dependency. |
| config/lio_sam_ouster.yaml | Adds new params (turn gating, prediction clamp, GPS alignment window) and adjusts mapping/loop settings. |
| config/lio_sam_default.yaml | Adds new GPS alignment params. |
| rviz/mapping.rviz | Updates RViz config to visualize new debug topics/clouds. |
| README.md | Updates workspace/run instructions and documents mapOptimization file layout + telemetry artifacts. |
| ARCHITECTURE.md | Documents the new mapOptimization split layout. |
| AGENTS.md | Updates repo/module structure documentation for the split implementation. |
| CHANGELOG.md | Records refactor + new diagnostics/scan debug behavior and build changes. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
|
|
||
| std::vector<PointType> laserCloudOriSurfVec; | ||
| std::vector<PointType> coeffSelSurfVec; | ||
| std::vector<bool> laserCloudOriSurfFlag; |
| float pc = matX0(2, 0); | ||
| float pd = 1; | ||
|
|
||
| float ps = sqrt(pa * pa + pb * pb + pc * pc); |
| void mapOptimization::updatePath(const PointTypePose& pose_in) | ||
| { | ||
| geometry_msgs::msg::PoseStamped pose_stamped; | ||
| rclcpp::Time t(static_cast<uint32_t>(pose_in.time * 1e9)); |
Comment on lines
175
to
193
| ### Plot time-slicing statistics | ||
|
|
||
| Diagnostics logging also maintains a stable symlink: | ||
|
|
||
| - `~/.ros/liorf_logs/latest` -> newest `run_YYYYMMDD_HHMMSS` directory | ||
|
|
||
| After a run, plot `timing_stats.csv` from diagnostics logs: | ||
|
|
||
| ```bash | ||
| python3 scripts/plot_time_slicing_stats.py | ||
| ``` | ||
|
|
||
| Diagnostics telemetry is also persisted as `telemetry.csv` in each run directory, including: | ||
|
|
||
| - `time_since_last_lidar_s` | ||
| - `time_since_last_gps_s` | ||
|
|
||
|
|
||
|
|
||
| By default, the script reads the latest run (via `~/.ros/liorf_logs/latest` when available), saves the PNG plot, and displays it. |
sevahul
force-pushed
the
alignment_separation
branch
from
dfcdc9d to
b19e197
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Separate the scan alignment into a separate object