Allowing External Angle Sources in Pose Estimation #1309
Conversation
…oTargetMeters method in PhotonUtils
|
How does this perform when the robot is rotating? A major problem with MegaTag2 is that you have no idea when the gyro reading is from without the timestamp, leading to inaccuracies. This is fine when you're sitting in place, but once you're moving, things start getting dicey. |
|
Unfortunately I don't have much information on that at the moment, as so far all of my testing has been done in simulation and for some reason I can't rotate the robot in teleop, as there is something broken in my simulation at the moment. I'll look into compensating for latency though. |
|
The other thing to note is that this still only uses a single tag. How does using the multitag pose but with the robot yaw perform? |
|
Suggest a refactor here to support an InterpretableBuffer of Rotation angles which would solve timestamp issues and also allow this to be used with multitag better with timestamp synchronization across measurements. I'd prefer to internalize this though as the PhotonPoseEstimator has grown a lot with different strategies which can lead to it being confusing. |
I definitely agree that moving the register function to a different class would be more viable, or else it would become tedious to work with if multiple PoseEstimators were used in addition to adding unnecessary options. Would PhotonUtils be an acceptable place to store the heading register? I plan on pushing the commits I've made over the past month this afternoon or evening. |
Last night I finally did a side-by-side comparison of the estimated pose's position before and after latency compensation was applied, and the results are quite substantial. At the test location I was using between the speaker and the amp in simulation, without latency compensation, the estimated pose would get flung out as far as the opposite side of the stage, but post-application at the same speeds, the farthest point the estimation was thrown by was no more than 2-2.5 feet or so, and that was more from the fact that it was detecting tags halfway across the field, and so the error spikes significantly at that point. It appeared to also hold up similarly when the simulated robot was rotating at 5 radians per second as opposed to the 2 radians per seconds the first two tests were performed with. I intend to upload the screen recordings I collected of the advantagescope visualization later this afternoon. |
I haven't tested what using the robot's yaw in a multitag solve looks like yet, but I did develop a solver algorithm that only uses two tags to calculate the robot's position without needing the robot's heading. It sidesteps the potential reflected ambiguity represented by only using two tags because it uses the leftmost tag leftmost in perspective of the best two tags as the anchor tag. This forces a singular perspective out of the system by calculating the angle between the anchor tag and the secondary tag and then rotating the offset translation from the anchor tag derived using trigonometry by that angle, the field-relative offset from the anchor tag is obtained and then applied to the field-relative position of the anchor tag from the tag layout, resulting in the robot's position on the field. Both the one tag and two tag solvers achieve greater stability by calculating the offset from the tag purely from its center and the camera's height and angular offsets rather than the SolvePNP transform from the tag's corners. |
|
No latency compensation, 2 radians per second |
|
This is what it looks like when it's stationary:
|
This pull request adds the IMU_SOLVE_BEST_TAG PoseStrategy. It takes the target returned by the getBestTarget method of PhotonPipelineResult and solves solely from that tag. When tested in simulation, the regular multitag solve could be off by as much as half a meter or more and about +/- 0.1m of jitter, but IMU_SOLVE_BEST_TAG maintained both high accuracy and low levels of noise: about +/- 0.01-0.02. (This is not accurate, I pulled those figures from memory and as such they are an approximation)
#746