On the Form of Jacobians

[GGYB]

Hello, thank you for your contribution to the community with your open-source code. I still have some minor doubts about the computation of the Jacobian matrix in GICPFactor. Shouldn't the derivative form for the translation be -I? However, in your code, this term is -R. Could you please explain why this is the case?

[Atticuszz]

wondering as well，and have u change it as I to see what's difference on accuracy?

[GGYB]

wondering as well，and have u change it as I to see what's difference on accuracy?

Sorry, I haven't tested this result, but I assume that if the Jacobian matrix uses the residual terms in the code as independent variables, then it should be an identity matrix for the translation terms. This is consistent with the form in fast_gicp.

[Atticuszz]

wondering as well，and have u change it as I to see what's difference on accuracy?

Sorry, I haven't tested this result, but I assume that if the Jacobian matrix uses the residual terms in the code as independent variables, then it should be an identity matrix for the translation terms. This is consistent with the form in fast_gicp.

i think it should be an identity matrix as well

[Atticuszz]

i test it as identify，did not decrease accuracy on my dataset

[koide3]

The difference stems from the SE3 retraction. fast_gicp uses left multiplying new_pose = exp(delta) * old_pose while small_gicp uses right multiplying new_pose = old_pose * exp(delta). We can see that the same Jacobian formula is used in GTSAM which is using the right multiplying.

To my understanding, they are almost equivalent theoretically, but in practice, the left multiplying tends to move points largely and show a bit worse convergence compared to the right multiplying. I'm not 100% sure though. Lie algebra is mysterious sometimes.

[koide3]

It can also be validated with the following simple numerical diff.

#!/usr/bin/python3
import numpy
import gtsam

def main():
  numpy.set_printoptions(precision=3, suppress=True)

  pose = gtsam.Pose3.Expmap(numpy.random.rand(6))
  pt = gtsam.Point3(numpy.random.rand(3))

  print('--- pose ---')
  print(pose.matrix())
  print('--- pt ---')
  print(pt)

  eps = 1e-6
  J = numpy.zeros((3, 6))

  pt0 = pose.transformFrom(pt)
  for i in range(6):
    delta = numpy.zeros(6)
    delta[i] = eps

    pti = pose.compose(gtsam.Pose3.Expmap(delta)).transformFrom(pt) # right-multiply
    # pti = gtsam.Pose3.Expmap(delta).compose(pose).transformFrom(pt) # left-multiply
    J[:, i] = (pti - pt0) / eps

  print('--- J ---')
  print(J)

if __name__ == '__main__':
  main()

--- pose ---
[[ 0.897  0.012  0.441  0.811]
 [ 0.033  0.995 -0.094  0.864]
 [-0.44   0.099  0.893  0.366]
 [ 0.     0.     0.     1.   ]]
--- pt ---
[0.105 0.511 0.487]
--- J ---
[[ 0.22   0.39  -0.457  0.897  0.012  0.441]
 [-0.532  0.026  0.088  0.033  0.995 -0.094]
 [ 0.408 -0.308  0.235 -0.44   0.099  0.893]]

[GGYB]

The difference stems from the SE3 retraction. fast_gicp uses left multiplying new_pose = exp(delta) * old_pose while small_gicp uses right multiplying new_pose = old_pose * exp(delta). We can see that the same Jacobian formula is used in GTSAM which is using the right multiplying.

To my understanding, they are almost equivalent theoretically, but in practice, the left multiplying tends to move points largely and show a bit worse convergence compared to the right multiplying. I'm not 100% sure though. Lie algebra is mysterious sometimes.

Thank you very much for your patience and professional explanation.

[GGYB]

i test it as identify，did not decrease accuracy on my dataset

Thank you for conducting the test, and please also take a look at 'koide3's' explanation of this.

[GGYB]

i test it as identify，did not decrease accuracy on my dataset

I think I may have found the reason for the previous error in my derivation. I'm not sure if this will help you, but when calculating the Jacobian, I mistakenly treated the 'pose' term in the 'error' as 'deltaPose' (error state). Therefore, the direct derivative of this part for translation is the identity matrix. However, if we consider the 'pose' in the 'error' as a large value (normal state) , then after applying the right multiplication disturbance model, it becomes -R. This depends on how the problem is constructed.

[Atticuszz]

Thank you for your detailed analysis and the patient explanation. As a beginner in this area, your discussion has been incredibly enlightening for me.

From what I understand, you're suggesting that if we directly apply a perturbation model from the Lie group to ( T ) and then differentiate, the result for the translation part becomes (-R), depending on how we construct the problem. Please correct me if I misunderstood, but this perspective seems to shift significantly from my initial approach.

Previously, I considered the rotation ( \Theta ) and translation ( t ) separately. I derived the derivatives for the rotation by considering a small perturbation represented as ( R \times [\delta\Theta]\times ), and for ( \delta q = R \times [\delta\Theta]\times \times q ). Utilizing the properties of skew-symmetric matrices, this changes to ( \delta q = R \times [\delta\Theta \times q]_\times ), leading to the partial derivatives being the skew-symmetric matrix of ( R \times q ) for rotations. For the translations, direct differentiation led to the identity matrix. I suspected that using the identity matrix directly might not significantly impact the accuracy due to the mathematical differences in how the Jacobian was constructed, but perhaps my derivation wasn’t comprehensive enough.

I have also uploaded my own Markdown file detailing the derivation of the Jacobian matrix for your reference. Hopefully, it might offer some insights or useful comparisons to your method.maybe obsidian app can properly recognize the formula.

Lately, I've been exploring using PyTorch's automatic differentiation to handle the specifics of the Jacobian matrix in GICP research. This approach could potentially simplify some of the complexities involved in manually calculating these derivatives.

Thanks again for your input; it has certainly provided a lot of food for thought and could guide my further experiments.