lio-mapping
lio-mapping copied to clipboard
Problem about EstimateGyroBias()
Hi, I noticed that parameter bg was first initialized when extrinsic calibration done. However, I still can't understand the theory behind the code in function EstimateGyroBias(). It seems that the residual to be optimized is 2*vec(IMU_ij^T * q_ij) (noted in EstimateGyroBias()). In my opinion, however, IMU_ij do not straightly equal q_ij because of extrinsic rotation R. Why does the extrinsic rotation R disappear in this part of code.
I think tmp_A
is corresponding to 1/2 * Jaccobian
in equation(15) (refer to VINS-MONO paper), that's why factor 2 should be multiplied.
BUT I think it's q_ij_imu = q_bl * q_ij * q_bl.conj()
in imu frame instead of q_ij
in laser frame) should be used in the cost function
I think
tmp_A
is corresponding to1/2 * Jaccobian
in equation(15) (refer to VINS-MONO paper), that's why factor 2 should be multiplied.BUT I think it's
q_ij_imu = q_bl * q_ij * q_bl.conj()
in imu frame instead ofq_ij
in laser frame) should be used in the cost function
I think factor 2 is used to convert quaternion to rotation vector. Quaternion (qw, qv) can be expressed by : qw = cos(theta/2) , qv = v * sin(theta/2). When theta is small, sin(theta/2) ~ theta/2, then qv = v * theta / 2. rotation vector = v * theta = qv * 2. That's why the residual to be optimized is 2*vec(IMU_ij^T * q_ij). However, I can't find q_bl in EstimateGyroBias(). Do you mean the code is wrong?
Yes, that is the theoretical reason for the constant factor in the cost function. It can also be derived directly using eq(15) as I said before.
The code should be modified following the derivation of the first order approximation of bias, but maybe there are some other considerations for the author to omit the rotation estimation of extrinsic parameters. @hyye
@getupgetup and @LiShuaixin thanks for pointing out. Missing q_bl could be a potential bug in the code (not sure). I will investigate it later.
@getupgetup and @LiShuaixin thanks for pointing out. Missing q_bl could be a potential bug in the code (not sure). I will investigate it later.
Thanks, Does VINS use the same code in this part?
Hi @hyye,
- Is there a conclusion to this matter?
- Besides the above discussion about
q_ij_imu = q_bl * q_ij * q_bl.conj()
, i found that if we suppose theq_ij
is correct. The jacobian of residual wrt bg in code isr = q_ij.conj() * delta_q
, notr = delta_q.conj() * delta_q
. Is there some other consideration ? Thanks a lot !