Submapping with a solid state lidar

[gfr-misha]

Currently, we are able to get very good results building maps from only visual inertial information. However, when we enable submapping, the bundle adjustment never converges. The system uses a triple camera solution with a non-overlapping FOV. So basically, the trajectory without submapping enabled comes out better currently.

Our LIDAR is a solid-state front-facing module with around 90 deg FOV. For the Super8 submapping to work well and produce meaningful factors, should it be a 360 lidar? I don't see any unusual logs when enabling the submapping, except that BA never converges.

What would be the best way to debug it? I tried to play with se2-lidar.yaml based on hilti2022 dataset, but was not successful - not all parameters are easy to understand there.

Thanks for doing such amazing work and open-sourcing it!

[SebsBarbas]

Hi!
Do you have framedrops? If so, I have added two new commits that improve the speed of the LiDAR integration and processing, which would benefit if it is the case. Also, what do you mean with the bundle adjustment not converging? Does the state-estimator diverge? Could you share a rosbag2 and the config files so that we can help you debug the issue? Thanks in advance

[SebsBarbas]

By the way, I have added a wiki on the repository with the hope that it can help you and other users to better understand the different parameters. If you ould provide us feedback it would be really helpful to further improve it!

[gfr-misha]

We use the synchronous app, so frame drops shouldn't happen, but the speed of integration is definitely welcome. We can't yet share the dataset, but the main question is whether the super eight submapping setup is suitable to work well with a narrow FOV lidar, or does it expect a 360 deg?

Thanks for the wiki!

[bochsim]

Hi @gfr-misha

Thanks a lot for using our framework!

We have thought about it carefully and while technically we support any type of LiDAR sensor (on the integration side, we do not make any assumption on the sensor type and treat every measurement as a separate measurement), we realized that parts of the pipeline - specifically the logic on when to create new keyframes or submaps and how to ensure overlaps between submaps (to use it for the tightly-coupled optimization) - have been designed based on assuming a 360 degree FoV.

So, I could imagine, that with a very near FoV LIDAR, we might create a larger number of submaps than necessary and potentially have a very limited overlap such that the LiDAR impact on the state estimator is limited. But this affects mainly the accuracy of the state estimation. With respect to mapping, you should still be able to get geometrically correct submaps.

Can you be more specific on the issue? Is it only that you do not see an improvement when using the LiDAR or does OKVIS2-X actually fail to provide meaningful poses & maps?

[gfr-misha]

With the submapping factors enabled, the full BA never reaches convergence. The same dataset, but with submapping disabled (only visual inertial) the convergence is reached pretty fast, and the trajectory is more accurate. The lidar extrinsics are correct, as overlaying lidar scans onto the trajectory produces a cohesive map.
The FoV is around 90 Deg. The se8.yaml used is the same as Hilti 2022.

lidar:
  elevation_resolution_angle: 1.0 # only used for ray-based integration [degree]
  azimuth_resolution_angle:   0.18 # only used for ray-based integration [degree]
  # Extrinsics IMU {S} <-> LiDAR {L}
  T_SL: [ -1.0, 0.0, 0.0, -0.108,  
          0.0, -1.0, 0.0, -0.016,
          0.0, 0.0, 1.0, 0.060,
          0.0, 0.0, 0.0, 1.0]

I20260316 16:55:10.336089 1066321 ViGraph.cpp:1888] 
Solver Summary (v 2.2.0-eigen-(3.4.0)-lapack-suitesparse-(7.6.1)-metis-(5.1.0)-eigensparse-cuda-(12000))

                                     Original                  Reduced
Parameter blocks                        94231                    94230
Parameters                             402437                   402430
Effective parameters                   311392                   311388
Residual blocks                        845319                   845319
Residuals                             1580142                  1580142

Minimizer                        TRUST_REGION
Trust region strategy                  DOGLEG (TRADITIONAL)
Sparse linear algebra library    SUITE_SPARSE 

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                    12                       12
Linear solver ordering              AUTOMATIC                    94230

Cost:
Initial                          7.014124e+05
Final                            2.344474e+05
Change                           4.669650e+05

Minimizer iterations                      151
Successful steps                          125
Unsuccessful steps                         26

Time (in seconds):
Preprocessor                         0.795755

  Residual only evaluation           4.926943 (150)
  Jacobian & residual evaluation    13.476180 (125)
  Linear solver                   1593.671713 (125)
Minimizer                         1626.238881

Postprocessor                        0.046189
Total                             1627.080825

Termination:                   NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 150.)
Running optimisation again without priors & fixation (verbose=true)

[bochsim]

What do you mean by "the trajectory is more accurate"? Do you have any numbers (ATE or similar, which length of the trajectory) so that I get a feeling for your problem? 🙂 Given that your sensor is pointing only forward and depending on the motion pattern, I would not be surprised if the accuracy is not benefiting due to limited overlap of current observations with previous submaps. But it also should not be less accurate.

Regarding the BA - I double checked with some of our results on large sequences (given your outputs, it seems to be rather a large sequence), would also sometimes take a large number of iterations to actually converge, so you could try something really big for debugging (e.g. 1000 iterations). However, in our cases it did not have a major impact on the accuracy anymore.

Could you try that and provide some more details and specifics about the dataset and the accuracies?

[SebsBarbas]

@gfr-misha @bochsim hi, do you have any new updates on this matter. We are really looking forward to help our community, but with the info provided it is practically impossible. Let us know if we can close the ticket or there are remaining issues.

[gfr-misha]

Hi, sorry for the late reply to this. We can provide a dataset for you to look at, along with a patch to consume this dataset; however, we can't share it publicly. Would it be possible to send a link to it via email?

The dataset contains a drone flying through a set of obstacles, making a loop, and returning to the same location. By the "trajectory is more accurate", I mean that with submapping disabled, the trajectory closes the loop and produces a good final map when point clouds are overlayed on it. With it enabled, it can still close the loop, although at a later point, but the final point cloud looks less sharp - there are more double walls, and it struggles with bundle adjustment. It seems that local trajectory accuracy degrades while remaining somewhat consistent globally.

I understand that this is not a very scientific approach to comparing things, but we don't have a GT trajectory to compare against yet, so the best judgments we can make are based on the final point cloud map by transforming raw lidar readings to the poses found by okvis.

I would like us to work together on it if possible. This package is a good complement to our pipeline and we would like to keep testing it.