DMSA params

[chengwei0427]

Hi, @davidskdds Forgive me for bothering you again. In the test, there are some doubts

1. I'm confused about num_clouds_submap and numControlPoints ; That's how I understand it. Please point out any mistakes: In the program, the data of num_clouds_submap frames are processed at a time, and the trajectory of these data is controlled by numControlPoints? Here, numControlPoints are used to control this continuous trajectory,and the pose of each laser point is interpolated by this numControlPoints control point?
2. How do you understand the Jacobi calculation here Jacobian.col(k) = one_div_incr * (errorVec - error0);

Waiting for your reply. Thanks.

[davidskdds]

Hello @chengwei0427 ,

no problem at all, I am pleased about the interest.

1. Yes, you got it right. In the default settings 5 point clouds (num_clouds_submap) are processed at one time. The continuous trajectory, whose poses are used to transform the points into the world frame, is defined by 6 control poses (numControlPoints).

Using the control poses, spline interpolation (translation) and slerp (orientation) is used to generate a "continuous trajectory" with a temporal resolution of 0.1 ms, which is then used to transform the points.

With the current settings, there is a single start pose and a control pose for each point cloud (full rotation of the LiDAR). To map higher frequency trajectories, the number of control poses can also be increased. In my experience, however, the robustness decreases with too many control poses.

2. In the line you mention the Jacobian is calculated numerically. The "algorithm" behind it is just the difference quotient. "one_div_incr" corresponds to 1 / h, "errorVec" corresponds to f(x + h) and "error0" corresponds to f(x).

I hope the answers help you. If you have any further questions, let me know.

[chengwei0427]

Thank you for your patience. There may be other questions that need to be consulted, so I will keep this issue open for the time being.

[chengwei0427]

Hi， @davidskdds I have a question about optimizeSet:

1. static points are added to ensure continuous trajectory in Sliding Window DMSA Continuous Trajectory Optimization,
2. but there seems to be no constraint to ensure that the trajectory of keyframe optimization is continuous in DMSA Keyframe Optimization, I mean, how does the optimized keyframe pose in the sliding window remain continuous with the pose outside the sliding window.

[davidskdds]

Hello @chengwei0427 ,

thank you for your interest.

I hope I have understood your question correctly. I will expand a little. In the "Sliding Window Continuous Trajectory" optimization, continuity is ensured by the fact that the first control pose of the respective time horizon is not changed during the optimization. For example, if we use 6 control poses for 5 point clouds to be optimized, only 5 of the control poses will be optimized. The first one is always initialized by the poses of the previous time step and is not changed. In addition, the static points also help to maintain continuity.

Let's assume that the current map has 10 keyframes (id 0 to id 9). The keyframe id 9 was added in this time step, which triggers a keyframe optimization from keyframe id 5 to id 9 by "DMSA Keyframe Optimization". In order to maintain continuity with the future "Sliding Window Continuous Trajectory", the first control pose of the past continuous trajectory (whose point clouds became keyframe id 9) is replaced with the new pose of keyframe id 9. This is done in line 231 and 232 in include/DMSA/DmsaSlam.h. The subsequent control poses of the last sliding window continuous trajectory are then updated by the function "relative2global();". In the next time step, these updated control poses are then used to initialize the "new" continuous trajectory.

This update ensures that consistency is maintained after keyframe optimizations.

In addition, all non-keyframe poses that lie in the period between keyframe id 5 and id 9 must also be updated. This is ensured by the fact that non-keyframe poses are not saved in relation to the map frame but relative to the keyframe with the most overlap together with its keyframe id.

This functionality is stored in the "include/DMSA/OutputManagement.h" class. Non-keyframes are updated there using the function "addNonKeyframePose".

I hope this has answered your question. If you have any further questions, please feel free to ask.