Skip to content

fix ground truth map loading with LocalMap voxel shifting to resolve … #59

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
adrienzhh wants to merge 2 commits into
base: ros2
Choose a base branch
from
Open

fix ground truth map loading with LocalMap voxel shifting to resolve … #59

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1800fa3
fix ground truth map loading with LocalMap voxel shifting to resolve …
Feb 2, 2026
c72657a
Merge branch 'superxslam:ros2' into ros2
adrienzhh Feb 12, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
./vins_estimator/camke-build-debug
./vins_estimator/camke-build-release
./super_odometry/camke-build-debug
./super_odometry/config/*.yaml
*.idea
*.vscode
./build
Expand All @@ -15,3 +16,4 @@ __pycache__
*.pyw
*.pyz
*.pywz
*.sh
4 changes: 4 additions & 0 deletions script/run.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,9 @@ session_name: SuperOdom_ROS2

suppress_history: false

options:
mouse: on

before_script: docker start superodom-ros2

windows:
Expand Down Expand Up @@ -37,3 +40,4 @@ windows:
- cd /root/ros2_ws/src/SuperOdom/super_odometry
- sleep 3
- rviz2 -d ros2.rviz

24 changes: 21 additions & 3 deletions super_odometry/include/super_odometry/LidarProcess/LocalMap.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -156,9 +156,18 @@ class LocalMap {
if(t_w_cur.z() + halfVoxelResulation < 0)
centerCubeK--;

origin_.x() = -centerCubeI;
origin_.y() = -centerCubeJ;
origin_.z() = -centerCubeK;
if(localization_mode_) {
// For localization: center the grid around the init position
// so that init position maps to the CENTER of the grid
origin_.x() = laserCloudWidth / 2 - centerCubeI;
origin_.y() = laserCloudHeight / 2 - centerCubeJ;
origin_.z() = laserCloudDepth / 2 - centerCubeK;
} else {
// For SLAM: original behavior
origin_.x() = -centerCubeI;
origin_.y() = -centerCubeJ;
origin_.z() = -centerCubeK;
}

return origin_;
} // function setOrigin end
Expand All @@ -180,6 +189,12 @@ class LocalMap {
if(t_w_cur.z() + halfVoxelResulation < 0)
centerCubeK--;

// In localization mode, DO NOT shift the map - keep GT map static
// Just return the current position in the grid
if(localization_mode_) {
return Eigen::Vector3i{centerCubeI, centerCubeJ, centerCubeK};
}

while(centerCubeI < 3) {
for(int j = 0; j < laserCloudHeight; j++) {
for(int k = 0; k < laserCloudDepth; k++) {
Expand Down Expand Up @@ -761,6 +776,9 @@ class LocalMap {
float planeRes_ = 0.4;

Eigen::Vector3i origin_;

// For localization mode: don't shift the map, keep GT map static
bool localization_mode_ = false;
};

#endif // LOCALMAPOCTREE_H
Loading