Regarding the issue of corresponding formulas between code and paper

[q87956256]

Thank you for your wonderful work!
I have some questions about the code:

1.In your paper, the formula 8:
R_G_L * R_Lk * (n_Lk + n),
where n_Lk means the normal vector of the ground points. But in your code (line 1289):
V3D meas_gp_ = R_LG_ * state.rot_end * e3;,
you directly use e3 instead. Can you tell me why?

2.In your paper, the formula 9:
e3 * R_G_L * p_k should be zero. But in your code (line 1298):
V3D meas_gp_last = R_LG_ * state.pos_end;
double meas_gp_last_result = e3.transpose() * meas_gp_last;
meas_vec(effect_feat_num + 2) = meas_gp_last_result - 1,0;(this is the line 1298)
I would like to know why you subtract 1,0. And what does the comma , mean? I think it might be a mistake in your code. Could you confirm?

[Taeyoung96]

@q87956256
Thanks for interesting our work!

1. I used e3 instead of a normal vector because lidars are usually mounted flat on the ground, which I realize is an excessive assumption. I will modify the code to fit the formula.

2. The comma is a typo, and the reason for extracting 1.0 is an engineering trick. As you said, by fomula 9, e3 * R_G_L * p_k should be 0. But since we defined V3D e3(0.0, 0.0, 1.0); (Line 1267) like this, we have a scale for 1.0. We needed to account for these components to make it zero, so we subtracted 1.0.

Thanks for taking a look at the formula and code. I hope you find the answers helpful.

[q87956256]

Thanks for your reply!

However, I still have some questions regarding the second point in your response. Below are the details:

1. About Formula (9):
   As I understand it, Formula (9) means the Z-component of the vector R_G_L * P_Lk should be zero, as illustrated below:
   
   From this perspective, I believe the result is independent of the scale factor. Even if V3D e3 is set as (0.0, 0.0, 10.0), the outcome of Formula (9) should still be zero.
   Is my understanding correct?
   If so, would it still be valid to omit the subtraction of 1.0?

2.Printed Results of meas_gp_last_result:
I added some print code to print the values involved. Below is a snapshot of my print output:

And the results:

As shown above:
The value of meas_gp_last_result is close to 0 before subtracting 1.0.
After the subtraction, it becomes approximately -1.0.

I’m having difficulty understanding the purpose of subtracting 1.0 in this context. Could you clarify:
What is the intention behind subtracting 1.0?
How does this relate to observing residuals?

Looking forward to your response!
Best regards,

[q87956256]

Sorry to bother you again, but I have another question about the iterative update process.
In the code:

It matches Formula (14) from the paper FAST-LIO2: Fast Direct LiDAR-Inertial Odometry:

From my understanding, the variable meas_vec in the code corresponds to -z in the paper. I checked it with the residual from the point-to-plane calculation:

However, in the subsequent code:

Shouldn't a negative sign be added to ensure consistency with the paper? Or am I missing something here?

Looking forward to your clarification!
Best regards,