// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "frc/ComputerVisionUtil.h"

#include "units/math.h"

namespace frc {

frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
                              const frc::Transform3d& cameraToObject,
                              const frc::Transform3d& robotToCamera) {
  const auto objectToCamera = cameraToObject.Inverse();
  const auto cameraToRobot = robotToCamera.Inverse();
  return objectInField + objectToCamera + cameraToRobot;
}

}  // namespace frc