Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Reduce calculation in strategy functions #753

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

Reduce calculation in strategy functions #753

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
c161992
Reduce calculation in strategy functions
rwlee Jan 19, 2023
f406d3c
fix clang format issue
rwlee Jan 20, 2023
202a9e8
Apply suggestions from code review
rwlee Jan 20, 2023
ebb2170
Update photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
rwlee Jan 20, 2023
67f0a72
Merge branch 'master' into rwlee/optimizelogic
srimanachanta Oct 15, 2023
8d0e896
Formatting fixes
srimanachanta Oct 15, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 9 additions & 11 deletions photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,14 +77,14 @@ public enum PoseStrategy {
* Create a new PhotonPoseEstimator.
*
* @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects
* with respect to the FIRST field using the <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
* with respect to the FIRST field using the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
rwlee marked this conversation as resolved.
Show resolved Hide resolved
* Coordinate System</a>.
* @param strategy The strategy it should use to determine the best pose.
* @param camera PhotonCameras and
* @param robotToCamera Transform3d from the center of the robot to the camera mount positions
* (ie, robot ➔ camera) in the <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* (ie, robot ➔ camera) in the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
rwlee marked this conversation as resolved.
Show resolved Hide resolved
* Coordinate System</a>.
*/
public PhotonPoseEstimator(
Expand Down Expand Up @@ -316,7 +316,8 @@ private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelin
.transformBy(target.getBestCameraToTarget().inverse())
.getZ());

if (alternateTransformDelta < smallestHeightDifference) {
if (alternateTransformDelta < smallestHeightDifference
&& alternateTransformDelta < bestTransformDelta) {
Comment on lines -488 to +489
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So the point of this function is to find a transform such that the Z difference is minimized right? I think we can better represent this intent with something similar to (but not necessarily a) list comprehension? Probably out of scope, and I don't think anyone uses this function anyways.

smallestHeightDifference = alternateTransformDelta;
closestHeightTarget =
new EstimatedRobotPose(
Expand All @@ -325,9 +326,7 @@ private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelin
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds());
}

if (bestTransformDelta < smallestHeightDifference) {
} else if (bestTransformDelta < smallestHeightDifference) {
smallestHeightDifference = bestTransformDelta;
closestHeightTarget =
new EstimatedRobotPose(
Expand Down Expand Up @@ -393,12 +392,11 @@ private Optional<EstimatedRobotPose> closestToReferencePoseStrategy(
double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition));
double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition));

if (altDifference < smallestPoseDelta) {
if (altDifference < smallestPoseDelta && altDifference < bestDifference) {
smallestPoseDelta = altDifference;
lowestDeltaPose =
new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds());
}
if (bestDifference < smallestPoseDelta) {
} else if (bestDifference < smallestPoseDelta) {
smallestPoseDelta = bestDifference;
lowestDeltaPose =
new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds());
Expand Down
51 changes: 26 additions & 25 deletions photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -153,22 +153,23 @@ RobotPoseEstimator::ClosestToCameraHeightStrategy() {
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
units::meter_t alternativeDifference = units::math::abs(
p.second.Z() -
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.Z());
units::meter_t bestDifference = units::math::abs(
p.second.Z() -
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
if (alternativeDifference < smallestHeightDifference) {
auto altCameraTransform =
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse());
units::meter_t alternativeDifference =
units::math::abs(p.second.Z() - altCameraTransform.Z());
auto bestCameraTransform =
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
units::meter_t bestDifference =
units::math::abs(p.second.Z() - bestCameraTransform.Z());

if (alternativeDifference < smallestHeightDifference &&
alternativeDifference < bestDifference) {
smallestHeightDifference = alternativeDifference;
pose = targetPose.TransformBy(
target.GetAlternateCameraToTarget().Inverse());
pose = altCameraTransform;
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
if (bestDifference < smallestHeightDifference) {
} else if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
pose = bestCameraTransform;
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
}
Expand Down Expand Up @@ -198,25 +199,25 @@ RobotPoseEstimator::ClosestToReferencePoseStrategy() {
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
auto altCameraTransform =
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse());
units::meter_t alternativeDifference =
units::math::abs(referencePose.Translation().Distance(
targetPose
.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.Translation()));
altCameraTransform.Translation()));
auto bestCameraTansform =
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
units::meter_t bestDifference =
units::math::abs(referencePose.Translation().Distance(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.Translation()));
if (alternativeDifference < smallestDifference) {
bestCameraTansform.Translation()));

if (alternativeDifference < smallestDifference &&
alternativeDifference < bestDifference) {
smallestDifference = alternativeDifference;
pose = targetPose.TransformBy(
target.GetAlternateCameraToTarget().Inverse());
pose = altCameraTransform;
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}

if (bestDifference < smallestDifference) {
} else if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
pose = besttCameraTransform;
rwlee marked this conversation as resolved.
Show resolved Hide resolved
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
}
Expand Down