-
Notifications
You must be signed in to change notification settings - Fork 49
Fix covariance of pose inverse. #84
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Conversation
The covariance propagation of pose inverse via adjoint introduced in #2598 followed the lamar convention, which was just found to be actually broken here: microsoft/lamar-benchmark#84. This PR fixes the propagation and added more cycle-consistent tests on the property of the adjoint matrix. To simplify the code for calculating adjoint inverse the function ``CrossProductMatrix`` is moved into ``rigid3.h``.
I wrote this code precisely based on the page that you link, which clearly states that, if
This is implemented according to Eq. 15 of the same link. There is no Jacobian (I don't know that you are referring to) and we assume that the poses are not correlated. |
In Eq. (6) section the noise is defined in the body frame, so T_{WB} is exactly the pose inverse. And if we do some basic derivation, the derivative of -R^Tt with respect to t is -R^T rather than R. Similarly, the Jacobian of the pose composition is not like what you wrote here, but is out of scope for this PR. In Eq. (15) the convention of covariance is also different from your assumption. |
|
The covariance propagation via adjoint is valid because that you could write first-order derivatives (Jacobian) using adjoint. I don't understand what you meant by "there is no Jacobian". |
|
Edit: this PR proposal is also wrong. The expression should be in other form. In the end the adjoint should not be relevant here due to the discrepancy between SE(3) and SO(3) x R^3. Will close this for now and open another one soon to fix it. |
The Jacobian of the inverse operation corresponds to the adjoint of the pose inverse rather than the adjoint of the pose. This is also discussed here: https://gtsam.org/2021/02/23/uncertainties-part3.html
In addition (out of the scope of this PR), the covariance propagation of the composited pose (here: https://github.com/microsoft/lamar-benchmark/blob/main/scantools/capture/pose.py#L133) also does not feel correct to me (the Jacobians are different in my derivation). And the cross correlation, albeit being very important to covariance propagation at pose composition, is dropped in the computation.