001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math;
006
007import edu.wpi.first.math.geometry.Pose3d;
008import edu.wpi.first.math.geometry.Transform3d;
009
010/** Computer vision utility functions. */
011public final class ComputerVisionUtil {
012  private ComputerVisionUtil() {
013    throw new AssertionError("utility class");
014  }
015
016  /**
017   * Returns the robot's pose in the field coordinate system given an object's field-relative pose,
018   * the transformation from the camera's pose to the object's pose (obtained via computer vision),
019   * and the transformation from the robot's pose to the camera's pose.
020   *
021   * <p>The object could be a target or a fiducial marker.
022   *
023   * @param objectInField An object's field-relative pose.
024   * @param cameraToObject The transformation from the camera's pose to the object's pose. This
025   *     comes from computer vision.
026   * @param robotToCamera The transformation from the robot's pose to the camera's pose. This can
027   *     either be a constant for a rigidly mounted camera, or variable if the camera is mounted to
028   *     a turret. If the camera was mounted 3 inches in front of the "origin" (usually physical
029   *     center) of the robot, this would be new Transform3d(Units.inchesToMeters(3.0), 0.0, 0.0,
030   *     new Rotation3d()).
031   * @return The robot's field-relative pose.
032   */
033  public static Pose3d objectToRobotPose(
034      Pose3d objectInField, Transform3d cameraToObject, Transform3d robotToCamera) {
035    final var objectToCamera = cameraToObject.inverse();
036    final var cameraToRobot = robotToCamera.inverse();
037    return objectInField.plus(objectToCamera).plus(cameraToRobot);
038  }
039}