First steps to locating the robot on the field using just apriltags and 2 cameras

[maxspier]

DO NOT MERGE
PR FOR FEEDBACK ONLY

** This localization code uses a camera to find an apriltag. This code tells the robot the coordinates of the apriltag. This code then uses the coordinates it gets from the apriltag in reference to the robot and compares it to where it knows that apriltag is on the field, allowing us to know where it is.

—not tested on robot yet

[dejabot]

similar high-level comments as with other PR
in java, the convention is to start with uppercase letters for classes, and lowercase letters for methods and for variables.

see https://www.oreilly.com/library/view/java-8-pocket/9781491901083/ch01.html for more about java naming conventions.

the reason to use them is it makes it easier for other developers to read and more immediately understand your code.

[dejabot]

similar high-level comments as with other PR
in java, the convention is to start with uppercase letters for classes, and lowercase letters for methods and for variables.

see https://www.oreilly.com/library/view/java-8-pocket/9781491901083/ch01.html for more about java naming conventions.

the reason to use them is it makes it easier for other developers to read and more immediately understand your code.

in addition, it would be helpful to provide some comments and/or documentation, to explain what it is that you're doing, what the classes are / what they're responsible for, and the overall flow/integration of those classes into the rest of the code.

if it's easier to talk through this sometime, happy to do that and with that context, dig into the logic and how that achieves the goals / make suggestions. I expect that some structural changes to separate out concerns and a few comments will go a long way for helping others read, understand, and provide feedback.

[wolffg]

Some thoughts on this draft.

[wolffg]

I'd recommend all these places where you have dynamic x,y pairs using a point class. You have some options; within 766, the logical choice is com.team766.odometry.Point, but there are other options, like:

https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/geometry/Translation2d.html

[maxspier]

Sure, but storing it in a local variable is good enough for now (early testing).

[wolffg]

Something like this might work with the final keyword in Java, which will guarantee the immutability of it.

final Point location;

FixedLocation(oldpoint) {
location = new Point(oldpoint)
}

Once it's final, you don't need to make copies in other contexts, as you know it's constant.

You could use this class to additionally contain a tagID or other descriptor, too (or a subclass).

[wolffg]

As elsewhere, I'd create an array of whatever point class you're using instead of an arraylist of doubles that represent x, y values alternating.

It's sort of [[x1,y1],[x2,y2]] vs. [x1,y1,x2,y2]

[maxspier]

Sure, but for early testing this still works.

[wolffg]

So, what you have here works, and it's totally fine, but to remind you of what we discussed in person:

You could have fewer ifs and case statements if you split the task into two steps:

1. Prep
2. Compute

In Prep, you can assemble a list of valid locations in an array, and then Compute you can iterate through that list.

[maxspier]

At the end, I will end up using a for each statement that asks for an apriltagLocalization variable. This will work for everything since apriltagLocalization is an interface.

[rcahoon]

You're on the right track here, but there's an implicit assumption in this code that's incorrect.

In order to do math on position coordinates, they have to be axis-aligned. That is, their X axes have to be pointed in the same direction and their Y axes have to be pointed in the same direction. This is because adding or subtracting a position is representative of moving a certain distance along the X and Y axes. If the positions involved in the computation don't agree on what it means to move along the X axis (i.e. which direction of movement that represents), then the result is meaningless.

In this case, the X/Y axes of the ScoringAreaApriltag, as well as the axes of the robot location that you're trying to find, are defined by the playing field. However, the axes of the observed tag position are defined by the direction that the robot is pointing. So if the robot is rotated, then the axes will not be aligned.

You have a few options: 1. incorporate the orientation of the april tag as observed by the camera (e.g. camera1rel.getRotation().getZ()) 2. use the observed positions of two different april tags, which gives you enough information to estimate both the position and rotation of the robot using only the position of the april tags 3. use the robot's gyro (or, more generally, the Odometry code) to give you the orientation of the robot.

Option (3) will give you the most precise estimate of the robot's orientation, but the accuracy will decrease over time since errors in the gyro's measurement of the robot's rotation accumulate. Options (1) and (2) will both give you pretty low accuracy estimates, but the accuracy won't be dependent on time. The relative accuracy of (1) and (2) will vary depending on the position of the robot relative to the april tags and how well it can see them, but I would expect that Option (2) would give better estimates on average (however, it requires two april tags to be visible). The best solution would to do sensor fusion with all three types measurements, but there's a fair amount of complexity to that.

[jamesi8086]

For option 2, we need to assume that tags may or may not be visible at every time step.
For each time step which a tag is missing, we need to derate and consider gyro data for short term assumptions until more (at least 2) tags are available later.

[1yd1a]

Please put spaces between the brackets and "else" here and everywhere else.

[jamesi8086]

Linting needs to be done before each commit. It would be easier if linting is configured on the IDE of the laptop you're using. That assumes you're always using the same laptop though.