Map misalignment and GMM map question

[bboyack]

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?

[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.

[bboyack]

I got fastlio2 working (I had to remove nan points to get it to work), but I'm still having the same problem where the second session is slightly misaligned to the first session. It is misaligned right from the start with the ICP for the initial pose. After that, it never corrects itself. I tried reducing the down-sampling to provide the ICP algorithm with as many points as possible to align the initial cloud, but it still didn't work.

The blue point cloud is the initial guess from /initial_cloud. The orange is the prior map and the black is the second session

This image shows that the map never corrects itself.

I also tried changing the second session to be a different bag that starts in a different location, and I got the same results

[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.

[bboyack]

I implemented the changes you mentioned. It helped align the maps initially but the rest of the map is still misaligned. Now that I understand what's going on better, I think there is an issue with the loop closure between the frames and the prior map. As seen in the image below, there is initially some overlap, but as I move toward the previous map trajectory, the overlap gets smaller although I would expect it to get larger. Any thoughts with this?

Terminal output for second session:

Here is the corresponding map for the terminal image above:

Also, here are the parameters I'm using if it's helpful.

common:
  lid_topic: "/ouster_top/points"
  imu_topic: "/inertial_sense/imu_raw"

  acc_cov: 0.06498801215018458  # acc noise and bias
  b_acc_cov: 1.2085272913345782e-03
  gyr_cov: 7.628553521510558e-03   # gyro noise and bias
  b_gyr_cov: 2.510775466023761e-04
  extrinsic_T: [-0.937882, 0.202898, -1.5748]
  extrinsic_R: [-1.0,  0.0,  0.0,
                 0.0,  1.0,  0.0,
                 0.0,  0.0, -1.0]

  odom_link: "odom"
  scan_filter_size: 0.2
  map_viewer_size: 0.4
  map_saved_size: 0.2

  useImuFrame: false                        # when we use imu frame, we will get dense color cloud
  useLoopClosure: true                       # if we enable loop closure

  # save data
  saveResultBodyFrame: true
  saveLoopdata: false                       # useless, just preparing for the new algo
  saveKeyFrame: true                        # if save keyframe pcd
  

  # use cloudcompare or place recognition algorisms to generate the initial pose
  # Leave as identity and use Rviz to set initial pose
  initial_pose: [ 1.0, 0.0, 0.0, 0.0,
                  0.0, 1.0, 0.0, 0.0, 
                  0.0, 0.0, 1.0, 0.0,
                  0.0, 0.0, 0.0, 1.0 ]
  

pgo:
  SKIP_FRAMES: 10                                    # map visualization, every x frame we add it in the globalmap and publish it

  useKeyframe: true
  keyframe_meter_gap: 0.2
  keyframe_deg_gap: 0.1            
  
  useRawCloud: true
  method: 2                                       # ICP for Loop closure. 0: icp 1:point2plane 2:gicp
  baseline: 0                                     # 0:F2F, 1:M2F
  useMultiMode: true                              # true for multi-session, false for single session

  loopZOffset: 5.0                                # for escaltor
  historyKeyframeSearchRadius: 25.0
  historyKeyframeSearchTimeDiff: 40.0
  historyKeyframeSearchNum: 25
  loopFitnessScoreThreshold: 0.3
  loopClosureFrequency: 0.5                       # if you want to detect more loop, you can set it larger

  filterDis: 2.0                                  # we did not add loop if the distance of consecutive node less than 10m
  filterNodeNum: 10                               # we did noot detect loop every 10 nodes for ecah loop detetcion method

lio:
  lidar_type: 3                         # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai
  scan_line: 64
  timestamp_unit: 3                     # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
  blind: 4                            # remove the nearest point cloud

  fov_degree: 360
  det_range: 150.0

  point_filter_num: 8
  max_iteration: 3
  filter_size_corner: 0.2
  filter_size_surf: 0.6                 # 0.1-0.2 for indooor
  filter_size_map: 0.6
  cube_side_length: 1000

[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_corner: 0.2
  filter_size_surf: 0.5                 # 0.1-0.2 for indooor
  filter_size_map: 0.5
  cube_side_length: 1000

[bboyack]

Yes and no. I should have mentioned that I usually use the Rviz "2D Pose Estimate" to set the initial pose which is effectively the same thing as setting it in the config file (see the InitialCallback function in ms_mapping.cpp).

However, to rule this out as an issue, I changed the fast-lio parameters to match your suggestions and set the initial pose in the config as suggested, and it didn't make any difference in the inter session loop closure detection. It still fails every time.

[JokerJohn]

@bboyack

I can identify two issues here:

1. RViz's "2D Pose Estimate" only allows specifying (x, y, yaw), but the initial z, roll, and pitch cannot be set. This affects the quality of initial matching.
2. I haven't thoroughly tested the initialization function using RViz poses, and I'm uncertain if it works reliably in these scenarios.

Regarding your log, the issue is straightforward: loop closure detection is failed. Your source and target point clouds have reasonable point counts, so I suspect the problem is an inaccurate initial pose for the first frame.

Suggestion: Just try providing an accurate 6-DOF pose for the first frame and test again.

[bboyack]

Here is what I see between rviz and cloud compare.
Initial pose from rviz:

  -0.961971118   -0.273151181    0.000000000  106.941650391
   0.273151181   -0.961971118    0.000000000 -123.888809204
   0.000000000    0.000000000    1.000000000    0.000000000
   0.000000000    0.000000000    0.000000000    1.000000000

initial pose from rviz after initial ICP:

  -0.990776974   -0.135429482   -0.004454507  105.527189531
   0.135329114   -0.990636316    0.018047752 -124.117512525
  -0.006856994    0.017278472    0.999827203    2.161568120
   0.000000000    0.000000000    0.000000000    1.000000000

Initial pose from CloudCompare alignment:

  -0.991000000   -0.137000000   -0.007000000  105.537000000
   0.137000000   -0.990000000    0.016000000 -124.082000000
  -0.009000000    0.015000000    1.000000000    2.217000000
   0.000000000    0.000000000    0.000000000    1.000000000

Initial pose from CloudCompare after initial ICP:

  -0.991147692   -0.136030386   -0.004558861  105.530040011
   0.136053768   -0.990097127    0.017924005 -124.118273840
  -0.006828260    0.017245682    0.999981473    2.161760207
   0.000000000    0.000000000    0.000000000    1.000000000

Although the "2D pose estimator" in rviz may not be as accurate, after the initial ICP, there doesn't appear to be any significant differences between the rviz initial pose and the CloudCompare initial pose. I don't think that the initial pose is the issue in this case. However, for sake of eliminating variability, I will use the initial pose from CloudCompare until I figure out this issue.

While debugging, I decided to publish the cureKeyframeCloud and targetKeyframeCloud from the CloudProcess::PerformICP() funtion in cloud_process.cpp. I believe these are the two point clouds that are used to determine loop closures. In the image below, the red cloud is the cureKeyframeCloud and the green cloud is the targetKeyframeCloud. The green cloud is supposed to align with the 1st session map, but it doesn't.

However, if I continue until I have a loop closure within the same session (in session 2 in this case) then the target cloud aligns perfectly with the map and same session loop closures work great. I also tested it with the PK1 and RB2 data, and it doesn't actually appear to successfully perform any intersession loop closures either (at least per the debugging statements that are output by default).

[JokerJohn]

@bboyack

Let me clarify one point. Based on your description, the cureKeyframeCloud you publish for session2 is only in the LiDAR coordinate system and does not use the given initial pose to transform to the map coordinate system, thereby achieving registeration between the transformed cureKeyframeCloud and the localmap (session 1). As you can see from the following code reference:

icp = pipelines::registration::RegistrationGeneralizedICP(
    *source, *target, max_correspondence_distance, init_pose.cast<double>(),
    pipelines::registration::TransformationEstimationForGeneralizedICP(), criteria);

Additionally, I have already published this cureKeyframeCloud used for debugging. You can simply subscribe to the "/initial_cloud" topic in rviz to see if the initial pose is accurate, as shown in the code below:

publishCloud(pubInitialCloud, unused_result, ros::Time().fromSec(measurement.odom_time), odom_link);

If the initial pose is correct, continuously observe the following two topics to check if they are approximately in the same coordinate system. Ultimately, you still need to investigate why loop closure matching is failing. Based on your image observation, loop closure matching should work fine in this scenario.

I'm not sure how you tested with pk1 and rb2 without detecting constraints. If you use the code and initial pose I originally provided, you should be able to achieve results consistent with the provided PK1-RB2 data. You can check the pose_graph.g2o file in the data to examine the corresponding constraints.

[bboyack]

Sorry, my last comment must have been confusing so let me try to clarify my issue a little bit more and check my understanding.

Initialization:
I am using CloudCompare to get an initial transformation and setting that in the param file. I am able to verify the initial pose is working correctly by looking at the published /localization_map topic and /initial_cloud topic. I'm good there and everything initializes exactly as I would expect.

Loop Closures Between Sessions:
Please check my understanding. Theoretically, what is supposed to happen is as my robot traverses an area that has already been seen before, session 1, and it crosses or is close to the session 1 trajectory, it should perform a loop closure between the session 2 trajectory and session 1 trajectory constraining the session 2 map to be consistent (aligned) with the session 1 map. Is this correctl?

Assuming the above is correct, I should be able to visually see a loop closure in rviz between the blue (session 1) and red (session 2) trajectories as well as see a terminal output of ICP Loop detected!. However, I don't see either of these happening on my data or the PK1-RB2 data.

Debugging:
The variable pubLoopScanLocal is never actually used anywhere and thus /loop_scan_local never actually has any data. Also, /loop_submap_local is only published to when there is a loop closure which doesn't help for determining why loop closures aren't happening.

This is why I created my own publishers to publish the cureKeyframeCloud and targetKeyframeCloud (as mentioned in my previous comment). The pipeline for these is MSMapping::LoopDetection() -> MSMapping::PerformRSLoopClosure() -> CloudProcess::DoICPVirtualRelative3() -> CloudProcess::PerformICP() -> cureKeyframeCloud & targetKeyframeCloud. My previous image of this was not very good because it was close to the initialization point, but hopefully this next image is more clear. The red scan is the cureKeyframeCloud (this matches what I would expect) and the green scan is the targetKeyframeCloud this publishes the exact same as /loop_submap_local except it publishes every time a loop closure is attempted and not just when one succeeds. I believe that the green point cloud (loop submap) should always align with the prior map, but as seen below, it doesn't. The only time it aligns is when loop closures are attepted between session 2 and session 2.

If I'm missing something or not understanding something fundamentally, please help me understand it better. Thanks for your patience and persistence with helping me with this issue.

[JokerJohn]

According to the algorithm principles, assuming the initial pose is accurate, several parameters affect the loop closure execution:

loopZOffset: 5.0                                # for escalator
historyKeyframeSearchRadius: 25.0
historyKeyframeSearchTimeDiff: 40.0
historyKeyframeSearchNum: 25
loopFitnessScoreThreshold: 0.3
loopClosureFrequency: 0.5                       # if you want to detect more loops, you can set it larger
filterDis: 2.0                                  # we do not add loop if the distance of consecutive nodes is less than 2m
filterNodeNum: 10                               # we do not detect loop every 10 nodes for each loop detection method

1. loopClosureFrequency: This parameter determines the execution frequency of the loop closure function, with loop detection running at 0.5Hz by default.
2. filterDis: This parameter filters loop closures based on cumulative motion distance. After detecting a previous loop closure, it waits for an additional 2 meters before continuing to add next loop closures.
3. filterNodeNum: This parameter filters some loop closures. If the current node establishes a loop constraint, loop constraints will only begin to be added after the 10th subsequent node.
4. loopZOffset: If XY are close encough but Z-axis differences are large (e.g., going up/down stairs), loop detection is not considered.

Please check if these parameters are causing the issue.

If it's not due to these parameter issues and loop detection executes ICP but registeration fails, you should further examine the ICP parameters. Pay attention to the following:

1. historyKeyframeSearchRadius: Can be increased, e.g., to 30 meters
2. loopFitnessScoreThreshold: Can be increased to relax the ICP registeration score and allow more loop constraints
3. historyKeyframeSearchNum: Can be increased to 30 for building local maps with more frames

However, I see that your targetKeyframeCloud (green point cloud) appears obviously incorrect, which may be the core reason for loop detection registeration failure. I haven't encountered this specific situation before, so I'm not clear on the exact cause. If you can provide test data, I might be able to help debug the issue.

[bboyack]

Here is the link to two of the bags I've been using, the results from the first bag, and my config and launch files. The link will be valid for 3 days. Note that the inertial_sense imu is in NED coordinates. Fast-lio converts everything to the imu frame so the map is upside down in a global reference.

https://byu.box.com/s/khch8bevj2rd3bhrr54k28am7vhb3vsw

I just recloned a fresh copy of MS-Mapping, moved the publisher for /loop_submap_local to be before the "if" statement, and ran RB2 with PK1 as the prior map. Although the maps initialize well and appear to be fairly well aligned, I still don't see any loop closures happening between PK1 and RB2, and the /loop_submap_local (blue points) is still not correct.

[JokerJohn]

@bboyack

Loop closure should work normally during single-session mapping, but fails during multi-session mapping. I suspect the issue lies in the CreateLocalMapFromMeasurements function, which may not be accurately constructing a local map by concatenating data from the old session. I'll debug this when I have time. I recall that this issue didn't exist initially, so it's possible I inadvertently modified some parts while organizing the code previously.