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
898 stars 320 forks source link

Details on jacobian of point to plane factor #25

Closed andre-nguyen closed 4 years ago

andre-nguyen commented 4 years ago

Hi,

would it be possible to give the derivation for the jacobians of the point to plane factor? I am a bit confused on how to find the Jacobians from equations (3) and (4) and how the jacobian matrix is 7 x 3.

Thanks,

hyye commented 4 years ago

Because our transform is represented in the form of vector (3 elements) and quaternion (4 elements), and the residual is related to 1. frame Lp, 2. frame Lα, and 3. the extrinsic parameter.

andre-nguyen commented 4 years ago

@hyye I'm still trying to properly derive the analytical expressions I read "A primer on the Differential Calculus of 3D orientations" but I am a bit unclear on how to properly apply the chain rule and the product rule. Here is what I can understand:

image

I believe I have the correct expressions for the position derivatives:

image

But I have a hard time obtaining the same thing you have for the rotations

image

For (10) I am missing negative signs on the skew terms. For 11 I have R{B\alpha}^{W} inside the skew operator instead of outside. For (12) I have a similar problem where I am unsure how to apply the chain rule. I have tried using first principles and applying a disturbance onto the rotation however I still do not get the same results you do. Would it be possible to show your analytical derivation?

hyye commented 4 years ago

Hi @andre-nguyen, in this work, we use the update of rotation on the right side, which may cause the differences in (10) and (11). For (12), I believe you are very close to the final results by applying a disturbance to the rotation and omitting the high-order terms.

andre-nguyen commented 4 years ago

we use the update of rotation on the right side

Thanks, I understand now, I was adding my pertubation on the left but now that I add the pertubation on the right using image I get the same derivatives except for the last one image Which matches the code https://github.com/hyye/lio-mapping/blob/8485a4d9c359b7e486ad019a10815ecb8fd1257f/src/factor/PivotPointPlaneFactor.cc#L86-L88

I also get the same expression for the 2nd jacobian image

Which is equivalent to the code here if you distribute the minus sign into the cross operator. https://github.com/hyye/lio-mapping/blob/8485a4d9c359b7e486ad019a10815ecb8fd1257f/src/factor/PivotPointPlaneFactor.cc#L100-L102

However for the last jacobian wrt R_lidar_body I have an extra term at the end image

And in the code there is no -SkewSymmetric(tlb) term. https://github.com/hyye/lio-mapping/blob/8485a4d9c359b7e486ad019a10815ecb8fd1257f/src/factor/PivotPointPlaneFactor.cc#L125-L129

Did I make a mistake in the derivation or is there a mistake in the code? I ran the code with the extra term and I'm not sure I can see a difference.

hyye commented 4 years ago

I think that in Eqn. (6) of your derivation, R^L_B should time the first two items but not P^L_B.

andre-nguyen commented 4 years ago

I'm not sure why you say that but thank you for your time.

andre-nguyen commented 4 years ago

OK I double checked and you are right, it should not be multiplied. Thank you!

The correct residual term is thus image

and now all my equations match.