"Resetting" odometry with limelight?

Technical Programming
,
#1

Hey guys! I noticed while watching behind the bumpers today that quite a few teams used their limelights to “reset” their odometry to their position on the field using the limelight to account for drift. How is this done? I’ve been trying to figure out how a limelight can be used to correct for drift in odometry and I can’t think of anything that’d work too well.

I have a couple small things in my head but they aren’t visualized too well yet so I don’t think I can explain them well enough to not confuse anyone.

2 Likes
#2

Kalman filters and pose estimation is probably the technique you’ll wanna be looking into.

Photonvision does have an example from 2021/2 for it. Specifically, this line is where the camera information is incorporated into the current on-field pose estimate. There is a description of what the example does, though you’ll have to excuse the incorrect code lines referenced in the blocks… they’re still in need of fixup.

1 Like
#3

I recommend reading WPILib’s pose estimator docs.

1 Like
#4

Thanks! That definitely clarifies things up and as a bonus it talks about PnP and links to a Wikipedia page explaining how it works which is nice :stuck_out_tongue:

1 Like
#5

Interesting, I did not know these post estimator’s existed. This would actually help improve upon what we used this year. (Below are code excerpts from this year)

    if(currentTime > 0.250 && Limelight.valid()){
        double dL = Limelight.getDistance()*0.0254;
        double tR = m_drive.getGyro().getRadians();
        double tT = m_turret.getMeasurement()-Math.PI;
        double tL = -1.0*Limelight.tx();

        Pose2d pose = calcPoseFromVision(dL, tR, tT, tL, GoalConstants.kGoalLocation);

        if(m_updatePose){
            m_drive.setPose(pose);
     }

private Pose2d calcPoseFromVision(double dL, double tR, double tT, double tL, Translation2d goal){
            double tG = tR+tT+tL;
            double rX = goal.getX()-dL*Math.cos(tG);
            double rY = goal.getY()-dL*Math.sin(tG);
        
            return new Pose2d(rX,rY, new Rotation2d(-tR));
}
3 Likes