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
010public final class ComputerVisionUtil {
011  private ComputerVisionUtil() {
012    throw new AssertionError("utility class");
013  }
014
015  /**
016   * Returns the robot's pose in the field coordinate system given an object's field-relative pose,
017   * the transformation from the camera's pose to the object's pose (obtained via computer vision),
018   * and the transformation from the robot's pose to the camera's pose.
019   *
020   * <p>The object could be a target or a fiducial marker.
021   *
022   * @param objectInField An object's field-relative pose.
023   * @param cameraToObject The transformation from the camera's pose to the object's pose. This
024   *     comes from computer vision.
025   * @param robotToCamera The transformation from the robot's pose to the camera's pose. This can
026   *     either be a constant for a rigidly mounted camera, or variable if the camera is mounted to
027   *     a turret. If the camera was mounted 3 inches in front of the "origin" (usually physical
028   *     center) of the robot, this would be new Transform3d(Units.inchesToMeters(3.0), 0.0, 0.0,
029   *     new Rotation3d()).
030   * @return The robot's field-relative pose.
031   */
032  public static Pose3d objectToRobotPose(
033      Pose3d objectInField, Transform3d cameraToObject, Transform3d robotToCamera) {
034    final var objectToCamera = cameraToObject.inverse();
035    final var cameraToRobot = robotToCamera.inverse();
036    return objectInField.plus(objectToCamera).plus(cameraToRobot);
037  }
038}