Derivation of jacobian formula about ssl_slam3 HOT 5 OPEN

Here is my formula derivation proces about jacobian_i.block<3, 3>(0, 0) and and jacobian_j.block<3, 3>(0, 0),Please point out if there is any error.

jacobian_i.block<3, 3>(0, 12) :I haven't been able to derive the formula yet!

from ssl_slam3.

jacobian_i.block<3, 3>(0, 12)

from ssl_slam3.

Hi @chengwei0427
Thanks for posting questions here.

for the derivation of lie algebra, I would like to recommend this website, which has all SLAM maths included (note that there are some typo mistakes)
http://www.mathsword.com/category/slam/

In summary, you question is how to derive the formula for log(R1 * R * R2)/x, where Exp(x)=R, let delta_x be a small perturbation,
thus derivate of log(R1 * R * R2)/x is {log(R1 * R * Exp(delta_x) * R2) - log(R1 * R * R2)}/delta_x

we know that for any rotation matrix R0, we have R0 * Exp(delta_x) * R0' = Exp(R0 * delta_x)
Hence, log(R1 * R * Exp(delta_x) * R2) = log(R1 * R * R2 * Exp(R2' * delta_x))
we know that when delta_x is a small number, log(Exp(x) * Exp(delta_x)) ~= x + Jr^-1(x) * delta_x
Thus we have
{log(R1 * R * Exp(delta_x) * R2) - log(R1 * R * R2)} /delta_x
= {log(R1 * R * R2 * Exp(R2' * delta_x)) - log(R1 * R * R2) }/delta_x
= {log(R1 * R * R2) + Jr^-1(R1R R2) *delta_x - log(R1 * R * R2) }/delta_x
= Jr^-1(R1 * R * R2)

from ssl_slam3.

Hi, @wh200720041
Thanks for your patient reply.

I have another question about the IMU propagate process in imuPreintegrationClass.cpp.

As the comment in the propagate function, the q p v can compute as follows:

 // q = q * Exp{(gyr-bg)*t}   
// p = p + v * t + 0.25 * q * acc0 * t^2 + 0.25 * q * Exp{(gyr-bg)*t} * acc1 * t^2
 // v = v + 0.5 * q * acc0 * t + 0.5 * q * Exp{(gyr-bg)*t} * acc1 * t

But I cannot unstand how to derivation the F and V, it's inconsistent with the derivation in vins-mono or fast-lio? Can you give me some hints (related papers, theories)?

from ssl_slam3.

Hi, @wh200720041

I cannot unstand the derivation F and V;

The bellow picture is the Jacobi in 简明ESKF

The jacobi about theta to delta_theta is exp(-(w_m-b_g)dt), and in your code, the jacobi is F.block<3, 3>(0, 0) = A_inv * (Eigen::Matrix3d::Identity() - 0.5 * gyr0_skew * dt);, I am confused how to derivate it.

One way to deal with it is :

exp(-(gyr0+gyr1)/2*dt)
=exp(-0.5*gyr0*dt+(-0.5*gry1*dt))
= exp(-0.5*gyr0)exp(J_r(exp(-0.5*gyr0*dt))(-0.5*gyr1))   -- BCH
=exp(0.5*gyr1*dt)^(-1)exp(-0.5*gyr0*dt)  --- J_r(exp(-0.5*gyr0*dt))=I , if exp(-0.5*gyr0*dt)--->0

But I donot know if it is right?

Can you give me some suggest?

from ssl_slam3.