Examples with VectorF org.firstinspires.ftc.robotcore.external.matrices.VectorF used on opensource projects

Example 1 with VectorF

use of org.firstinspires.ftc.robotcore.external.matrices.VectorF in project robotcode by OutoftheBoxFTC.

the class Orientation method getOrientation.

/**
 * Given a rotation matrix, and an {@link AxesReference} and {@link AxesOrder}, returns an orientation
 * that would produce that rotation matrix.
 *
 * @param rot           the matrix whose orientation is to be determined
 * @param axesReference whether wish an extrinsic or intrinsic reference for the axes
 * @param axesOrder     the order in which the axes are to be rotated
 * @param unit          the angle units in which the orientation is to be returned
 * @return an orientation that will produce the given rotation matrix
 * @see Orientation
 * @see #getOrientation(MatrixF, AxesReference, AxesOrder, AngleUnit, AngleSet)
 * @see <a href="https://en.wikipedia.org/wiki/Rotation_matrix">Rotation Matrix</a>
 */
public static Orientation getOrientation(MatrixF rot, AxesReference axesReference, AxesOrder axesOrder, AngleUnit unit) {
    /**
     * Run both choices and return the one that uses smaller sets of angles. This is just a heuristic
     * to choose which angle set is the most aesthetically pleasing. Both angle sets are equally <em>valid</em>.
     */
    Orientation one = getOrientation(rot, axesReference, axesOrder, unit, AngleSet.THEONE);
    Orientation theOther = getOrientation(rot, axesReference, axesOrder, unit, AngleSet.THEOTHER);
    VectorF vOne = new VectorF(one.firstAngle, one.secondAngle, one.thirdAngle);
    VectorF vOther = new VectorF(theOther.firstAngle, theOther.secondAngle, theOther.thirdAngle);
    return vOne.magnitude() <= vOther.magnitude() ? one : theOther;
}

Example 2 with VectorF

use of org.firstinspires.ftc.robotcore.external.matrices.VectorF in project robotcode by OutoftheBoxFTC.

the class ImageTransformProvider method run.

/**
 * Reads and records the pictograph type, relative distance, and relative rotation from the image
 * As well as filtering it through a data filer
 */
@Override
public void run() {
    VectorF translation;
    Orientation rotation;
    while (running) {
        if (!RelicRecoveryVuMark.from(template).equals(RelicRecoveryVuMark.UNKNOWN)) {
            OpenGLMatrix transform = ((VuforiaTrackableDefaultListener) template.getListener()).getPose();
            translation = transform.getTranslation();
            rotation = Orientation.getOrientation(transform, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
            xTrans.update(translation.get(0));
            yTrans.update(translation.get(1));
            zTrans.update(translation.get(2));
            xRot.update(rotation.firstAngle);
            yRot.update(rotation.secondAngle);
            zRot.update(rotation.thirdAngle);
            imageSeen = true;
            System.out.println(translation.get(0));
        } else
            imageSeen = false;
        try {
            Thread.sleep(UPDATE_INTERVAL);
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    }
}

Example 3 with VectorF

use of org.firstinspires.ftc.robotcore.external.matrices.VectorF in project FTC-2017 by FIRST-4030.

the class ConceptVuMarkIdentification method runOpMode.

@Override
public void runOpMode() {
    /*
         * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone);
         * If no camera monitor is desired, use the parameterless constructor instead (commented out below).
         */
    int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
    VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
    // OR...  Do Not Activate the Camera Monitor View, to save power
    // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
    /*
         * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
         * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
         * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
         * web site at https://developer.vuforia.com/license-manager.
         *
         * Vuforia license keys are always 380 characters long, and look as if they contain mostly
         * random data. As an example, here is a example of a fragment of a valid key:
         *      ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
         * Once you've obtained a license key, copy the string from the Vuforia web site
         * and paste it in to your code onthe next line, between the double quotes.
         */
    parameters.vuforiaLicenseKey = "ATsODcD/////AAAAAVw2lR...d45oGpdljdOh5LuFB9nDNfckoxb8COxKSFX";
    /*
         * We also indicate which camera on the RC that we wish to use.
         * Here we chose the back (HiRes) camera (for greater range), but
         * for a competition robot, the front camera might be more convenient.
         */
    parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
    this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);
    /**
     * Load the data set containing the VuMarks for Relic Recovery. There's only one trackable
     * in this data set: all three of the VuMarks in the game were created from this one template,
     * but differ in their instance id information.
     * @see VuMarkInstanceId
     */
    VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
    VuforiaTrackable relicTemplate = relicTrackables.get(0);
    // can help in debugging; otherwise not necessary
    relicTemplate.setName("relicVuMarkTemplate");
    telemetry.addData(">", "Press Play to start");
    telemetry.update();
    waitForStart();
    relicTrackables.activate();
    while (opModeIsActive()) {
        /**
         * See if any of the instances of {@link relicTemplate} are currently visible.
         * {@link RelicRecoveryVuMark} is an enum which can have the following values:
         * UNKNOWN, LEFT, CENTER, and RIGHT. When a VuMark is visible, something other than
         * UNKNOWN will be returned by {@link RelicRecoveryVuMark#from(VuforiaTrackable)}.
         */
        RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
        if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
            /* Found an instance of the template. In the actual game, you will probably
                 * loop until this condition occurs, then move on to act accordingly depending
                 * on which VuMark was visible. */
            telemetry.addData("VuMark", "%s visible", vuMark);
            /* For fun, we also exhibit the navigational pose. In the Relic Recovery game,
                 * it is perhaps unlikely that you will actually need to act on this pose information, but
                 * we illustrate it nevertheless, for completeness. */
            OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) relicTemplate.getListener()).getPose();
            telemetry.addData("Pose", format(pose));
            /* We further illustrate how to decompose the pose into useful rotational and
                 * translational components */
            if (pose != null) {
                VectorF trans = pose.getTranslation();
                Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
                // Extract the X, Y, and Z components of the offset of the target relative to the robot
                double tX = trans.get(0);
                double tY = trans.get(1);
                double tZ = trans.get(2);
                // Extract the rotational components of the target relative to the robot
                double rX = rot.firstAngle;
                double rY = rot.secondAngle;
                double rZ = rot.thirdAngle;
            }
        } else {
            telemetry.addData("VuMark", "not visible");
        }
        telemetry.update();
    }
}

Example 4 with VectorF

use of org.firstinspires.ftc.robotcore.external.matrices.VectorF in project robotcode by OutoftheBoxFTC.

the class ConceptVuMarkIdentification method runOpMode.

@Override
public void runOpMode() {
    /*
         * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone);
         * If no camera monitor is desired, use the parameterless constructor instead (commented out below).
         */
    int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
    VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
    // OR...  Do Not Activate the Camera Monitor View, to save power
    // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
    /*
         * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
         * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
         * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
         * web site at https://developer.vuforia.com/license-manager.
         *
         * Vuforia license keys are always 380 characters long, and look as if they contain mostly
         * random data. As an example, here is a example of a fragment of a valid key:
         *      ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
         * Once you've obtained a license key, copy the string from the Vuforia web site
         * and paste it in to your code onthe next line, between the double quotes.
         */
    parameters.vuforiaLicenseKey = "ATsODcD/////AAAAAVw2lR...d45oGpdljdOh5LuFB9nDNfckoxb8COxKSFX";
    /*
         * We also indicate which camera on the RC that we wish to use.
         * Here we chose the back (HiRes) camera (for greater range), but
         * for a competition robot, the front camera might be more convenient.
         */
    parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
    this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);
    /**
     * Load the data set containing the VuMarks for Relic Recovery. There's only one trackable
     * in this data set: all three of the VuMarks in the game were created from this one template,
     * but differ in their instance id information.
     * @see VuMarkInstanceId
     */
    VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
    VuforiaTrackable relicTemplate = relicTrackables.get(0);
    // can help in debugging; otherwise not necessary
    relicTemplate.setName("relicVuMarkTemplate");
    telemetry.addData(">", "Press Play to start");
    telemetry.update();
    waitForStart();
    relicTrackables.activate();
    while (opModeIsActive()) {
        /**
         * See if any of the instances of {@link relicTemplate} are currently visible.
         * {@link RelicRecoveryVuMark} is an enum which can have the following values:
         * UNKNOWN, LEFT, CENTER, and RIGHT. When a VuMark is visible, something other than
         * UNKNOWN will be returned by {@link RelicRecoveryVuMark#from(VuforiaTrackable)}.
         */
        RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
        if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
            /* Found an instance of the template. In the actual game, you will probably
                 * loop until this condition occurs, then move on to act accordingly depending
                 * on which VuMark was visible. */
            telemetry.addData("VuMark", "%s visible", vuMark);
            /* For fun, we also exhibit the navigational pose. In the Relic Recovery game,
                 * it is perhaps unlikely that you will actually need to act on this pose information, but
                 * we illustrate it nevertheless, for completeness. */
            OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) relicTemplate.getListener()).getPose();
            telemetry.addData("Pose", format(pose));
            /* We further illustrate how to decompose the pose into useful rotational and
                 * translational components */
            if (pose != null) {
                VectorF trans = pose.getTranslation();
                Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
                // Extract the X, Y, and Z components of the offset of the target relative to the robot
                double tX = trans.get(0);
                double tY = trans.get(1);
                double tZ = trans.get(2);
                // Extract the rotational components of the target relative to the robot
                double rX = rot.firstAngle;
                double rY = rot.secondAngle;
                double rZ = rot.thirdAngle;
            }
        } else {
            telemetry.addData("VuMark", "not visible");
        }
        telemetry.update();
    }
}

Example 5 with VectorF

use of org.firstinspires.ftc.robotcore.external.matrices.VectorF in project Relic_Main by TeamOverdrive.

the class ConceptVuMarkIdentification method runOpMode.

@Override
public void runOpMode() {
    /*
         * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone);
         * If no camera monitor is desired, use the parameterless constructor instead (commented out below).
         */
    int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
    VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
    // OR...  Do Not Activate the Camera Monitor View, to save power
    // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
    /*
         * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
         * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
         * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
         * web site at https://developer.vuforia.com/license-manager.
         *
         * Vuforia license keys are always 380 characters long, and look as if they contain mostly
         * random data. As an example, here is a example of a fragment of a valid key:
         *      ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
         * Once you've obtained a license key, copy the string from the Vuforia web site
         * and paste it in to your code onthe next line, between the double quotes.
         */
    parameters.vuforiaLicenseKey = "ATsODcD/////AAAAAVw2lR...d45oGpdljdOh5LuFB9nDNfckoxb8COxKSFX";
    /*
         * We also indicate which camera on the RC that we wish to use.
         * Here we chose the back (HiRes) camera (for greater range), but
         * for a competition robot, the front camera might be more convenient.
         */
    parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
    this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);
    /**
     * Load the data set containing the VuMarks for Relic Recovery. There's only one trackable
     * in this data set: all three of the VuMarks in the game were created from this one template,
     * but differ in their instance id information.
     * @see VuMarkInstanceId
     */
    VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
    VuforiaTrackable relicTemplate = relicTrackables.get(0);
    // can help in debugging; otherwise not necessary
    relicTemplate.setName("relicVuMarkTemplate");
    telemetry.addData(">", "Press Play to start");
    telemetry.update();
    waitForStart();
    relicTrackables.activate();
    while (opModeIsActive()) {
        /**
         * See if any of the instances of {@link relicTemplate} are currently visible.
         * {@link RelicRecoveryVuMark} is an enum which can have the following values:
         * UNKNOWN, LEFT, CENTER, and RIGHT. When a VuMark is visible, something other than
         * UNKNOWN will be returned by {@link RelicRecoveryVuMark#from(VuforiaTrackable)}.
         */
        RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
        if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
            /* Found an instance of the template. In the actual game, you will probably
                 * loop until this condition occurs, then move on to act accordingly depending
                 * on which VuMark was visible. */
            telemetry.addData("VuMark", "%s visible", vuMark);
            /* For fun, we also exhibit the navigational pose. In the Relic Recovery game,
                 * it is perhaps unlikely that you will actually need to act on this pose information, but
                 * we illustrate it nevertheless, for completeness. */
            OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) relicTemplate.getListener()).getPose();
            telemetry.addData("Pose", format(pose));
            /* We further illustrate how to decompose the pose into useful rotational and
                 * translational components */
            if (pose != null) {
                VectorF trans = pose.getTranslation();
                Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
                // Extract the X, Y, and Z components of the offset of the target relative to the robot
                double tX = trans.get(0);
                double tY = trans.get(1);
                double tZ = trans.get(2);
                // Extract the rotational components of the target relative to the robot
                double rX = rot.firstAngle;
                double rY = rot.secondAngle;
                double rZ = rot.thirdAngle;
            }
        } else {
            telemetry.addData("VuMark", "not visible");
        }
        telemetry.update();
    }
}