mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-21 01:01:41 +00:00
Update C++ Simulation to match Java (#1026)
This commit is contained in:
Drew Williams
@@ -0,0 +1,61 @@
|
||||
/*
|
||||
* Copyright (C) Photon Vision.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
|
||||
namespace photon {
|
||||
class CameraTargetRelation {
|
||||
public:
|
||||
const frc::Pose3d camPose;
|
||||
const frc::Transform3d camToTarg;
|
||||
const units::meter_t camToTargDist;
|
||||
const units::meter_t camToTargDistXY;
|
||||
const frc::Rotation2d camToTargYaw;
|
||||
const frc::Rotation2d camToTargPitch;
|
||||
|
||||
const frc::Rotation2d camToTargAngle;
|
||||
|
||||
const frc::Transform3d targToCam;
|
||||
const frc::Rotation2d targToCamYaw;
|
||||
const frc::Rotation2d targToCamPitch;
|
||||
|
||||
const frc::Rotation2d targToCamAngle;
|
||||
|
||||
CameraTargetRelation(const frc::Pose3d& cameraPose,
|
||||
const frc::Pose3d& targetPose)
|
||||
: camPose(cameraPose),
|
||||
camToTarg(frc::Transform3d{cameraPose, targetPose}),
|
||||
camToTargDist(camToTarg.Translation().Norm()),
|
||||
camToTargDistXY(units::math::hypot(camToTarg.Translation().X(),
|
||||
camToTarg.Translation().Y())),
|
||||
camToTargYaw(frc::Rotation2d{camToTarg.X().to<double>(),
|
||||
camToTarg.Y().to<double>()}),
|
||||
camToTargPitch(frc::Rotation2d{camToTargDistXY.to<double>(),
|
||||
-camToTarg.Z().to<double>()}),
|
||||
camToTargAngle(frc::Rotation2d{units::math::hypot(
|
||||
camToTargYaw.Radians(), camToTargPitch.Radians())}),
|
||||
targToCam(frc::Transform3d{targetPose, cameraPose}),
|
||||
targToCamYaw(frc::Rotation2d{targToCam.X().to<double>(),
|
||||
targToCam.Y().to<double>()}),
|
||||
targToCamPitch(frc::Rotation2d{camToTargDistXY.to<double>(),
|
||||
-targToCam.Z().to<double>()}),
|
||||
targToCamAngle(frc::Rotation2d{units::math::hypot(
|
||||
targToCamYaw.Radians(), targToCamPitch.Radians())}) {}
|
||||
};
|
||||
} // namespace photon
|
||||
@@ -0,0 +1,291 @@
|
||||
/*
|
||||
* Copyright (C) Photon Vision.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <opencv2/calib3d.hpp>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
#include "RotTrlTransform3d.h"
|
||||
|
||||
#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
|
||||
#include <opencv2/core/eigen.hpp>
|
||||
#include "photon/targeting/PNPResult.h"
|
||||
#include "photon/targeting/MultiTargetPNPResult.h"
|
||||
|
||||
namespace photon {
|
||||
namespace OpenCVHelp {
|
||||
|
||||
static frc::Rotation3d NWU_TO_EDN{
|
||||
(Eigen::Matrix3d() << 0, -1, 0, 0, 0, -1, 1, 0, 0).finished()};
|
||||
static frc::Rotation3d EDN_TO_NWU{
|
||||
(Eigen::Matrix3d() << 0, 0, 1, -1, 0, 0, 0, -1, 0).finished()};
|
||||
|
||||
static std::vector<cv::Point2f> GetConvexHull(
|
||||
const std::vector<cv::Point2f>& points) {
|
||||
std::vector<int> outputHull{};
|
||||
cv::convexHull(points, outputHull);
|
||||
std::vector<cv::Point2f> convexPoints;
|
||||
for (size_t i = 0; i < outputHull.size(); i++) {
|
||||
convexPoints.push_back(points[outputHull[i]]);
|
||||
}
|
||||
return convexPoints;
|
||||
}
|
||||
|
||||
static cv::RotatedRect GetMinAreaRect(const std::vector<cv::Point2f>& points) {
|
||||
return cv::minAreaRect(points);
|
||||
}
|
||||
|
||||
static frc::Translation3d TranslationNWUtoEDN(const frc::Translation3d& trl) {
|
||||
return trl.RotateBy(NWU_TO_EDN);
|
||||
}
|
||||
|
||||
static frc::Rotation3d RotationNWUtoEDN(const frc::Rotation3d& rot) {
|
||||
return -NWU_TO_EDN + (rot + NWU_TO_EDN);
|
||||
}
|
||||
|
||||
static std::vector<cv::Point3f> TranslationToTVec(
|
||||
const std::vector<frc::Translation3d>& translations) {
|
||||
std::vector<cv::Point3f> retVal;
|
||||
retVal.reserve(translations.size());
|
||||
for (size_t i = 0; i < translations.size(); i++) {
|
||||
frc::Translation3d trl = TranslationNWUtoEDN(translations[i]);
|
||||
retVal.emplace_back(cv::Point3f{trl.X().to<float>(), trl.Y().to<float>(),
|
||||
trl.Z().to<float>()});
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
static std::vector<cv::Point3f> RotationToRVec(
|
||||
const frc::Rotation3d& rotation) {
|
||||
std::vector<cv::Point3f> retVal{};
|
||||
frc::Rotation3d rot = RotationNWUtoEDN(rotation);
|
||||
retVal.emplace_back(cv::Point3d{
|
||||
rot.GetQuaternion().ToRotationVector()(0),
|
||||
rot.GetQuaternion().ToRotationVector()(1),
|
||||
rot.GetQuaternion().ToRotationVector()(2),
|
||||
});
|
||||
return retVal;
|
||||
}
|
||||
|
||||
[[maybe_unused]] static cv::Point2f AvgPoint(std::vector<cv::Point2f> points) {
|
||||
if (points.size() == 0) {
|
||||
return cv::Point2f{};
|
||||
}
|
||||
cv::reduce(points, points, 0, cv::REDUCE_AVG);
|
||||
return points[0];
|
||||
}
|
||||
|
||||
[[maybe_unused]] static std::vector<std::pair<float, float>> PointsToCorners(
|
||||
const std::vector<cv::Point2f>& points) {
|
||||
std::vector<std::pair<float, float>> retVal;
|
||||
retVal.reserve(points.size());
|
||||
for (size_t i = 0; i < points.size(); i++) {
|
||||
retVal.emplace_back(std::make_pair(points[i].x, points[i].y));
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
[[maybe_unused]] static std::vector<cv::Point2f> CornersToPoints(
|
||||
const std::vector<std::pair<float, float>>& corners) {
|
||||
std::vector<cv::Point2f> retVal;
|
||||
retVal.reserve(corners.size());
|
||||
for (size_t i = 0; i < corners.size(); i++) {
|
||||
retVal.emplace_back(cv::Point2f{corners[i].first, corners[i].second});
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
[[maybe_unused]] static cv::Rect GetBoundingRect(
|
||||
const std::vector<cv::Point2f>& points) {
|
||||
return cv::boundingRect(points);
|
||||
}
|
||||
|
||||
static std::vector<cv::Point2f> ProjectPoints(
|
||||
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
|
||||
const Eigen::Matrix<double, 5, 1>& distCoeffs,
|
||||
const RotTrlTransform3d& camRt,
|
||||
const std::vector<frc::Translation3d>& objectTranslations) {
|
||||
std::vector<cv::Point3f> objectPoints = TranslationToTVec(objectTranslations);
|
||||
std::vector<cv::Point3f> rvec = RotationToRVec(camRt.GetRotation());
|
||||
std::vector<cv::Point3f> tvec = TranslationToTVec({camRt.GetTranslation()});
|
||||
cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
|
||||
cv::eigen2cv(cameraMatrix, cameraMat);
|
||||
cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
|
||||
cv::eigen2cv(distCoeffs, distCoeffsMat);
|
||||
std::vector<cv::Point2f> imagePoints{};
|
||||
cv::projectPoints(objectPoints, rvec, tvec, cameraMat, distCoeffsMat,
|
||||
imagePoints);
|
||||
return imagePoints;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
static std::vector<T> ReorderCircular(const std::vector<T> elements,
|
||||
bool backwards, int shiftStart) {
|
||||
size_t size = elements.size();
|
||||
int dir = backwards ? -1 : 1;
|
||||
std::vector<T> reordered{elements};
|
||||
for (size_t i = 0; i < size; i++) {
|
||||
int index = (i * dir + shiftStart * dir) % size;
|
||||
if (index < 0) {
|
||||
index = size + index;
|
||||
}
|
||||
reordered[i] = elements[index];
|
||||
}
|
||||
return reordered;
|
||||
}
|
||||
|
||||
static frc::Translation3d TranslationEDNToNWU(const frc::Translation3d& trl) {
|
||||
return trl.RotateBy(EDN_TO_NWU);
|
||||
}
|
||||
|
||||
static frc::Rotation3d RotationEDNToNWU(const frc::Rotation3d& rot) {
|
||||
return -EDN_TO_NWU + (rot + EDN_TO_NWU);
|
||||
}
|
||||
|
||||
static frc::Translation3d TVecToTranslation(const cv::Mat& tvecInput) {
|
||||
cv::Vec3f data{};
|
||||
cv::Mat wrapped{tvecInput.rows, tvecInput.cols, CV_32F};
|
||||
tvecInput.convertTo(wrapped, CV_32F);
|
||||
data = wrapped.at<cv::Vec3f>(cv::Point{0, 0});
|
||||
return TranslationEDNToNWU(frc::Translation3d{units::meter_t{data[0]},
|
||||
units::meter_t{data[1]},
|
||||
units::meter_t{data[2]}});
|
||||
}
|
||||
|
||||
static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {
|
||||
cv::Vec3f data{};
|
||||
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]}});
|
||||
}
|
||||
|
||||
[[maybe_unused]] static photon::PNPResult SolvePNP_Square(
|
||||
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
|
||||
const Eigen::Matrix<double, 5, 1>& distCoeffs,
|
||||
std::vector<frc::Translation3d> modelTrls,
|
||||
std::vector<cv::Point2f> imagePoints) {
|
||||
modelTrls = ReorderCircular(modelTrls, true, -1);
|
||||
imagePoints = ReorderCircular(imagePoints, true, -1);
|
||||
std::vector<cv::Point3f> objectMat = TranslationToTVec(modelTrls);
|
||||
std::vector<cv::Mat> rvecs;
|
||||
std::vector<cv::Mat> tvecs;
|
||||
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F);
|
||||
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F);
|
||||
cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F);
|
||||
|
||||
cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_32F);
|
||||
cv::eigen2cv(cameraMatrix, cameraMat);
|
||||
cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_32F);
|
||||
cv::eigen2cv(distCoeffs, distCoeffsMat);
|
||||
|
||||
cv::Vec2d errors{};
|
||||
frc::Transform3d best{};
|
||||
std::optional<frc::Transform3d> alt{std::nullopt};
|
||||
|
||||
for (int tries = 0; tries < 2; tries++) {
|
||||
cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs,
|
||||
tvecs, false, cv::SOLVEPNP_IPPE_SQUARE, rvec, tvec,
|
||||
reprojectionError);
|
||||
|
||||
errors = reprojectionError.at<cv::Vec2f>(cv::Point{0, 0});
|
||||
best = frc::Transform3d{TVecToTranslation(tvecs.at(0)),
|
||||
RVecToRotation(rvecs[0])};
|
||||
|
||||
if (tvecs.size() > 1) {
|
||||
alt = frc::Transform3d{TVecToTranslation(tvecs.at(1)),
|
||||
RVecToRotation(rvecs[1])};
|
||||
}
|
||||
|
||||
if (!std::isnan(errors[0])) {
|
||||
break;
|
||||
} else {
|
||||
cv::Point2f pt = imagePoints[0];
|
||||
pt.x -= 0.001f;
|
||||
pt.y -= 0.001f;
|
||||
imagePoints[0] = pt;
|
||||
}
|
||||
}
|
||||
|
||||
if (std::isnan(errors[0])) {
|
||||
fmt::print("SolvePNP_Square failed!\n");
|
||||
}
|
||||
if (alt) {
|
||||
photon::PNPResult result;
|
||||
result.best = best;
|
||||
result.alt = alt.value();
|
||||
result.ambiguity = errors[0] / errors[1];
|
||||
result.bestReprojErr = errors[0];
|
||||
result.altReprojErr = errors[1];
|
||||
result.isPresent = true;
|
||||
return result;
|
||||
} else {
|
||||
photon::PNPResult result;
|
||||
result.best = best;
|
||||
result.bestReprojErr = errors[0];
|
||||
result.isPresent = true;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
[[maybe_unused]] static photon::PNPResult SolvePNP_SQPNP(
|
||||
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
|
||||
const Eigen::Matrix<double, 5, 1>& distCoeffs,
|
||||
std::vector<frc::Translation3d> modelTrls,
|
||||
std::vector<cv::Point2f> imagePoints) {
|
||||
std::vector<cv::Point3f> objectMat = TranslationToTVec(modelTrls);
|
||||
std::vector<cv::Mat> rvecs{};
|
||||
std::vector<cv::Mat> tvecs{};
|
||||
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F);
|
||||
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F);
|
||||
cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F);
|
||||
|
||||
cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
|
||||
cv::eigen2cv(cameraMatrix, cameraMat);
|
||||
cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
|
||||
cv::eigen2cv(distCoeffs, distCoeffsMat);
|
||||
|
||||
float error = 0;
|
||||
frc::Transform3d best{};
|
||||
|
||||
cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs,
|
||||
tvecs, false, cv::SOLVEPNP_SQPNP, rvec, tvec,
|
||||
reprojectionError);
|
||||
|
||||
error = reprojectionError.at<float>(cv::Point{0, 0});
|
||||
best = frc::Transform3d{TVecToTranslation(tvecs.at(0)),
|
||||
RVecToRotation(rvecs[0])};
|
||||
|
||||
if (std::isnan(error)) {
|
||||
fmt::print("SolvePNP_Square failed!\n");
|
||||
}
|
||||
photon::PNPResult result;
|
||||
result.best = best;
|
||||
result.bestReprojErr = error;
|
||||
result.isPresent = true;
|
||||
return result;
|
||||
}
|
||||
} // namespace OpenCVHelp
|
||||
} // namespace photon
|
||||
@@ -0,0 +1,102 @@
|
||||
/*
|
||||
* Copyright (C) Photon Vision.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
#include <frc/geometry/Rotation3d.h>
|
||||
#include <frc/geometry/Translation3d.h>
|
||||
|
||||
namespace photon {
|
||||
class RotTrlTransform3d {
|
||||
public:
|
||||
RotTrlTransform3d(const frc::Rotation3d& rot, const frc::Translation3d& trl)
|
||||
: trl(trl), rot(rot) {}
|
||||
RotTrlTransform3d(const frc::Pose3d& initial, const frc::Pose3d& last)
|
||||
: trl(last.Translation() - initial.Translation().RotateBy(rot)),
|
||||
rot(last.Rotation() - initial.Rotation()) {}
|
||||
explicit RotTrlTransform3d(const frc::Transform3d& trf)
|
||||
: RotTrlTransform3d(trf.Rotation(), trf.Translation()) {}
|
||||
RotTrlTransform3d()
|
||||
: RotTrlTransform3d(frc::Rotation3d{}, frc::Translation3d{}) {}
|
||||
|
||||
static RotTrlTransform3d MakeRelativeTo(const frc::Pose3d& pose) {
|
||||
return RotTrlTransform3d{pose.Rotation(), pose.Translation()}.Inverse();
|
||||
}
|
||||
|
||||
RotTrlTransform3d Inverse() const {
|
||||
frc::Rotation3d invRot = -rot;
|
||||
frc::Translation3d invTrl = -(trl.RotateBy(invRot));
|
||||
return RotTrlTransform3d{invRot, invTrl};
|
||||
}
|
||||
|
||||
frc::Transform3d GetTransform() const { return frc::Transform3d{trl, rot}; }
|
||||
|
||||
frc::Translation3d GetTranslation() const { return trl; }
|
||||
|
||||
frc::Rotation3d GetRotation() const { return rot; }
|
||||
|
||||
frc::Translation3d Apply(const frc::Translation3d& trlToApply) const {
|
||||
return trlToApply.RotateBy(rot) + trl;
|
||||
}
|
||||
|
||||
std::vector<frc::Translation3d> ApplyTrls(
|
||||
const std::vector<frc::Translation3d>& trls) const {
|
||||
std::vector<frc::Translation3d> retVal;
|
||||
retVal.reserve(trls.size());
|
||||
for (const auto& currentTrl : trls) {
|
||||
retVal.emplace_back(Apply(currentTrl));
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
frc::Rotation3d Apply(const frc::Rotation3d& rotToApply) const {
|
||||
return rotToApply + rot;
|
||||
}
|
||||
|
||||
std::vector<frc::Rotation3d> ApplyTrls(
|
||||
const std::vector<frc::Rotation3d>& rots) const {
|
||||
std::vector<frc::Rotation3d> retVal;
|
||||
retVal.reserve(rots.size());
|
||||
for (const auto& currentRot : rots) {
|
||||
retVal.emplace_back(Apply(currentRot));
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
frc::Pose3d Apply(const frc::Pose3d& poseToApply) const {
|
||||
return frc::Pose3d{Apply(poseToApply.Translation()),
|
||||
Apply(poseToApply.Rotation())};
|
||||
}
|
||||
|
||||
std::vector<frc::Pose3d> ApplyPoses(
|
||||
const std::vector<frc::Pose3d>& poses) const {
|
||||
std::vector<frc::Pose3d> retVal;
|
||||
retVal.reserve(poses.size());
|
||||
for (const auto& currentPose : poses) {
|
||||
retVal.emplace_back(Apply(currentPose));
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
private:
|
||||
const frc::Translation3d trl;
|
||||
const frc::Rotation3d rot;
|
||||
};
|
||||
} // namespace photon
|
||||
@@ -0,0 +1,115 @@
|
||||
/*
|
||||
* Copyright (C) Photon Vision.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
#include <frc/geometry/Translation3d.h>
|
||||
|
||||
#include "RotTrlTransform3d.h"
|
||||
|
||||
namespace photon {
|
||||
class TargetModel {
|
||||
public:
|
||||
TargetModel(units::meter_t width, units::meter_t height)
|
||||
: vertices({frc::Translation3d{0_m, -width / 2.0, -height / 2.0},
|
||||
frc::Translation3d{0_m, width / 2.0, -height / 2.0},
|
||||
frc::Translation3d{0_m, width / 2.0, height / 2.0},
|
||||
frc::Translation3d{0_m, -width / 2.0, height / 2.0}}),
|
||||
isPlanar(true),
|
||||
isSpherical(false) {}
|
||||
|
||||
TargetModel(units::meter_t length, units::meter_t width,
|
||||
units::meter_t height)
|
||||
: TargetModel({
|
||||
frc::Translation3d{length / 2.0, -width / 2.0, -height / 2.0},
|
||||
frc::Translation3d{length / 2.0, width / 2.0, -height / 2.0},
|
||||
frc::Translation3d{length / 2.0, width / 2.0, height / 2.0},
|
||||
frc::Translation3d{length / 2.0, -width / 2.0, height / 2.0},
|
||||
frc::Translation3d{-length / 2.0, -width / 2.0, height / 2.0},
|
||||
frc::Translation3d{-length / 2.0, width / 2.0, height / 2.0},
|
||||
frc::Translation3d{-length / 2.0, width / 2.0, -height / 2.0},
|
||||
frc::Translation3d{-length / 2.0, -width / 2.0, -height / 2.0},
|
||||
}) {}
|
||||
|
||||
explicit TargetModel(units::meter_t diameter)
|
||||
: vertices({
|
||||
frc::Translation3d{0_m, -diameter / 2.0, 0_m},
|
||||
frc::Translation3d{0_m, 0_m, -diameter / 2.0},
|
||||
frc::Translation3d{0_m, diameter / 2.0, 0_m},
|
||||
frc::Translation3d{0_m, 0_m, diameter / 2.0},
|
||||
}),
|
||||
isPlanar(false),
|
||||
isSpherical(true) {}
|
||||
|
||||
explicit TargetModel(const std::vector<frc::Translation3d>& verts)
|
||||
: isSpherical(false) {
|
||||
if (verts.size() <= 2) {
|
||||
vertices = std::vector<frc::Translation3d>();
|
||||
isPlanar = false;
|
||||
} else {
|
||||
bool cornersPlanar = true;
|
||||
for (const auto& corner : verts) {
|
||||
if (corner.X() != 0_m) {
|
||||
cornersPlanar = false;
|
||||
}
|
||||
}
|
||||
isPlanar = cornersPlanar;
|
||||
}
|
||||
vertices = verts;
|
||||
}
|
||||
|
||||
std::vector<frc::Translation3d> GetFieldVertices(
|
||||
const frc::Pose3d& targetPose) const {
|
||||
RotTrlTransform3d basisChange{targetPose.Rotation(),
|
||||
targetPose.Translation()};
|
||||
std::vector<frc::Translation3d> retVal;
|
||||
retVal.reserve(vertices.size());
|
||||
for (const auto& vert : vertices) {
|
||||
retVal.emplace_back(basisChange.Apply(vert));
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
static frc::Pose3d GetOrientedPose(const frc::Translation3d& tgtTrl,
|
||||
const frc::Translation3d& cameraTrl) {
|
||||
frc::Translation3d relCam = cameraTrl - tgtTrl;
|
||||
frc::Rotation3d orientToCam = frc::Rotation3d{
|
||||
0_rad,
|
||||
frc::Rotation2d{units::math::hypot(relCam.X(), relCam.Y()).to<double>(),
|
||||
-relCam.Z().to<double>()}
|
||||
.Radians(),
|
||||
frc::Rotation2d{relCam.X().to<double>(), relCam.Y().to<double>()}
|
||||
.Radians()};
|
||||
return frc::Pose3d{tgtTrl, orientToCam};
|
||||
}
|
||||
|
||||
std::vector<frc::Translation3d> GetVertices() const { return vertices; }
|
||||
bool GetIsPlanar() const { return isPlanar; }
|
||||
bool GetIsSpherical() const { return isSpherical; }
|
||||
|
||||
private:
|
||||
std::vector<frc::Translation3d> vertices;
|
||||
bool isPlanar;
|
||||
bool isSpherical;
|
||||
};
|
||||
|
||||
static const TargetModel kAprilTag16h5{6_in, 6_in};
|
||||
static const TargetModel kAprilTag36h11{6.5_in, 6.5_in};
|
||||
} // namespace photon
|
||||
@@ -0,0 +1,121 @@
|
||||
/*
|
||||
* Copyright (C) Photon Vision.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <frc/apriltag/AprilTag.h>
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
|
||||
#include "OpenCVHelp.h"
|
||||
#include "TargetModel.h"
|
||||
#include "photon/targeting/MultiTargetPNPResult.h"
|
||||
#include "photon/targeting/PhotonTrackedTarget.h"
|
||||
|
||||
namespace photon {
|
||||
namespace VisionEstimation {
|
||||
|
||||
static std::vector<frc::AprilTag> GetVisibleLayoutTags(
|
||||
const std::vector<PhotonTrackedTarget>& visTags,
|
||||
const frc::AprilTagFieldLayout& layout) {
|
||||
std::vector<frc::AprilTag> retVal{};
|
||||
for (const auto& tag : visTags) {
|
||||
int id = tag.GetFiducialId();
|
||||
auto maybePose = layout.GetTagPose(id);
|
||||
if (maybePose) {
|
||||
retVal.emplace_back(frc::AprilTag{id, maybePose.value()});
|
||||
}
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
static PNPResult EstimateCamPosePNP(
|
||||
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
|
||||
const Eigen::Matrix<double, 5, 1>& distCoeffs,
|
||||
const std::vector<PhotonTrackedTarget>& visTags,
|
||||
const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) {
|
||||
if (visTags.size() == 0) {
|
||||
return PNPResult();
|
||||
}
|
||||
|
||||
std::vector<std::pair<float, float>> corners{};
|
||||
std::vector<frc::AprilTag> knownTags{};
|
||||
|
||||
for (const auto& tgt : visTags) {
|
||||
int id = tgt.GetFiducialId();
|
||||
auto maybePose = layout.GetTagPose(id);
|
||||
if (maybePose) {
|
||||
knownTags.emplace_back(frc::AprilTag{id, maybePose.value()});
|
||||
auto currentCorners = tgt.GetDetectedCorners();
|
||||
corners.insert(corners.end(), currentCorners.begin(),
|
||||
currentCorners.end());
|
||||
}
|
||||
}
|
||||
if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
|
||||
return PNPResult{};
|
||||
}
|
||||
|
||||
std::vector<cv::Point2f> points = OpenCVHelp::CornersToPoints(corners);
|
||||
|
||||
if (knownTags.size() == 1) {
|
||||
PNPResult camToTag = OpenCVHelp::SolvePNP_Square(
|
||||
cameraMatrix, distCoeffs, tagModel.GetVertices(), points);
|
||||
if (!camToTag.isPresent) {
|
||||
return PNPResult{};
|
||||
}
|
||||
frc::Pose3d bestPose =
|
||||
knownTags[0].pose.TransformBy(camToTag.best.Inverse());
|
||||
frc::Pose3d altPose{};
|
||||
if (camToTag.ambiguity != 0) {
|
||||
altPose = knownTags[0].pose.TransformBy(camToTag.alt.Inverse());
|
||||
}
|
||||
frc::Pose3d o{};
|
||||
PNPResult result{};
|
||||
result.best = frc::Transform3d{o, bestPose};
|
||||
result.alt = frc::Transform3d{o, altPose};
|
||||
result.ambiguity = camToTag.ambiguity;
|
||||
result.bestReprojErr = camToTag.bestReprojErr;
|
||||
result.altReprojErr = camToTag.altReprojErr;
|
||||
return result;
|
||||
} else {
|
||||
std::vector<frc::Translation3d> objectTrls{};
|
||||
for (const auto& tag : knownTags) {
|
||||
auto verts = tagModel.GetFieldVertices(tag.pose);
|
||||
objectTrls.insert(objectTrls.end(), verts.begin(), verts.end());
|
||||
}
|
||||
PNPResult camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs,
|
||||
objectTrls, points);
|
||||
if (!camToOrigin.isPresent) {
|
||||
return PNPResult{};
|
||||
} else {
|
||||
PNPResult result{};
|
||||
result.best = camToOrigin.best.Inverse(),
|
||||
result.alt = camToOrigin.alt.Inverse(),
|
||||
result.ambiguity = camToOrigin.ambiguity;
|
||||
result.bestReprojErr = camToOrigin.bestReprojErr;
|
||||
result.altReprojErr = camToOrigin.altReprojErr;
|
||||
result.isPresent = true;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace VisionEstimation
|
||||
} // namespace photon
|
||||
Reference in New Issue
Block a user