After testing localization on IRL data we found out it was totally broken, so this fixes it.
The main problems were that the code wrongly assumed where the camera was facing when the rotation given was the identity, and which axis the ground was on, and didn't account for the fact that the gimbal's measured pitch isn't relative to the drone's body frame, it's relative to the world frame.
Test Plan
Download data using link at the top of tests/imaging/localization_visualization.ipynb and put it in correct folder like this
Run the notebook and manually inspect output
run imaging_tests.sh and verify the tests pass, except for the two that have not been passing this whole year.
Summary
After testing localization on IRL data we found out it was totally broken, so this fixes it. The main problems were that the code wrongly assumed where the camera was facing when the rotation given was the identity, and which axis the ground was on, and didn't account for the fact that the gimbal's measured pitch isn't relative to the drone's body frame, it's relative to the world frame.
Test Plan
tests/imaging/localization_visualization.ipynband put it in correct folder like thisimaging_tests.shand verify the tests pass, except for the two that have not been passing this whole year.