Absolute April Tag Localization with the FTC SDK

Transform3D.java

package org.firstinspires.ftc.teamcode.modules.vision;

import com.acmerobotics.roadrunner.geometry.Pose2d;

import org.apache.commons.math3.util.FastMath;
import org.firstinspires.ftc.robotcore.external.matrices.GeneralMatrixF;
import org.firstinspires.ftc.robotcore.external.matrices.MatrixF;
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
import org.firstinspires.ftc.robotcore.external.navigation.Rotation;

import kotlin.math.MathKt;

/**
 * Class for transformations in 3D Space. Only uses libraries in the FTC SDK,
 * with the optional addition of RoadRunner for ease of use with relocalization. 
 */
public class Transform3d {

    /**
     * Vector to describe a 3-dimensional position in space 
     */
    VectorF translation;
    /**
     * Quaternion to describe a 3-dimensional orientation in space
     */
    Quaternion rotation;

    private final static VectorF zeroVector = new VectorF(0,0,0);

    public Transform3d() {
        translation = new VectorF(0f,0f,0f);
        rotation = new Quaternion();
    }

    public Transform3d(VectorF t, Quaternion Q) {
        translation = t;
        rotation = Q;
    }

    public Transform3d unaryMinusInverse() {
        Quaternion Q = rotation.inverse();
        VectorF t = rotation.applyToVector(zeroVector.subtracted(translation));
        return new Transform3d(t,Q);
    }

    public Transform3d plus(Transform3d P) {
        VectorF t = translation.added(rotation.applyToVector(P.translation));
        Quaternion Q = rotation.multiply(P.rotation, System.nanoTime());
        return new Transform3d(t,Q);
    }

    public double getZ() {
        return Math.atan2(2.0*(rotation.y*rotation.z + rotation.w*rotation.x),
                rotation.w*rotation.w - rotation.x*rotation.x - rotation.y*rotation.y + rotation.z*rotation.z);
    }

    public static Quaternion MatrixToQuaternion(MatrixF m1) {
        float w = (float) (Math.sqrt(1.0 + m1.get(0,0) + m1.get(1,1) + m1.get(2,2)) / 2.0);
        float w4 = (float) (4.0 * w);
        float x = (m1.get(2,1) - m1.get(1,2)) / w4 ;
        float y = (m1.get(0,2) - m1.get(2,0)) / w4 ;
        float z = (m1.get(1,0) - m1.get(0,1)) / w4 ;
        return new Quaternion(w,x,y,z, System.nanoTime());
    }

    public Pose2d toPose2d() {
        return new Pose2d(
                translation.get(0),
                translation.get(1),
                this.getZ()
        );
    }
}

Vision.java

/***/
public ArrayList<Pose2d> getAprilTagPoses() {
    ArrayList<AprilTagDetection> tags = new ArrayList<>();

    if (visionPortal.getProcessorEnabled(tagProcessor)) {
        tags = tagProcessor.getDetections();
    }

    ArrayList<Pose2d> poses = new ArrayList<>();

    for (AprilTagDetection tag: tags) {
        if (tag.metadata != null) {

            // Get the tag absolute position on the field
            Transform3d tagPose = new Transform3d(
                    tag.metadata.fieldPosition,
                    tag.metadata.fieldOrientation
            );

            // Get the relative location of the tag from the camera
            Transform3d cameraToTagTransform = new Transform3d(
                    new VectorF(
                            (float) tag.rawPose.x,
                            (float) tag.rawPose.y,
                            (float) tag.rawPose.z
                    ),
                    Transform3d.MatrixToQuaternion(tag.rawPose.R)
            );

            // Inverse the previous transform to get the location of the camera from the tag
            Transform3d tagToCameraTransform = cameraToTagTransform.unaryMinusInverse();

            // Add the tag position and the relative position of the camera to the tag
            Transform3d cameraPose = tagPose.plus(tagToCameraTransform);

            // The the relative location of the camera to the robot
            //TODO: You have to tune this value for your camera
            Transform3d robotToCameraTransform = new Transform3d(
                    new VectorF(
                            -8.30f,
                             6.18f,
                            8.50f
                    ),
                    new Quaternion(0,0,1f,0, System.nanoTime())
            );

            // Inverse the previous transform again to get the location of the robot from the camera
            Transform3d cameraToRobotTransform = robotToCameraTransform.unaryMinusInverse();

            // Add the relative location of the robot to location of the Camera
            Transform3d robotPose = cameraPose.plus(cameraToRobotTransform);

            // Convert from a 3D transform to a 2D pose
            poses.add(robotPose.toPose2d());
        }
    }

    return poses;
}
/***/

Full code at our team repository