search

hyye / lio-mapping

Implementation of Tightly Coupled 3D Lidar Inertial Odometry and Mapping (LIO-mapping)
https://sites.google.com/view/lio-mapping
GNU General Public License v3.0
897 stars 320 forks source link

Problem about EstimateGyroBias() #44

Open getupgetup opened 4 years ago

getupgetup commented 4 years ago

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 2vec(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.

LiShuaixin commented 4 years ago

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

getupgetup commented 4 years ago

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 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 2vec(IMU_ij^T q_ij). However, I can't find q_bl in EstimateGyroBias(). Do you mean the code is wrong?

LiShuaixin commented 4 years ago

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

hyye commented 4 years ago

@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 commented 4 years ago

@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?

narutojxl commented 4 years ago

Hi @hyye,