Skip to content

Commit

Permalink
formatting fixes
Browse files Browse the repository at this point in the history
REMEMBER TO REMOVE UNUSED IMPORTS, IM JUST TOO LAZY TO CHECK RN
  • Loading branch information
srimanachanta committed Nov 18, 2023
1 parent 4b56be3 commit 905f764
Show file tree
Hide file tree
Showing 12 changed files with 32 additions and 44 deletions.
10 changes: 5 additions & 5 deletions photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -366,8 +366,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
result.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
result.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
}

return Update(result, std::nullopt, std::nullopt,
Expand Down Expand Up @@ -425,9 +425,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(

const Pose3d pose = detail::ToPose3d(tvec, rvec);

return photon::EstimatedRobotPose(
pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),
result.GetTargets(), MULTI_TAG_PNP_ON_RIO);
return photon::EstimatedRobotPose(pose.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(),
MULTI_TAG_PNP_ON_RIO);
}

std::optional<EstimatedRobotPose>
Expand Down
2 changes: 1 addition & 1 deletion photon-lib/src/main/native/include/photon/PhotonCamera.h
Original file line number Diff line number Diff line change
Expand Up @@ -41,9 +41,9 @@

#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/TargetCorner.h"

namespace cv {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,12 +31,11 @@
#include <frc/geometry/Transform3d.h>

#include "photon/PhotonCamera.h"

#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/TargetCorner.h"

namespace cv {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -28,9 +28,9 @@

#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/TargetCorner.h"

namespace photon {
Expand Down
2 changes: 1 addition & 1 deletion photon-lib/src/main/native/include/photon/PhotonUtils.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,9 @@

#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/TargetCorner.h"

namespace photon {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,15 +34,13 @@

#include "photon/PhotonCamera.h"
#include "photon/PhotonTargetSortMode.h"

#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/TargetCorner.h"


namespace photon {
class SimPhotonCamera : public PhotonCamera {
public:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,12 +34,11 @@

#include "SimPhotonCamera.h"
#include "SimVisionTarget.h"

#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/TargetCorner.h"

namespace photon {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,9 +29,9 @@

#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/TargetCorner.h"

namespace photon {
Expand Down
3 changes: 1 addition & 2 deletions photon-lib/src/test/native/cpp/PacketTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,12 +27,11 @@
#include <units/angle.h>

#include "gtest/gtest.h"

#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/TargetCorner.h"

TEST(PacketTest, PhotonTrackedTarget) {
Expand Down
25 changes: 11 additions & 14 deletions photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,15 +35,13 @@
#include <wpi/SmallVector.h>

#include "gtest/gtest.h"

#include "photon/PhotonCamera.h"
#include "photon/PhotonPoseEstimator.h"

#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/TargetCorner.h"

static std::vector<frc::AprilTag> tags = {
Expand Down Expand Up @@ -89,8 +87,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(11));

photon::PhotonPoseEstimator estimator(
aprilTags, photon::LOWEST_AMBIGUITY, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
frc::Pose3d pose = estimatedPose.value().estimatedPose;

Expand Down Expand Up @@ -187,9 +185,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(17));

photon::PhotonPoseEstimator estimator(aprilTags,
photon::CLOSEST_TO_REFERENCE_POSE,
std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, std::move(cameraOne), {});
estimator.SetReferencePose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
auto estimatedPose = estimator.Update();
Expand Down Expand Up @@ -232,8 +229,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(17));

photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_LAST_POSE, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
std::move(cameraOne), {});
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
auto estimatedPose = estimator.Update();
Expand Down Expand Up @@ -307,8 +304,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(15));

photon::PhotonPoseEstimator estimator(
aprilTags, photon::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
frc::Pose3d pose = estimatedPose.value().estimatedPose;

Expand Down Expand Up @@ -347,8 +344,8 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {

cameraOne.test = true;

photon::PhotonPoseEstimator estimator(
aprilTags, photon::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
std::move(cameraOne), {});

// empty input, expect empty out
estimator.GetCamera()->testResult = {2_ms, {}};
Expand Down
16 changes: 6 additions & 10 deletions photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -164,10 +164,9 @@ TEST_F(SimVisionSystemTest, TestNotVisibleTooFarForLEDs) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
photon::SimVisionSystem sys{
"Test", 80.0_deg, frc::Transform3d{}, 10_m, 640, 480, 1.0};
sys.AddSimVisionTarget(
photon::SimVisionTarget{targetPose, 1_m, 0.25_m, 78});
photon::SimVisionSystem sys{"Test", 80.0_deg, frc::Transform3d{}, 10_m, 640,
480, 1.0};
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 1_m, 0.25_m, 78});

frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
Expand All @@ -186,8 +185,7 @@ TEST_P(SimVisionSystemTestWithParamsTest, YawAngles) {
frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}};
photon::SimVisionSystem sys{
"Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
sys.AddSimVisionTarget(
photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});

sys.ProcessFrame(robotPose);

Expand All @@ -208,8 +206,7 @@ TEST_P(SimVisionSystemTestWithParamsTest, PitchAngles) {
frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{0_deg}};
photon::SimVisionSystem sys{
"Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
sys.AddSimVisionTarget(
photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});

sys.MoveCamera(frc::Transform3d{frc::Translation3d{},
frc::Rotation3d{0_deg, GetParam(), 0_deg}});
Expand Down Expand Up @@ -251,8 +248,7 @@ TEST_P(SimVisionSystemTestDistanceParamsTest, DistanceCalc) {
640,
480,
0};
sys.AddSimVisionTarget(
photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 0});
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 0});
sys.ProcessFrame(robotPose);

auto results = sys.cam.GetLatestResult();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@ class PhotonCameraWrapper {
public:
photon::PhotonPoseEstimator m_poseEstimator{
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
photon::MULTI_TAG_PNP_ON_RIO,
std::move(photon::PhotonCamera{"WPI2023"}), frc::Transform3d{}};
photon::MULTI_TAG_PNP_ON_RIO, std::move(photon::PhotonCamera{"WPI2023"}),
frc::Transform3d{}};

inline std::optional<photon::EstimatedRobotPose> Update(
frc::Pose2d estimatedPose) {
Expand Down

0 comments on commit 905f764

Please sign in to comment.