Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-23 01:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

[photonli?b C++] Fix rotation3d constructor (#1553)

Browse Source 
Fixes #1552 -- mixed up rpy and a rotation vector 

After changes:


![image](https://github.com/user-attachments/assets/0f58c2be-fc00-494c-af76-408c1ec438f9)
This commit is contained in:
Drew Williams
2024-11-11 11:06:00 -05:00
committed by GitHub
parent 5bee683661
commit 1fc93bd05d
4 changed files with 86 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
View File

@@ -201,9 +201,8 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {
cv::Mat wrapped{rvecInput.rows, rvecInput.cols, CV_32F};
rvecInput.convertTo(wrapped, CV_32F);
data = wrapped.at<cv::Vec3f>(cv::Point{0, 0});
return RotationEDNToNWU(frc::Rotation3d{units::radian_t{data[0]},
units::radian_t{data[1]},
units::radian_t{data[2]}});
return RotationEDNToNWU(
frc::Rotation3d{Eigen::Vector3d{data[0], data[1], data[2]}});
}
[[maybe_unused]] static std::optional<photon::PnpResult> SolvePNP_Square(

11
photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h
View File

@@ -26,11 +26,14 @@
namespace photon {
class RotTrlTransform3d {
public:
RotTrlTransform3d(const frc::Rotation3d& rot, const frc::Translation3d& trl)
: trl(trl), rot(rot) {}
RotTrlTransform3d(const frc::Rotation3d& newRot,
const frc::Translation3d& newTrl)
: trl{newTrl}, rot{newRot} {}
RotTrlTransform3d(const frc::Pose3d& initial, const frc::Pose3d& last)
: trl(last.Translation() - initial.Translation().RotateBy(rot)),
rot(last.Rotation() - initial.Rotation()) {}
: trl{last.Translation() - initial.Translation().RotateBy(
last.Rotation() - initial.Rotation())},
rot{last.Rotation() - initial.Rotation()} {}
explicit RotTrlTransform3d(const frc::Transform3d& trf)
: RotTrlTransform3d(trf.Rotation(), trf.Translation()) {}
RotTrlTransform3d()