How to ensure that dso and gps are fused at the same scale

[Tguangchao]

Hello, thank you for your open source work, I would like to ask you how to correctly integrate the dso trajectory with the gps trajectory, how to determine the scale of the dso system, is it through similar transformation, but I see that in your code is The fusion is done through linear Kalman filtering, so I would like to ask you how to ensure that the dso and gps tracks are fused at the same scale. Where is the specific implementation in your code, thank you

[GSORF]

Hello @Tguangchao,

Thank you for your interest and question. Sorry about my late response, I was on vacation.
The answer to your question is not simple, as there are many ways to tackle the problem with a proper scale estimation within sensor data fusion pipelines. Therefore, I will answer you by referring to my specific implementation. Keep in mind, that there are other, and many much better approaches while I chose to keep my fusion as simple and easy to understand as possible.

1. How to correctly integrate the dso trajectory with the gps trajectory? I my view, you need to distinguish between relative and absolute measurements. In my underlying master thesis you will see how I evaluate different fusion approaches, e.g. fusing GPS position and DSO position vs. GPS position and DSO velocity using a linear Kalman filter. The latter variant seemed to work better for my situations. You can take a look into my thesis document here: http://master.kalisz.co/MasterThesis_AdamKalisz_Online.pdf, especially chapter 5, page 60.
2. How to determine the scale of the dso system? Scale is always defined relative to some reference value. In order to determine the scale of DSO, you will need to have either a ground truth trajectory (as with my case from simulation in Blender) or a highly precise absolute measuring device (e.g. RTK GNSS). In my work, I am not modeling the scale specifically in my state space description but rather get it implicitly as a result of the sensor fusion algorithm.
3. Is it through similar transformation? I do not estimate a Sim(3) Lie Group within my fusion filter. However, I am investigating how pre-alignment of GPS and DSO will allow to use both sensors as absolute measurements. Compare Figure 5.2, page 62. Keep in mind that you might need to do some data preprocessing, such as sensor synchronization, or pay attention to motion patterns to avoid critical configurations (i.e. aligning the rotation of two linear trajectories). Therefore, I used Blender to generate my pre-aligned trajectories manually.
4. How to ensure that the dso and gps tracks are fused at the same scale? You need to decide what scale you want to use as final output, that is you need to mathematically formulate your state space. In my case, I use GPS as my final output coordinate system (it is even more general than GPS coordinates, because I am assuming that the geodetic coordinates (latitude, longitude, altitude) have been transformed to cartesian coordinates beforehand - thus my approach does not require a specific sensor setup and will also work with other sensors such as e.g. laserscanners or Wifi-Fingerprinting).
5. Where is the specific implementation in your code? You can find the complete code I have used to create the plots in my thesis and paper in the subdirectory 02_Utilities/FusionLinearKalmanFilter/01_LinearKalmanFilter_allEvaluations.blend in this repository. If you need assistance to use the code, feel free to ask. Unfortunately, I didn't have the time to make a (video) tutorial yet.

My answer above would be much more detailed if I wouldnt have a system crash while writing it in the first place. So, if you have any further questions, please feel free to ask. I love to discuss this topic, hence I am always open to any questions.

Kind regards,
Adam