prior factor jacobian

[narutojxl]

Hi @hyye ,

• Could you please tell me your reference material about jacobian of Lie Group? R_residual_prior = Log({Rm}^-1 * R), Rm is the measurement of Rlb, R is the variable Rlb to be optimized. J_R_residual_prior = I according to right purtubation formual, but i see in code is:

  if (jacobians[0]) { Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> > jacobian_prior(jacobians[0]); Eigen::Matrix<double, 6, 6> jaco_prior; jaco_prior.setIdentity();

    jaco_prior.bottomRightCorner<3, 3>() = LeftQuatMatrix(Q.inverse() * rot_).topLeftCorner<3, 3>();
  
    // FIXME: info
    jacobian_prior.setZero();
    jacobian_prior.leftCols<6>() = sqrt_info * jaco_prior;
    jacobian_prior.rightCols<1>().setZero();
  }

  I want to figure out how you get the expression, there are also many jacobians about SO(3) in imu and laser factor. Thanks for your help!

[hyye]

Hi @narutojxl, the prior factor is aimed at constraining the extrinsic parameters to the prior values. It is helpful when the translation is large and cannot be well-constrained under some motion patterns, e.g. planar motion.

[narutojxl]

Yes, it is for extrinsic between laser and body(imu). I mean how you calculate the jacobian of Log({Rm}^-1 * R) to R. I think the result should be identity matrix, but in code it is LeftQuatMatrix(Q.inverse() * rot_).topLeftCorner<3, 3>(), what's your reference material?

[hyye]

By applying quaternion multiplication with delta value and minus the original one, the Jacobian can be obtained. I found it might be a bug since it should be

jaco_prior.bottomRightCorner<3, 3>() = LeftQuatMatrix(rot_.inverse() * Q).topLeftCorner<3, 3>();

You can verify it by the Check function. https://github.com/hyye/lio-mapping/blob/8485a4d9c359b7e486ad019a10815ecb8fd1257f/src/factor/PriorFactor.cc#L69-L118

[narutojxl]

sorry, the jacobian of Log({Rm}^-1 * R) wrt R is not identity. My above result is not correct. It should be like this: I not sure whether we match the same result. I see your result isq_L(q.inverse() * qm ).topLeftCorner<3, 3>()

[hyye]

By applying quaternion multiplication with delta value and minus the original one, the Jacobian can be obtained. I found it might be a bug since it should be

jaco_prior.bottomRightCorner<3, 3>() = LeftQuatMatrix(rot_.inverse() * Q).topLeftCorner<3, 3>();

I think the final result is almost the same if the higher-order term is omitted. This can be verified by the Check function as I mentioned. I put forgot to put the Jacobian in my previous comment; now I corrected it.

[narutojxl]

Thank you very much :) I think my problem is solved, closed it now.