mirror of
https://github.com/PhotonVision/photonvision
synced 2026-07-03 03:01:40 +00:00
Bump wpilib to 2026 beta (#2192)
## Description <!-- What changed? Why? (the code + comments should speak for itself on the "how") --> <!-- Fun screenshots or a cool video or something are super helpful as well. If this touches platform-specific behavior, this is where test evidence should be collected. --> <!-- Any issues this pull request closes or pull requests this supersedes should be linked with `Closes #issuenumber`. --> Bump to wpilib 2026 beta. This does not bump our pythonlib, as robotpy hasn't come out yet. ## Meta Merge checklist: - [ ] Pull Request title is [short, imperative summary](https://cbea.ms/git-commit/) of proposed changes - [ ] The description documents the _what_ and _why_ - [ ] This PR has been [linted](https://docs.photonvision.org/en/latest/docs/contributing/linting.html). - [ ] If this PR changes behavior or adds a feature, user documentation is updated - [ ] If this PR touches photon-serde, all messages have been regenerated and hashes have not changed unexpectedly - [ ] If this PR touches configuration, this is backwards compatible with settings back to v2025.3.2 - [ ] If this PR touches pipeline settings or anything related to data exchange, the frontend typing is updated - [ ] If this PR addresses a bug, a regression test for it is added --------- Co-authored-by: Gold856 <117957790+Gold856@users.noreply.github.com>
This commit is contained in:
@@ -156,8 +156,7 @@ struct ProblemState {
|
||||
#undef MAKE_ARGV
|
||||
};
|
||||
|
||||
wpi::expected<constrained_solvepnp::RobotStateMat,
|
||||
sleipnir::SolverExitCondition>
|
||||
wpi::expected<constrained_solvepnp::RobotStateMat, slp::ExitStatus>
|
||||
constrained_solvepnp::do_optimization(
|
||||
bool heading_free, int nTags,
|
||||
constrained_solvepnp::CameraCalibration cameraCal,
|
||||
@@ -173,7 +172,7 @@ constrained_solvepnp::do_optimization(
|
||||
if constexpr (VERBOSE) fmt::println("Got unexpected num cols!");
|
||||
// TODO find a new error code
|
||||
return wpi::unexpected{
|
||||
sleipnir::SolverExitCondition::kNonfiniteInitialCostOrConstraints};
|
||||
slp::ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS};
|
||||
}
|
||||
|
||||
// rescale observations to homogenous pixel coordinates
|
||||
@@ -203,7 +202,7 @@ constrained_solvepnp::do_optimization(
|
||||
auto problemOpt = createProblem(nTags, heading_free);
|
||||
if (!problemOpt) {
|
||||
return wpi::unexpected{
|
||||
sleipnir::SolverExitCondition::kNonfiniteInitialCostOrConstraints};
|
||||
slp::ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS};
|
||||
}
|
||||
|
||||
ProblemState<3> pState{robot2camera, field2points, point_observations,
|
||||
@@ -233,7 +232,7 @@ constrained_solvepnp::do_optimization(
|
||||
|
||||
// Check for diverging iterates
|
||||
if (x.template lpNorm<Eigen::Infinity>() > 1e20 || !x.allFinite()) {
|
||||
return wpi::unexpected{sleipnir::SolverExitCondition::kDivergingIterates};
|
||||
return wpi::unexpected{slp::ExitStatus::DIVERGING_ITERATES};
|
||||
}
|
||||
|
||||
GradMat g = pState.calculateGradJ(x);
|
||||
@@ -254,7 +253,7 @@ constrained_solvepnp::do_optimization(
|
||||
auto H_ldlt = H.ldlt();
|
||||
if (H_ldlt.info() != Eigen::Success) {
|
||||
std::cerr << "LDLT decomp failed! H=" << std::endl << H << std::endl;
|
||||
return wpi::unexpected{sleipnir::SolverExitCondition::kLocallyInfeasible};
|
||||
return wpi::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE};
|
||||
}
|
||||
|
||||
// Make sure H is positive definite (all eigenvalues are > 0)
|
||||
@@ -278,8 +277,7 @@ constrained_solvepnp::do_optimization(
|
||||
|
||||
if (H_ldlt.info() != Eigen::Success) {
|
||||
std::cerr << "LDLT decomp failed! H=" << std::endl << H << std::endl;
|
||||
return wpi::unexpected{
|
||||
sleipnir::SolverExitCondition::kLocallyInfeasible};
|
||||
return wpi::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE};
|
||||
}
|
||||
|
||||
// If our eigenvalues aren't positive definite, pick a new δ for next
|
||||
@@ -289,8 +287,7 @@ constrained_solvepnp::do_optimization(
|
||||
|
||||
// If the Hessian perturbation is too high, report failure
|
||||
if (δ > 1e20) {
|
||||
return wpi::unexpected{
|
||||
sleipnir::SolverExitCondition::kLocallyInfeasible};
|
||||
return wpi::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE};
|
||||
}
|
||||
} else {
|
||||
// Done!
|
||||
@@ -301,8 +298,7 @@ constrained_solvepnp::do_optimization(
|
||||
}
|
||||
|
||||
if (i_reg == MAX_REG_STEPS) {
|
||||
return wpi::unexpected{
|
||||
sleipnir::SolverExitCondition::kLocallyInfeasible};
|
||||
return wpi::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE};
|
||||
}
|
||||
} else {
|
||||
// std::printf("Already regularized\n");
|
||||
@@ -345,8 +341,7 @@ constrained_solvepnp::do_optimization(
|
||||
|
||||
// If our step size shrank too much, report local infesibility
|
||||
if (alpha < α_min_frac * γConstraint) {
|
||||
return wpi::unexpected{
|
||||
sleipnir::SolverExitCondition::kLocallyInfeasible};
|
||||
return wpi::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -191,9 +191,8 @@ std::optional<photon::PnpResult> EstimateRobotPoseConstrainedSolvePNP(
|
||||
guess2.X().value(), guess2.Y().value(),
|
||||
guess2.Rotation().Radians().value()};
|
||||
|
||||
wpi::expected<constrained_solvepnp::RobotStateMat,
|
||||
sleipnir::SolverExitCondition>
|
||||
result = constrained_solvepnp::do_optimization(
|
||||
wpi::expected<constrained_solvepnp::RobotStateMat, slp::ExitStatus> result =
|
||||
constrained_solvepnp::do_optimization(
|
||||
headingFree, knownTags.size(), cameraCal, robotToCamera, guessMat,
|
||||
field2points, pointObservations, gyroTheta.Radians().value(),
|
||||
gyroErrorScaleFac);
|
||||
|
||||
@@ -18,7 +18,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <sleipnir/optimization/SolverExitCondition.hpp>
|
||||
#include <sleipnir/optimization/solver/exit_status.hpp>
|
||||
#include <wpi/expected>
|
||||
|
||||
namespace constrained_solvepnp {
|
||||
@@ -40,7 +40,7 @@ using RobotStateMat = Eigen::Matrix<casadi_real, 3, 1>;
|
||||
* to this. The number of columns in field2points and point_observations just be
|
||||
* exactly 4x nTags.
|
||||
*/
|
||||
wpi::expected<RobotStateMat, sleipnir::SolverExitCondition> do_optimization(
|
||||
wpi::expected<RobotStateMat, slp::ExitStatus> do_optimization(
|
||||
bool heading_free, int nTags, CameraCalibration cameraCal,
|
||||
// Note that casadi is column major, apparently
|
||||
Eigen::Matrix<casadi_real, 4, 4, Eigen::ColMajor> robot2camera,
|
||||
|
||||
@@ -84,9 +84,8 @@ Java_org_photonvision_jni_ConstrainedSolvepnpJni_do_1optimization
|
||||
std::cout << "observations:\n" << pointObservationsMat << std::endl;
|
||||
#endif
|
||||
|
||||
wpi::expected<constrained_solvepnp::RobotStateMat,
|
||||
sleipnir::SolverExitCondition>
|
||||
result = constrained_solvepnp::do_optimization(
|
||||
wpi::expected<constrained_solvepnp::RobotStateMat, slp::ExitStatus> result =
|
||||
constrained_solvepnp::do_optimization(
|
||||
headingFree, nTags, cameraCal_, robot2cameraMat, xGuessMat,
|
||||
field2pointsMat, pointObservationsMat, gyro_θ, gyro_error_scale_fac);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user