Derivation of jacobian formula

[chengwei0427]

Hi, @wh200720041

Thanks for your great work.

I have a question about the derivation of jacobian formula in imuOptimizationFactor.cpp.

The residual function is:
`

// residual 1 = log[delta_r' * Ri' * Rj]
// residual 2 = Ri'* (0.5 * g *dt2 + Pj - Pi -Vi * dt) - delta_p
// residual 3 = Ri'* (g * dt + Vj - Vi) - delta_v
// residual 4 = b_aj - b_ai
// residual 5 = b_gj - b_gi

`

the optimized variable is r p v ba bg

I'm confused about how do you derive the formula
`

       jacobian_i.block<3, 3>(0, 0) = - Utils::Jr_so3_inv(rot_residual) * Rj.transpose() * Ri;
        jacobian_i.block<3, 3>(0, 12) = - Utils::Jr_so3_inv(rot_residual) * Rj.transpose() * Ri * corrected_delta_R * dr_dbg;

`

and jacobian_j.block<3, 3>(0, 0) = Utils::Jr_so3_inv(rot_residual);

Can you give me some help?

[chengwei0427]

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!

[chengwei0427]

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

[wh200720041]

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)

[chengwei0427]

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

[chengwei0427]

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?