Prevent double-transform of prior-session keyframes during map load

[Copilot]

Multi-session runs showed near-overlap but persistent local misalignment and unstable cross-session loop registration. The issue path is consistent with prior-session local maps being constructed from clouds that were no longer in their original LiDAR frame.

• Root cause addressed: cloud frame mutation in prior-map loading

  • DataSaver::ReadPosesAndPointClouds was transforming each loaded keyframe cloud in place after assigning it to measurement.lidar.
  • That mutates stored keyframes from local frame to world frame, but downstream loop/local-map code assumes measurement.lidar is local and applies pose transforms again.

• Change made: preserve local-frame storage, transform only for aggregation

  • Keep measurements[index].lidar unchanged (raw/local frame).
  • Introduce a temporary cloud_world for global map accumulation only.

• Impact on multi-session behavior

  • Removes accidental double transformation of prior-session keyframes used in loop/local map construction.
  • Aligns local-map/ICP inputs with expected coordinate-frame semantics across session boundaries.

measurements[index].lidar = cloud;  // keep local frame

Pose3 pose = Pose6dTogtsamPose3(measurements.at(index).updated_pose);
Eigen::Matrix4f transformation = pose.matrix().cast<float>();
transformPointCloud(*cloud, *cloud_world, transformation);  // world-frame copy only
*globalmap_ptr += *cloud_world;

Original prompt

---

This section details on the original issue you should resolve

<issue_title>Map misalignment and GMM map question</issue_title>
<issue_description>I had a hard time getting fastlio working correctly for some reason so I switched to using lio-sam (followed comments in #9). I created a prior map just fine, and used rviz to set the pose for the next mapping session. It completed successfully; however, I realized that the map alignment isn't correct in a few places. It's close but not quite there. Is this expected?

Also, as seen in the above image and in the images in the tutorial section. Points are added to the prior map for more than just new objects (e.g. ground points). From the paper, I thought that the GMM idea was that only points that were significantly different from the prior map would be added, but this isn't what I'm seeing in practice. Am I misunderstanding something?</issue_description>

<agent_instructions>check if this problem exits?</agent_instructions>

Comments on the Issue (you are @copilot in this section)

@JokerJohn Hi @bboyack，

I think this issue may be related to the accuracy of LIO. I suggest trying Fast-LIO2 first to rule it out. When adapting Fast-LIO2, the key point is to ensure that the timestamps of points in the handler function are in milliseconds and fall within the range 0–100 ms.

Regarding your GMM module, I’m not sure how it has been implemented, so it’s difficult to trace the exact cause of the problem you mentioned. Theoretically, frames that have significant difference with the prior map should be treated as redundant and thus not used in the merged map.</comment_new>
<comment_new>@JokerJohn
Hi, @bboyack

I understand your issue now. The problem stems from pose errors in the initial frame of session 2, which leads to poor alignment. I suggest checking the following configurations:

1. Ensure you're using F2F mode instead of M2F. M2F performs localization on a prior map to achieve map merging. From a graph optimization perspective, it can be considered as two discrete pose graphs, which is suitable when session 1 has high accuracy and you want to fix session 1's coordinate system. F2F, however, constructs a complete pose graph through loop closure constraints, and with proper loop closure noise settings, allows all nodes in the graph to be adjusted.
2. Confirm the accuracy of the initial pose (obtained through manual registeration using CloudCompare). Your visualization in rviz looks reasonable.
3. Verify the registeration quality of session 2's first frame when the program starts running (using Open3D ICP), including the registeration score and overlap ratio (fitness).

If all the above are satisfactory and the problem persists, it indicates that the pose graph optimization hasn't properly adjusted the constraint edges at the starting point of session 2.
This occurs because the code directly applies odometry noise (1e-6, 1e-4) to construct relative constraints between the starting point and session 1's old pose graph. The system places high trust in this relative pose and therefore makes minimal adjustments. You can modify this to use loop closure noise (my default setting is 1e-2, 1e-2), allowing the graph optimization to adjust this constraint with loop closure weights.

auto LOOPGaussianNoise = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished());

Additionally, there's an alternative approach: if you have high confidence in the initial pose, you can directly use M2F mode, which will fix session 2's initial point. The drawback is that the overall map consistency may not be as good.

@JokerJohn @bboyack

After reviewing your log, I've found several issues:

1. You haven't set the initial pose. You need to set the initial_pose by manually aligning session 2's first frame with session 1's map using CloudCompare (this process is described in the tutorial, you can find this two point cloud in the results folder).
2. failed to establish the loop closure constraints between session 2 and session 1. Due to the incorrect initial pose, the subsequent loop detection ICP refinement processes all failed. The reason your execution proceeded smoothly is simply because session 2's default initial pose happened to be near session 1's map.

I also notice some issues with your Fast-LIO parameter settings. I recommend configuring:

  blind: 1.0                           # remove the nearest point cloud

  fov_degree: 360
  det_range: 150.0

  point_filter_num: 4
  max_iteration: 3
  filter_size_cor...

</details>



<!-- START COPILOT CODING AGENT SUFFIX -->

- Fixes JokerJohn/MS-Mapping#10

<!-- START COPILOT CODING AGENT TIPS -->
---

⚡ Quickly spin up Copilot coding agent tasks from anywhere on your macOS or Windows machine with [Raycast](https://gh.io/cca-raycast-docs).