diff --git a/:w b/:w new file mode 100644 index 0000000..61e09e9 --- /dev/null +++ b/:w @@ -0,0 +1,238 @@ +#include +#include +#include "src/camera/camera_constants.h" +#include "src/utils/camera_utils.h" +#include "src/utils/intrinsics_from_json.h" +#include "src/utils/log.h" +#include "src/utils/pch.h" + +// https://firstfrc.blob.core.windows.net/frc2026/FieldAssets/2026-apriltag-images-user-guide.pdf +// Big tag size = close to tag +// Small tag size = far away from tag +constexpr double ktag_size = 0.1651; // meters +const frc::AprilTagFieldLayout kapriltag_layout = + frc::AprilTagFieldLayout("/bos/constants/2026-rebuilt-andymark.json"); +constexpr int kimage_tag_size = 25; +constexpr int ktag_id = 26; +constexpr int ktag_offset_y = 0; +constexpr int ktag_offset_x = -10; + +void decomposeMatrix(const cv::Mat& T, cv::Mat& rvec, cv::Mat& tvec) { + // Check that the matrix is 4x4 + if (T.rows != 4 || T.cols != 4) { + throw std::runtime_error("Input matrix must be 4x4"); + } + + // Extract translation vector + tvec = (cv::Mat_(3, 1) << T.at(0, 3), T.at(1, 3), + T.at(2, 3)); + + // Extract rotation matrix + cv::Mat R = T(cv::Range(0, 3), cv::Range(0, 3)); + + // Convert rotation matrix to rotation vector + cv::Rodrigues(R, rvec); +} + +auto cvPoseToEigen(cv::Mat pose) -> Eigen::MatrixX4d { + Eigen::Matrix4d eigen; + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + eigen(r, c) = pose.at(r, c); + } + } + return eigen; +} + +auto inverse(cv::Mat& rvec, cv::Mat& tvec) { + cv::Mat R; + cv::Rodrigues(rvec, R); + + cv::Mat R_inv = R.t(); // transpose = inverse for rotation matrix + // Invert the rotation + + // Invert the translation + tvec = -R_inv * tvec; + + // Convert back to rvec if needed + cv::Mat rvec_inv; + cv::Rodrigues(R_inv, rvec); +} + +auto createTransformMatrix(double rx, double ry, double rz, double tx, + double ty, double tz) -> cv::Mat { + // --- Rotation matrices --- + cv::Mat Rx = (cv::Mat_(3, 3) << 1, 0, 0, 0, cos(rx), -sin(rx), 0, + sin(rx), cos(rx)); + + cv::Mat Ry = (cv::Mat_(3, 3) << cos(ry), 0, sin(ry), 0, 1, 0, + -sin(ry), 0, cos(ry)); + + cv::Mat Rz = (cv::Mat_(3, 3) << cos(rz), -sin(rz), 0, sin(rz), + cos(rz), 0, 0, 0, 1); + + // Combined rotation: Z * Y * X + cv::Mat R = Rz * Ry * Rx; + + // --- Translation vector --- + cv::Mat t = (cv::Mat_(3, 1) << tx, ty, tz); + + // --- 4x4 Homogeneous transformation matrix --- + cv::Mat T = cv::Mat::eye(4, 4, CV_64F); // initialize identity + R.copyTo(T(cv::Rect(0, 0, 3, 3))); // top-left 3x3 rotation + t.copyTo(T(cv::Rect(3, 0, 1, 3))); // top-right 3x1 translation + + return T; +} + +auto createTransformMatrix(frc::Pose3d transform) -> cv::Mat { + return createTransformMatrix( + transform.Rotation().Y().to(), + transform.Rotation().Z().to(), + transform.Rotation().X().to(), transform.Y().to(), + transform.Z().to(), transform.X().to()); +} + +auto createTransformMatrix(frc::Transform3d transform) -> cv::Mat { + return createTransformMatrix( + transform.Rotation().Y().to(), + transform.Rotation().Z().to(), + transform.Rotation().X().to(), transform.Y().to(), + transform.Z().to(), transform.X().to()); +} + +auto createTransformMatrix(cv::Mat rvec, cv::Mat tvec) -> cv::Mat { + return createTransformMatrix(rvec.at(0), rvec.at(1), + rvec.at(2), tvec.at(0), + tvec.at(1), tvec.at(2)); +} + +auto main() -> int { + std::vector apriltag_corners = { + {-ktag_size / 2, ktag_size / 2, 0}, + {ktag_size / 2, ktag_size / 2, 0}, + {ktag_size / 2, -ktag_size / 2, 0}, + {-ktag_size / 2, -ktag_size / 2, 0}}; + + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F); // output rotation vector + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64F); // output translation vector + auto config = camera::Camera::STOVETOP_BOT_FRONT_RIGHT; + auto camera_matrix = utils::camera_matrix_from_json( + utils::read_intrinsics(camera::camera_constants[config].intrinsics_path)); + + auto distortion_coefficients = + utils::distortion_coefficients_from_json(utils::read_intrinsics( + camera::camera_constants[config].intrinsics_path)); + + std::vector image_points = { + cv::Point2f(1000 - kimage_tag_size + ktag_offset_x, + 500 + kimage_tag_size + ktag_offset_y), + cv::Point2f(1000 + kimage_tag_size + ktag_offset_x, + 500 + kimage_tag_size + ktag_offset_y), + cv::Point2f(1000 + kimage_tag_size + ktag_offset_x, + 500 - kimage_tag_size + ktag_offset_y), + cv::Point2f(1000 - kimage_tag_size + ktag_offset_x, + 500 - kimage_tag_size + ktag_offset_y), + }; + + auto tag_pose = kapriltag_layout.GetTagPose(ktag_id).value().RelativeTo({}); + auto feild_to_tag = createTransformMatrix(frc::Transform3d({}, tag_pose)); + + for (auto& apriltag_corner : apriltag_corners) { + (void)apriltag_corner; + apriltag_corner.x = -apriltag_corner.x; + // apriltag_corner.y = -apriltag_corner.y; + // + // cv::Mat pt = (cv::Mat_(4, 1) << apriltag_corner.x, + // apriltag_corner.y, apriltag_corner.z, 1.0); + // cv::Mat result = feild_to_tag * pt; + // apriltag_corner = cv::Point3d(result.at(0), result.at(1), + // result.at(2)); + } + + // [886.039, 530.664][1088.88, 521.248][1074.46, 333.045][880.557, 333.923] + cv::solvePnP(apriltag_corners, image_points, camera_matrix, + distortion_coefficients, rvec, tvec, false, + cv::SOLVEPNP_ITERATIVE); + + LOG(INFO) << "\n" << rvec; + LOG(INFO) << "\n" << tvec; + + std::vector projectedPoints; + cv::projectPoints(apriltag_corners, rvec, tvec, camera_matrix, + distortion_coefficients, projectedPoints); + + // Compute reprojection error + double totalError = 0.0; + for (size_t i = 0; i < image_points.size(); i++) { + double err = cv::norm(image_points[i] - projectedPoints[i]); + totalError += err * err; + } + + double meanError = std::sqrt(totalError / image_points.size()); + std::cout << "Mean reprojection error: " << meanError << " pixels" + << std::endl; + + { + // inverse(rvec, tvec); + // const double translation_x = tvec.ptr()[2]; + // const double translation_y = tvec.ptr()[0]; + // const double translation_z = tvec.ptr()[1]; + // + // const double rotation_x = rvec.ptr()[2]; + // const double rotation_y = rvec.ptr()[0]; + // const double rotation_z = rvec.ptr()[1]; + // frc::Pose3d camera_pose( + // units::meter_t{translation_x}, units::meter_t{translation_y}, + // units::meter_t{translation_z}, + // frc::Rotation3d(units::radian_t{rotation_x}, units::radian_t{rotation_y}, + // units::radian_t{rotation_z})); + // PrintPose3d(camera_pose); + } + + { + auto camera_to_tag = createTransformMatrix(rvec, tvec); + auto tag_to_camera = camera_to_tag.inv(); + + auto tag_pose = kapriltag_layout.GetTagPose(ktag_id).value(); + auto feild_to_tag = createTransformMatrix(tag_pose); + auto tag_to_feild = createTransformMatrix({tag_pose, {}}); + std::cout << tag_to_feild; + return 0; + + auto rotaion_matrix = + createTransformMatrix(0, std::numbers::pi, 0, 0, 0, 0); + std::cout << "rotation matrix\n"; + std::cout << rotaion_matrix << std::endl; + + std::cout << "tag to camera\n"; + std::cout << tag_to_camera << std::endl; + + std::cout << "feild to tag\n"; + std::cout << feild_to_tag << std::endl; + + auto feild_to_camera = feild_to_tag * tag_to_camera; + std::cout << "feild to camera\n"; + std::cout << feild_to_camera << std::endl; + + decomposeMatrix(feild_to_camera, rvec, tvec); + + const double translation_x = tvec.ptr()[2]; + const double translation_y = tvec.ptr()[0]; + const double translation_z = tvec.ptr()[1]; + + const double rotation_x = rvec.ptr()[2]; + const double rotation_y = rvec.ptr()[0]; + const double rotation_z = rvec.ptr()[1]; + frc::Pose3d camera_pose(units::meter_t{translation_x}, + units::meter_t{-translation_y}, + units::meter_t{translation_z}, + frc::Rotation3d(units::radian_t{rotation_x}, + units::radian_t{rotation_y}, + units::radian_t{rotation_z})); + std::cout << "Camera pose\n"; + PrintPose3d(camera_pose); + } + + return 0; +} diff --git a/constants/dev_orin_extrinsics.json b/constants/dev_orin_extrinsics.json new file mode 100644 index 0000000..a4709fa --- /dev/null +++ b/constants/dev_orin_extrinsics.json @@ -0,0 +1,8 @@ +{ + "translation_x": 0, + "translation_y": 0, + "translation_z": 0.7493, + "rotation_x": 0, + "rotation_y": 0, + "rotation_z": 0 +} diff --git a/constants/dev_orin_intrinsics.json b/constants/dev_orin_intrinsics.json new file mode 100644 index 0000000..3fd9e98 --- /dev/null +++ b/constants/dev_orin_intrinsics.json @@ -0,0 +1,12 @@ +{ + "cx": 625.3426025032783, + "cy": 372.4797842477203, + "fx": 898.0928868060495, + "fy": 897.7157683218877, + "k1": 0.04705864839856624, + "k2": -0.08074506402566872, + "k3": 0.01743537811199019, + "p1": 0.001136572471268671, + "p2": -0.0003280265291867128 +} + diff --git a/constants/stovetop_bot/front_right_intrinsics.json b/constants/stovetop_bot/front_right_intrinsics.json index 1a4bd0d..4601bda 100644 --- a/constants/stovetop_bot/front_right_intrinsics.json +++ b/constants/stovetop_bot/front_right_intrinsics.json @@ -1,12 +1,12 @@ { - "cx": 1012.0165186318737, - "cy": 509.96268009601044, - "fx": 598.8562147696354, - "fy": 598.7791606886024, - "k1": 0.01265280096846185, - "k2": -0.02749231422823394, - "k3": 0.004217234226796008, - "p1": 6.344713648157099e-05, - "p2": -0.0015890075748726767 + "cx": 1000, + "cy": 500, + "fx": 598, + "fy": 598, + "k1": 0, + "k2": 0, + "k3": 0, + "p1": 0, + "p2": 0 } diff --git a/src/camera/camera_constants.h b/src/camera/camera_constants.h index 680df89..dba1ffc 100644 --- a/src/camera/camera_constants.h +++ b/src/camera/camera_constants.h @@ -56,6 +56,8 @@ enum Camera { STOVETOP_BOT_BACK_RIGHT, STOVETOP_BOT_BACK_LEFT, + DEV_ORIN, + DEFAULT_USB0, REALSENSE, CAMERA_LENGTH, @@ -186,6 +188,15 @@ inline const camera_constant_t camera_constants[CAMERA_LENGTH] = { .intrinsics_path = "/bos/constants/vision_bot_back_left_intrinsics.json", .extrinsics_path = "/bos/constants/vision_bot_back_left_extrinsics.json", .name = "vision_bot_back_left"}, + [Camera::DEV_ORIN] = + camera_constant_t{ + .pipeline = "/dev/v4l/by-path/" + "platform-3610000.usb-usb-0:2.1:1.0-video-index0", + .intrinsics_path = + "/bos/constants/dev_orin_intrinsics.json", + .extrinsics_path = + "/bos/constants/dev_orin_extrinsics.json", + .name = "dev_orin"}, [Camera::DEFAULT_USB0] = camera_constant_t{ .pipeline = "platform-3610000.usb-usb-0:2.2:1.0-video-index0", diff --git a/src/camera/cv_camera.cc b/src/camera/cv_camera.cc index ae5a39b..7c50d38 100644 --- a/src/camera/cv_camera.cc +++ b/src/camera/cv_camera.cc @@ -5,7 +5,6 @@ namespace camera { CVCamera::CVCamera(const CameraConstant& c) : cap_(cv::VideoCapture(c.pipeline)) { - auto set_if = [&](int prop, const auto& opt) { if (opt) { cap_.set(prop, *opt); diff --git a/src/localization/gpu_apriltag_detector.cc b/src/localization/gpu_apriltag_detector.cc index 770b4c4..3d3fff1 100644 --- a/src/localization/gpu_apriltag_detector.cc +++ b/src/localization/gpu_apriltag_detector.cc @@ -15,9 +15,9 @@ constexpr auto RadianToDegree(double radian) -> double { GPUAprilTagDetector::GPUAprilTagDetector(uint image_width, uint image_height, const nlohmann::json& intrinsics, bool verbose) - : camera_matrix_(camera_matrix_from_json(intrinsics)), + : camera_matrix_(utils::camera_matrix_from_json(intrinsics)), distortion_coefficients_( - distortion_coefficients_from_json(intrinsics)) { + utils::distortion_coefficients_from_json(intrinsics)) { apriltag_detector_ = apriltag_detector_create(); @@ -30,8 +30,9 @@ GPUAprilTagDetector::GPUAprilTagDetector(uint image_width, uint image_height, gpu_detector_ = std::make_unique( image_width, image_height, apriltag_detector_, - camera_matrix_from_json(intrinsics), - distortion_coefficients_from_json( + utils::camera_matrix_from_json( + intrinsics), + utils::distortion_coefficients_from_json( intrinsics)); } auto GPUAprilTagDetector::GetTagDetections( diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index e2bfaf0..8a4fd43 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -2,6 +2,7 @@ #include #include #include "src/utils/camera_utils.h" +#include "src/utils/extrinsics_from_json.h" #include "src/utils/intrinsics_from_json.h" namespace localization { @@ -13,63 +14,158 @@ static const Eigen::MatrixXd PI = []() { return pi; }(); +auto cvPoseToEigen(cv::Mat pose) -> Eigen::MatrixX4d { + Eigen::Matrix4d eigen; + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + eigen(r, c) = pose.at(r, c); + } + } + return eigen; +} + +auto inverse(cv::Mat& rvec, cv::Mat& tvec) { + cv::Mat R; + cv::Rodrigues(rvec, R); + + cv::Mat R_inv = R.t(); + tvec = -R_inv * tvec; + + cv::Mat rvec_inv; + cv::Rodrigues(R_inv, rvec); +} + +auto createTransformMatrix(double rx, double ry, double rz, double tx, + double ty, double tz) -> cv::Mat { + // --- Rotation matrices --- + cv::Mat Rx = (cv::Mat_(3, 3) << 1, 0, 0, 0, cos(rx), -sin(rx), 0, + sin(rx), cos(rx)); + + cv::Mat Ry = (cv::Mat_(3, 3) << cos(ry), 0, sin(ry), 0, 1, 0, + -sin(ry), 0, cos(ry)); + + cv::Mat Rz = (cv::Mat_(3, 3) << cos(rz), -sin(rz), 0, sin(rz), + cos(rz), 0, 0, 0, 1); + + // Combined rotation: Z * Y * X + cv::Mat R = Rz * Ry * Rx; + + // --- Translation vector --- + cv::Mat t = (cv::Mat_(3, 1) << tx, ty, tz); + + // --- 4x4 Homogeneous transformation matrix --- + cv::Mat T = cv::Mat::eye(4, 4, CV_64F); // initialize identity + R.copyTo(T(cv::Rect(0, 0, 3, 3))); // top-left 3x3 rotation + t.copyTo(T(cv::Rect(3, 0, 1, 3))); // top-right 3x1 translation + + return T; +} + +auto createTransformMatrix(frc::Pose3d transform) -> cv::Mat { + return createTransformMatrix( + transform.Rotation().Y().to(), + transform.Rotation().Z().to(), + transform.Rotation().X().to(), transform.Y().to(), + transform.Z().to(), transform.X().to()); +} + +auto createTransformMatrix(cv::Mat rvec, cv::Mat tvec) -> cv::Mat { + return createTransformMatrix(rvec.at(0), rvec.at(1), + rvec.at(2), tvec.at(0), + tvec.at(1), tvec.at(2)); +} + JointSolver::JointSolver(const std::string& intrinsics_path, const std::string& extrinsics_path, AprilTagFieldLayout layout, double tag_size) : layout_(std::move(layout)), - camera_matrix_(camera_matrix_from_json( + camera_matrix_(utils::camera_matrix_from_json( utils::read_intrinsics(intrinsics_path))), - initial_solver_(utils::read_extrinsics(extrinsics_path)) {} + distortion_coefficients_( + utils::distortion_coefficients_from_json( + utils::read_intrinsics(intrinsics_path))), + camera_to_robot_(utils::ExtrinsicsJsonToCameraToRobot( + utils::read_extrinsics(extrinsics_path))) {} JointSolver::JointSolver(camera::Camera camera_config, const frc::AprilTagFieldLayout& layout, double tag_size) : JointSolver(camera::camera_constants[camera_config].intrinsics_path, camera::camera_constants[camera_config].extrinsics_path, - layout, tag_size) {} + layout, tag_size) { + for (const auto& tag : layout.GetTags()) { + // TODO document + std::vector absolute_tag_corners; + absolute_tag_corners.reserve(4); + frc::Pose3d tag_pose = tag.pose; + auto feild_to_tag = createTransformMatrix(tag_pose); + feild_to_tag.at(0, 3) = -feild_to_tag.at(0, 3); + feild_to_tag.at(0, 2) = -feild_to_tag.at(0, 2); + feild_to_tag.at(0, 1) = -feild_to_tag.at(0, 1); + feild_to_tag.at(0, 0) = -feild_to_tag.at(0, 0); + + for (const cv::Point3f& apriltag_corner : kapriltag_corners) { + cv::Mat tag_corner = (cv::Mat_(4, 1) << -apriltag_corner.x, + apriltag_corner.y, apriltag_corner.z, 1.0); + cv::Mat result = feild_to_tag * tag_corner; + absolute_tag_corners.push_back(cv::Point3d( + result.at(0), result.at(1), result.at(2))); + } + absolute_apriltag_corners_.insert({tag.ID, absolute_tag_corners}); + } +} auto JointSolver::EstimatePosition( const std::vector& detections) -> std::vector { - if (detections.empty()) { + std::vector apriltag_corners; + std::vector image_points; + for (auto& detection : detections) { + auto apriltag_corner = absolute_apriltag_corners_.find(detection.tag_id); + if (apriltag_corner == absolute_apriltag_corners_.end()) { + LOG(WARNING) << "Invalid tag id: " << detection.tag_id; + continue; + } + apriltag_corners.insert(apriltag_corners.end(), + apriltag_corner->second.begin(), + apriltag_corner->second.end()); + image_points.insert(image_points.end(), detection.corners.begin(), + detection.corners.end()); + } + + if (apriltag_corners.empty()) { + // Need at least one tag return {}; } - position_estimate_t initial_position_estimate = - initial_solver_.EstimatePosition(detections)[0]; - initial_position_estimate.pose.ToMatrix(); + // detections should all have the same timestamp (if they come from the same camera) + const double timestamp = detections[0].timestamp; - // initilaize w - std::vector w(detections.size()); - std::array tag_corners; - for (const auto& detection : detections) { - auto feild_to_tag = layout_.GetTagPose(detection.tag_id)->ToMatrix(); - w.emplace_back(camera_matrix_ * PI * feild_to_tag); - } - Eigen::Matrix4d A = initial_position_estimate.pose.ToMatrix(); - for (size_t i = 0; i < detections.size(); i++) { - for (int j = 0; j < 4; j++) { - Eigen::Vector3d projected_points = w[i] * A * tag_corners[j]; + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64F); + cv::solvePnP(apriltag_corners, image_points, camera_matrix_, + distortion_coefficients_, rvec, tvec, false, + cv::SOLVEPNP_ITERATIVE); - const double u = projected_points[0]; - const double v = projected_points[1]; - const double lambda = projected_points[2]; + inverse(rvec, tvec); - const float u_true = detections[i].corners[j].x; - const float v_true = detections[i].corners[j].y; + const double translation_x = tvec.ptr()[2]; + const double translation_y = tvec.ptr()[0]; + const double translation_z = tvec.ptr()[1]; - Eigen::Vector3d dprojected_points( - (u / lambda - u_true) / lambda, (v / lambda - v_true) / lambda, - -(u / lambda - u_true) / (lambda * lambda) - - (v / lambda - v_true) / (lambda * lambda)); + const double rotation_x = rvec.ptr()[2]; + const double rotation_y = -rvec.ptr()[0]; + const double rotation_z = rvec.ptr()[1]; - auto dA = w[i] * dprojected_points; + frc::Pose3d camera_pose( + units::meter_t{translation_x}, units::meter_t{-translation_y}, + units::meter_t{translation_z}, + frc::Rotation3d(units::radian_t{rotation_x}, units::radian_t{rotation_y}, + units::radian_t{rotation_z})); - LOG(INFO) << "projected points " << projected_points; - LOG(INFO) << "dA " << dA; - } - } - return {}; + frc::Pose3d robot_pose = camera_pose.TransformBy(camera_to_robot_); + return {position_estimate_t{ + .pose = robot_pose, .distance = 0, .timestamp = timestamp}}; } } // namespace localization diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index d2943cc..325c799 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -23,7 +23,9 @@ class JointSolver : public IPositionSolver { private: frc::AprilTagFieldLayout layout_; - Eigen::MatrixXd camera_matrix_; - SquareSolver initial_solver_; + cv::Mat camera_matrix_; + cv::Mat distortion_coefficients_; + frc::Transform3d camera_to_robot_; + std::unordered_map> absolute_apriltag_corners_; }; } // namespace localization diff --git a/src/localization/position.h b/src/localization/position.h index beaf805..e34f277 100644 --- a/src/localization/position.h +++ b/src/localization/position.h @@ -12,7 +12,7 @@ using point3d_t = struct Point3d { using tag_detection_t = struct TagDetection { int tag_id; - std::array corners; // Image coordinates of tag corners + std::array corners; // Image coordinates of tag corners double timestamp; double confidence; }; diff --git a/src/localization/square_solver.cc b/src/localization/square_solver.cc index 1afdf65..7d3f9f8 100644 --- a/src/localization/square_solver.cc +++ b/src/localization/square_solver.cc @@ -1,35 +1,25 @@ #include "src/localization/square_solver.h" #include +#include #include #include "src/utils/camera_utils.h" +#include "src/utils/extrinsics_from_json.h" #include "src/utils/intrinsics_from_json.h" namespace localization { -auto ExtrinsicsJsonToCameraToRobot(nlohmann::json extrinsics_json) - -> frc::Transform3d { - frc::Pose3d camera_pose( - units::meter_t{extrinsics_json["translation_x"]}, - units::meter_t{extrinsics_json["translation_y"]}, - units::meter_t{extrinsics_json["translation_z"]}, - frc::Rotation3d(units::radian_t{extrinsics_json["rotation_x"]}, - units::radian_t{extrinsics_json["rotation_y"]}, - units::radian_t{extrinsics_json["rotation_z"]})); - frc::Transform3d robot_to_camera(frc::Pose3d(), camera_pose); - return robot_to_camera.Inverse(); -} - SquareSolver::SquareSolver(const std::string& intrinsics_path, const std::string& extrinsics_path, frc::AprilTagFieldLayout layout, std::vector tag_corners) : layout_(std::move(layout)), tag_corners_(std::move(tag_corners)), - camera_matrix_(camera_matrix_from_json( + camera_matrix_(utils::camera_matrix_from_json( utils::read_intrinsics(intrinsics_path))), - distortion_coefficients_(distortion_coefficients_from_json( - utils::read_intrinsics(intrinsics_path))), - camera_to_robot_(ExtrinsicsJsonToCameraToRobot( + distortion_coefficients_( + utils::distortion_coefficients_from_json( + utils::read_intrinsics(intrinsics_path))), + camera_to_robot_(utils::ExtrinsicsJsonToCameraToRobot( utils::read_extrinsics(extrinsics_path))) {} SquareSolver::SquareSolver(camera::Camera camera_config, @@ -42,17 +32,16 @@ SquareSolver::SquareSolver(camera::Camera camera_config, auto SquareSolver::EstimatePosition( const std::vector& detections) -> std::vector { - // map? - std::vector position_estimates(detections.size()); + std::vector position_estimates; + position_estimates.reserve(detections.size()); + for (const auto& detection : detections) { - cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); // output rotation vector - cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); // output translation vector + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); cv::solvePnP(tag_corners_, detection.corners, camera_matrix_, distortion_coefficients_, rvec, tvec, false, cv::SOLVEPNP_IPPE_SQUARE); - // Currently we do not use transation z, rotation x and rotation y - // Converting to wpi coordinates const double translation_x = tvec.ptr()[2]; const double translation_y = tvec.ptr()[0]; const double translation_z = tvec.ptr()[1]; @@ -60,7 +49,6 @@ auto SquareSolver::EstimatePosition( const double rotation_x = rvec.ptr()[2]; const double rotation_y = rvec.ptr()[0]; const double rotation_z = rvec.ptr()[1]; - LOG(INFO) << translation_x << " " << translation_y; auto pose = frc::Pose3d(frc::Translation3d(units::meter_t{translation_x}, units::meter_t{translation_y}, @@ -77,34 +65,10 @@ auto SquareSolver::EstimatePosition( units::radian_t{-pose.Rotation().Z()} + 180_deg)); frc::Transform3d tag_to_camera = camera_to_tag.Inverse(); - frc::Pose3d tag_pose = layout_.GetTagPose(detection.tag_id).value(); - frc::Pose3d camera_pose = tag_pose.TransformBy(tag_to_camera); - frc::Pose3d robot_pose = camera_pose.TransformBy(camera_to_robot_); - // // TODO check if there is a way of doing this in a better way - // frc::Transform3d camera_to_tag( - // units::meter_t{translation_x}, units::meter_t{-translation_y}, - // units::meter_t{-translation_z}, - // frc::Rotation3d(units::radian_t{rotation_x}, - // units::radian_t{-rotation_y}, - // units::radian_t{-rotation_z} + 180_deg)); - - // frc::Transform3d tag_to_camera = camera_to_tag.Inverse(); - // PrintTransform3d(tag_to_camera); - // - // auto maybe_tag_pose = layout_.GetTagPose(detection.tag_id); - // if (!maybe_tag_pose.has_value()) { - // LOG(WARNING) << "Got invalid tag id: " << detection.tag_id; - // } - // frc::Pose3d tag_pose = maybe_tag_pose.value(); - // - // frc::Pose3d camera_pose = tag_pose.TransformBy(tag_to_camera); - // - // frc::Pose3d robot_pose = camera_pose.TransformBy(camera_to_robot_); - position_estimates.push_back({robot_pose, std::hypot(translation_x, translation_y), detection.timestamp}); @@ -112,4 +76,121 @@ auto SquareSolver::EstimatePosition( return position_estimates; } -} // namespace localization +auto SquareSolver::EstimatePositionJoint( + const std::vector& detections) + -> std::optional { + + if (detections.empty()) { + return std::nullopt; + } + + std::vector camera_poses; + double avg_timestamp = 0.0; + + for (const auto& detection : detections) { + auto maybe_tag_pose = layout_.GetTagPose(detection.tag_id); + if (!maybe_tag_pose.has_value()) { + LOG(WARNING) << "Got invalid tag id: " << detection.tag_id; + continue; + } + + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); + cv::solvePnP(tag_corners_, detection.corners, camera_matrix_, + distortion_coefficients_, rvec, tvec, false, + cv::SOLVEPNP_IPPE_SQUARE); + + const double translation_x = tvec.ptr()[2]; + const double translation_y = tvec.ptr()[0]; + const double translation_z = tvec.ptr()[1]; + + const double rotation_x = rvec.ptr()[2]; + const double rotation_y = rvec.ptr()[0]; + const double rotation_z = rvec.ptr()[1]; + + auto pose = frc::Pose3d(frc::Translation3d(units::meter_t{translation_x}, + units::meter_t{translation_y}, + units::meter_t{translation_z}), + frc::Rotation3d(units::radian_t{rotation_x}, + units::radian_t{rotation_y}, + units::radian_t{rotation_z})); + + frc::Transform3d camera_to_tag( + units::meter_t{pose.X()}, units::meter_t{-pose.Y()}, + units::meter_t{-pose.Z()}, + frc::Rotation3d(units::radian_t{pose.Rotation().X()}, + units::radian_t{-pose.Rotation().Y()}, + units::radian_t{-pose.Rotation().Z()} + 180_deg)); + + frc::Transform3d tag_to_camera = camera_to_tag.Inverse(); + frc::Pose3d camera_pose = maybe_tag_pose.value().TransformBy(tag_to_camera); + + camera_poses.push_back(camera_pose); + avg_timestamp += detection.timestamp; + } + + if (camera_poses.empty()) { + return std::nullopt; + } + + avg_timestamp /= detections.size(); + + // Average translation + double avg_x = 0, avg_y = 0, avg_z = 0; + for (const auto& cp : camera_poses) { + avg_x += cp.X().value(); + avg_y += cp.Y().value(); + avg_z += cp.Z().value(); + } + avg_x /= camera_poses.size(); + avg_y /= camera_poses.size(); + avg_z /= camera_poses.size(); + + // Average rotation using quaternions + double qw = 0, qx = 0, qy = 0, qz = 0; + for (const auto& cp : camera_poses) { + auto q = cp.Rotation().GetQuaternion(); + if (qw * q.W() + qx * q.X() + qy * q.Y() + qz * q.Z() < 0) { + qw -= q.W(); + qx -= q.X(); + qy -= q.Y(); + qz -= q.Z(); + } else { + qw += q.W(); + qx += q.X(); + qy += q.Y(); + qz += q.Z(); + } + } + + // Normalize averaged quaternion + double norm = std::sqrt(qw*qw + qx*qx + qy*qy + qz*qz); + qw /= norm; + qx /= norm; + qy /= norm; + qz /= norm; + + frc::Pose3d camera_pose( + frc::Translation3d(units::meter_t{avg_x}, units::meter_t{avg_y}, units::meter_t{avg_z}), + frc::Rotation3d(frc::Quaternion(qw, qx, qy, qz)) + ); + + frc::Pose3d robot_pose = camera_pose.TransformBy(camera_to_robot_); + + // Calculate average distance to detected tags + double total_distance = 0.0; + for (const auto& detection : detections) { + auto tag_pose = layout_.GetTagPose(detection.tag_id); + if (tag_pose.has_value()) { + double dx = camera_pose.X().value() - tag_pose->X().value(); + double dy = camera_pose.Y().value() - tag_pose->Y().value(); + double dz = camera_pose.Z().value() - tag_pose->Z().value(); + total_distance += std::sqrt(dx*dx + dy*dy + dz*dz); + } + } + double avg_distance = total_distance / camera_poses.size(); + + return position_estimate_t{robot_pose, avg_distance, avg_timestamp}; +} + +} // namespace localization \ No newline at end of file diff --git a/src/localization/square_solver.h b/src/localization/square_solver.h index e67da86..d2b40fe 100644 --- a/src/localization/square_solver.h +++ b/src/localization/square_solver.h @@ -19,6 +19,9 @@ class SquareSolver : public IPositionSolver { std::vector tag_corners = kapriltag_corners); auto EstimatePosition(const std::vector& detections) -> std::vector override; + + auto EstimatePositionJoint(const std::vector& detections) + -> std::optional; private: frc::AprilTagFieldLayout layout_; diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index fea531d..e4db239 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -28,3 +28,8 @@ target_link_libraries(gamepiece_test_no_img PRIVATE gamepiece yolo camera utils add_executable(absl_flag_test absl_flag_test.cc) target_link_libraries(absl_flag_test absl::flags absl::flags_parse) +add_executable(joint_solve_test joint_solve_test.cc) +target_link_libraries(joint_solve_test absl::flags absl::flags_parse utils wpilibc apriltag) + +add_executable(joint_solve_test_uncooked joint_solver_test_uncooked.cc) +target_link_libraries(joint_solve_test_uncooked localization absl::flags absl::flags_parse utils wpilibc apriltag) diff --git a/src/test/joint_solve_test.cc b/src/test/joint_solve_test.cc new file mode 100644 index 0000000..41bac13 --- /dev/null +++ b/src/test/joint_solve_test.cc @@ -0,0 +1,242 @@ +#include +#include +#include "src/camera/camera_constants.h" +#include "src/utils/camera_utils.h" +#include "src/utils/intrinsics_from_json.h" +#include "src/utils/log.h" +#include "src/utils/pch.h" + +// https://firstfrc.blob.core.windows.net/frc2026/FieldAssets/2026-apriltag-images-user-guide.pdf +// Big tag size = close to tag +// Small tag size = far away from tag +constexpr double ktag_size = 0.1651; // meters +const frc::AprilTagFieldLayout kapriltag_layout = + frc::AprilTagFieldLayout("/bos/constants/2026-rebuilt-andymark.json"); +constexpr int kimage_tag_size = 50; +constexpr int ktag_id = 19; +constexpr int ktag_offset_y = 0; +constexpr int ktag_offset_x = -10; + +void decomposeMatrix(const cv::Mat& T, cv::Mat& rvec, cv::Mat& tvec) { + // Check that the matrix is 4x4 + if (T.rows != 4 || T.cols != 4) { + throw std::runtime_error("Input matrix must be 4x4"); + } + + // Extract translation vector + tvec = (cv::Mat_(3, 1) << T.at(0, 3), T.at(1, 3), + T.at(2, 3)); + + // Extract rotation matrix + cv::Mat R = T(cv::Range(0, 3), cv::Range(0, 3)); + + // Convert rotation matrix to rotation vector + cv::Rodrigues(R, rvec); +} + +auto cvPoseToEigen(cv::Mat pose) -> Eigen::MatrixX4d { + Eigen::Matrix4d eigen; + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + eigen(r, c) = pose.at(r, c); + } + } + return eigen; +} + +auto inverse(cv::Mat& rvec, cv::Mat& tvec) { + cv::Mat R; + cv::Rodrigues(rvec, R); + + cv::Mat R_inv = R.t(); // transpose = inverse for rotation matrix + // Invert the rotation + + // Invert the translation + tvec = -R_inv * tvec; + + // Convert back to rvec if needed + cv::Mat rvec_inv; + cv::Rodrigues(R_inv, rvec); +} + +auto createTransformMatrix(double rx, double ry, double rz, double tx, + double ty, double tz) -> cv::Mat { + // --- Rotation matrices --- + cv::Mat Rx = (cv::Mat_(3, 3) << 1, 0, 0, 0, cos(rx), -sin(rx), 0, + sin(rx), cos(rx)); + + cv::Mat Ry = (cv::Mat_(3, 3) << cos(ry), 0, sin(ry), 0, 1, 0, + -sin(ry), 0, cos(ry)); + + cv::Mat Rz = (cv::Mat_(3, 3) << cos(rz), -sin(rz), 0, sin(rz), + cos(rz), 0, 0, 0, 1); + + // Combined rotation: Z * Y * X + cv::Mat R = Rz * Ry * Rx; + + // --- Translation vector --- + cv::Mat t = (cv::Mat_(3, 1) << tx, ty, tz); + + // --- 4x4 Homogeneous transformation matrix --- + cv::Mat T = cv::Mat::eye(4, 4, CV_64F); // initialize identity + R.copyTo(T(cv::Rect(0, 0, 3, 3))); // top-left 3x3 rotation + t.copyTo(T(cv::Rect(3, 0, 1, 3))); // top-right 3x1 translation + + return T; +} + +auto createTransformMatrix(frc::Pose3d transform) -> cv::Mat { + return createTransformMatrix( + transform.Rotation().Y().to(), + transform.Rotation().Z().to(), + transform.Rotation().X().to(), transform.Y().to(), + transform.Z().to(), transform.X().to()); +} + +auto createTransformMatrix(frc::Transform3d transform) -> cv::Mat { + return createTransformMatrix( + transform.Rotation().Y().to(), + transform.Rotation().Z().to(), + transform.Rotation().X().to(), transform.Y().to(), + transform.Z().to(), transform.X().to()); +} + +auto createTransformMatrix(cv::Mat rvec, cv::Mat tvec) -> cv::Mat { + return createTransformMatrix(rvec.at(0), rvec.at(1), + rvec.at(2), tvec.at(0), + tvec.at(1), tvec.at(2)); +} + +auto main() -> int { + std::vector apriltag_corners = { + {-ktag_size / 2, ktag_size / 2, 0}, + {ktag_size / 2, ktag_size / 2, 0}, + {ktag_size / 2, -ktag_size / 2, 0}, + {-ktag_size / 2, -ktag_size / 2, 0}}; + + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F); // output rotation vector + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64F); // output translation vector + auto config = camera::Camera::STOVETOP_BOT_FRONT_RIGHT; + auto camera_matrix = utils::camera_matrix_from_json( + utils::read_intrinsics(camera::camera_constants[config].intrinsics_path)); + + auto distortion_coefficients = + utils::distortion_coefficients_from_json(utils::read_intrinsics( + camera::camera_constants[config].intrinsics_path)); + + std::vector image_points = { + cv::Point2f(1000 - kimage_tag_size + ktag_offset_x, + 500 + kimage_tag_size + ktag_offset_y), + cv::Point2f(1000 + kimage_tag_size + ktag_offset_x, + 500 + kimage_tag_size + ktag_offset_y), + cv::Point2f(1000 + kimage_tag_size + ktag_offset_x, + 500 - kimage_tag_size + ktag_offset_y), + cv::Point2f(1000 - kimage_tag_size + ktag_offset_x, + 500 - kimage_tag_size + ktag_offset_y), + }; + + auto tag_pose = kapriltag_layout.GetTagPose(ktag_id).value().RelativeTo({}); + auto field_to_tag = createTransformMatrix(frc::Transform3d({}, tag_pose)); + field_to_tag.at(0, 3) = -field_to_tag.at(0, 3); + field_to_tag.at(0, 2) = -field_to_tag.at(0, 2); + field_to_tag.at(0, 1) = -field_to_tag.at(0, 1); + field_to_tag.at(0, 0) = -field_to_tag.at(0, 0); + + for (auto& apriltag_corner : apriltag_corners) { + (void)apriltag_corner; + apriltag_corner.x = -apriltag_corner.x; + + cv::Mat pt = (cv::Mat_(4, 1) << apriltag_corner.x, + apriltag_corner.y, apriltag_corner.z, 1.0); + cv::Mat result = field_to_tag * pt; + apriltag_corner = cv::Point3d(result.at(0), result.at(1), + result.at(2)); + } + + // [886.039, 530.664][1088.88, 521.248][1074.46, 333.045][880.557, 333.923] + cv::solvePnP(apriltag_corners, image_points, camera_matrix, + distortion_coefficients, rvec, tvec, false, + cv::SOLVEPNP_ITERATIVE); + + LOG(INFO) << "\n" << rvec; + LOG(INFO) << "\n" << tvec; + + std::vector projectedPoints; + cv::projectPoints(apriltag_corners, rvec, tvec, camera_matrix, + distortion_coefficients, projectedPoints); + + // Compute reprojection error + double totalError = 0.0; + for (size_t i = 0; i < image_points.size(); i++) { + double err = cv::norm(image_points[i] - projectedPoints[i]); + totalError += err * err; + } + + double meanError = std::sqrt(totalError / image_points.size()); + std::cout << "Mean reprojection error: " << meanError << " pixels" + << std::endl; + + { + inverse(rvec, tvec); + const double translation_x = tvec.ptr()[2]; + const double translation_y = tvec.ptr()[0]; + const double translation_z = tvec.ptr()[1]; + + const double rotation_x = rvec.ptr()[2]; + const double rotation_y = rvec.ptr()[0]; + const double rotation_z = rvec.ptr()[1]; + frc::Pose3d camera_pose(units::meter_t{translation_x}, + units::meter_t{-translation_y}, + units::meter_t{translation_z}, + frc::Rotation3d(units::radian_t{rotation_x}, + units::radian_t{rotation_y}, + units::radian_t{rotation_z})); + PrintPose3d(camera_pose); + } + + // { + // auto camera_to_tag = createTransformMatrix(rvec, tvec); + // auto tag_to_camera = camera_to_tag.inv(); + // + // auto tag_pose = kapriltag_layout.GetTagPose(ktag_id).value(); + // auto field_to_tag = createTransformMatrix(tag_pose); + // auto tag_to_field = createTransformMatrix({tag_pose, {}}); + // tag_to_field.at(0, 3) = -tag_to_field.at(0, 3); + // field_to_tag.at(0, 3) = -field_to_tag.at(0, 3); + // + // auto rotaion_matrix = + // createTransformMatrix(0, std::numbers::pi, 0, 0, 0, 0); + // std::cout << "rotation matrix\n"; + // std::cout << rotaion_matrix << std::endl; + // + // std::cout << "tag to camera\n"; + // std::cout << tag_to_camera << std::endl; + // + // std::cout << "field to tag\n"; + // std::cout << field_to_tag << std::endl; + // + // auto field_to_camera = field_to_tag * tag_to_camera; + // std::cout << "field to camera\n"; + // std::cout << field_to_camera << std::endl; + // + // decomposeMatrix(field_to_camera, rvec, tvec); + // + // const double translation_x = tvec.ptr()[2]; + // const double translation_y = tvec.ptr()[0]; + // const double translation_z = tvec.ptr()[1]; + // + // const double rotation_x = rvec.ptr()[2]; + // const double rotation_y = rvec.ptr()[0]; + // const double rotation_z = rvec.ptr()[1]; + // frc::Pose3d camera_pose(units::meter_t{translation_x}, + // units::meter_t{-translation_y}, + // units::meter_t{translation_z}, + // frc::Rotation3d(units::radian_t{rotation_x}, + // units::radian_t{rotation_y}, + // units::radian_t{rotation_z})); + // std::cout << "Camera pose\n"; + // PrintPose3d(camera_pose); + // } + + return 0; +} diff --git a/src/test/joint_solver_test_uncooked.cc b/src/test/joint_solver_test_uncooked.cc new file mode 100644 index 0000000..2b48545 --- /dev/null +++ b/src/test/joint_solver_test_uncooked.cc @@ -0,0 +1,142 @@ +#include +#include "src/localization/square_solver.h" +#include "src/camera/camera_constants.h" + +constexpr double ktag_size = 0.1651; +const frc::AprilTagFieldLayout kapriltag_layout = + frc::AprilTagFieldLayout("/bos/constants/2026-rebuilt-andymark.json"); + +// Generate tag corners in tag's local coordinate system +auto GetTagCorners(double tag_size) -> std::vector { + double half_size = tag_size / 2.0; + return { + cv::Point3f(-half_size, half_size, 0.0f), // top-left + cv::Point3f(half_size, half_size, 0.0f), // top-right + cv::Point3f(half_size, -half_size, 0.0f), // bottom-right + cv::Point3f(-half_size, -half_size, 0.0f) // bottom-left + }; +} + +auto main(int argc, char* argv[]) -> int { + auto config = camera::Camera::STOVETOP_BOT_FRONT_RIGHT; + + std::vector tag_corners = GetTagCorners(ktag_size); + + localization::SquareSolver solver( + camera::camera_constants[config].intrinsics_path, + camera::camera_constants[config].extrinsics_path, + kapriltag_layout, + tag_corners); + + // Create multiple detections of the same tag for testing + std::vector tag_detections; + + localization::tag_detection_t detection1; + detection1.tag_id = 1; + detection1.timestamp = 0.0; + detection1.corners = { + cv::Point2f(100.0f, 100.0f), + cv::Point2f(200.0f, 100.0f), + cv::Point2f(200.0f, 200.0f), + cv::Point2f(100.0f, 200.0f) + }; + tag_detections.push_back(detection1); + + localization::tag_detection_t detection2; + detection2.tag_id = 1; + detection2.timestamp = 0.0; + detection2.corners = { + cv::Point2f(100.0f, 100.0f), + cv::Point2f(200.0f, 100.0f), + cv::Point2f(200.0f, 200.0f), + cv::Point2f(100.0f, 200.0f) + }; + tag_detections.push_back(detection2); + + localization::tag_detection_t detection3; + detection3.tag_id = 1; + detection3.timestamp = 0.0; + detection3.corners = { + cv::Point2f(100.0f, 100.0f), + cv::Point2f(200.0f, 100.0f), + cv::Point2f(200.0f, 200.0f), + cv::Point2f(100.0f, 200.0f) + }; + tag_detections.push_back(detection3); + + LOG(INFO) << "Testing SquareSolver with " << tag_detections.size() + << " detections of the same tag (ID " << detection1.tag_id << ")"; + + LOG(INFO) << "Individual tag solving"; + std::vector individual_estimates = + solver.EstimatePosition(tag_detections); + + for (size_t i = 0; i < individual_estimates.size(); ++i) { + LOG(INFO) << "Detection " << i+1 << " (Tag " << tag_detections[i].tag_id + << ") - Position: " << individual_estimates[i]; + } + + // Calculate average of individual estimates + if (!individual_estimates.empty()) { + double avg_x = 0.0, avg_y = 0.0, avg_z = 0.0; + double avg_roll = 0.0, avg_pitch = 0.0, avg_yaw = 0.0; + for (const auto& est : individual_estimates) { + avg_x += est.pose.X().value(); + avg_y += est.pose.Y().value(); + avg_z += est.pose.Z().value(); + avg_roll += est.pose.Rotation().X().value(); + avg_pitch += est.pose.Rotation().Y().value(); + avg_yaw += est.pose.Rotation().Z().value(); + } + avg_x /= individual_estimates.size(); + avg_y /= individual_estimates.size(); + avg_z /= individual_estimates.size(); + avg_roll /= individual_estimates.size(); + avg_pitch /= individual_estimates.size(); + avg_yaw /= individual_estimates.size(); + + LOG(INFO) << "Average of individual estimates:"; + LOG(INFO) << " Position: (" << avg_x << " m, " << avg_y << " m, " << avg_z << " m)"; + LOG(INFO) << " Rotation: (" << avg_roll*180/M_PI << " deg, " + << avg_pitch*180/M_PI << " deg, " << avg_yaw*180/M_PI << " deg)"; + } + + LOG(INFO) << "\nJoint Tag Solving"; + auto joint_estimate = solver.EstimatePositionJoint(tag_detections); + + if (joint_estimate.has_value()) { + LOG(INFO) << "Joint estimate: " << joint_estimate.value(); + } else { + LOG(WARNING) << "Joint solving failed!"; + } + + if (joint_estimate.has_value() && !individual_estimates.empty()) { + double avg_x = 0.0, avg_y = 0.0, avg_z = 0.0; + for (const auto& est : individual_estimates) { + avg_x += est.pose.X().value(); + avg_y += est.pose.Y().value(); + avg_z += est.pose.Z().value(); + } + avg_x /= individual_estimates.size(); + avg_y /= individual_estimates.size(); + avg_z /= individual_estimates.size(); + + double diff_x = joint_estimate->pose.X().value() - avg_x; + double diff_y = joint_estimate->pose.Y().value() - avg_y; + double diff_z = joint_estimate->pose.Z().value() - avg_z; + double diff_magnitude = std::sqrt(diff_x*diff_x + diff_y*diff_y + diff_z*diff_z); + + LOG(INFO) << "Position difference (joint vs avg individual):"; + LOG(INFO) << " Delta: (" << diff_x << " m, " << diff_y << " m, " << diff_z << " m)"; + LOG(INFO) << " Magnitude: " << diff_magnitude << " m"; + + if (diff_magnitude < 0.001) { + LOG(INFO) << "PASS: Results are essentially identical (tested <1mm difference)"; + } else { + LOG(WARNING) << "FAIL"; + } + } + LOG(INFO) << "\n\nTest complete!"; + + return 0; +} \ No newline at end of file diff --git a/src/turret_bot_main.cc b/src/turret_bot_main.cc index d62c473..e9bf7d1 100644 --- a/src/turret_bot_main.cc +++ b/src/turret_bot_main.cc @@ -4,7 +4,6 @@ #include "src/camera/cv_camera.h" #include "src/localization/joint_solver.h" #include "src/localization/run_localization.h" -#include "src/localization/square_solver.h" #include "src/utils/camera_utils.h" #include "src/utils/nt_utils.h" auto main() -> int { @@ -20,7 +19,7 @@ auto main() -> int { std::make_unique( camera::camera_constants[camera::Camera::TURRET_BOT_FRONT_LEFT])); - std::this_thread::sleep_for(std::chrono::seconds(7)); + std::this_thread::sleep_for(std::chrono::seconds(3)); // camera::CameraSource back_right_camera = camera::CameraSource( // "back_right", @@ -41,7 +40,7 @@ auto main() -> int { utils::read_intrinsics( camera::camera_constants[camera::Camera::TURRET_BOT_FRONT_RIGHT] .intrinsics_path)), - std::make_unique( + std::make_unique( camera::Camera::TURRET_BOT_FRONT_RIGHT), camera::camera_constants[camera::Camera::TURRET_BOT_FRONT_RIGHT] .extrinsics_path, @@ -54,11 +53,12 @@ auto main() -> int { utils::read_intrinsics( camera::camera_constants[camera::Camera::TURRET_BOT_FRONT_LEFT] .intrinsics_path)), - std::make_unique( + std::make_unique( camera::Camera::TURRET_BOT_FRONT_LEFT), camera::camera_constants[camera::Camera::TURRET_BOT_FRONT_LEFT] .extrinsics_path, 4972, false); + LOG(INFO) << "Started estimators square"; - front_left_thread.join(); + front_right_thread.join(); } diff --git a/src/utils/CMakeLists.txt b/src/utils/CMakeLists.txt index 5fe6337..091d8e4 100644 --- a/src/utils/CMakeLists.txt +++ b/src/utils/CMakeLists.txt @@ -1,4 +1,4 @@ -add_library(utils timer.cc nt_utils.cc camera_utils.cc log.cc intrinsics_from_json.cc) +add_library(utils timer.cc nt_utils.cc camera_utils.cc log.cc intrinsics_from_json.cc extrinsics_from_json.cc) target_link_libraries(utils PUBLIC wpilibc nlohmann_json::nlohmann_json absl::log absl::check) if(!ENABLE_CLANG_TIDY) diff --git a/src/utils/extrinsics_from_json.cc b/src/utils/extrinsics_from_json.cc new file mode 100644 index 0000000..fd58a98 --- /dev/null +++ b/src/utils/extrinsics_from_json.cc @@ -0,0 +1,16 @@ +#include "src/utils/extrinsics_from_json.h" +namespace utils { + +auto ExtrinsicsJsonToCameraToRobot(nlohmann::json extrinsics_json) + -> frc::Transform3d { + frc::Pose3d camera_pose( + units::meter_t{extrinsics_json["translation_x"]}, + units::meter_t{extrinsics_json["translation_y"]}, + units::meter_t{extrinsics_json["translation_z"]}, + frc::Rotation3d(units::radian_t{extrinsics_json["rotation_x"]}, + units::radian_t{extrinsics_json["rotation_y"]}, + units::radian_t{extrinsics_json["rotation_z"]})); + frc::Transform3d robot_to_camera(frc::Pose3d(), camera_pose); + return robot_to_camera.Inverse(); +} +} // namespace utils diff --git a/src/utils/extrinsics_from_json.h b/src/utils/extrinsics_from_json.h new file mode 100644 index 0000000..57825c2 --- /dev/null +++ b/src/utils/extrinsics_from_json.h @@ -0,0 +1,9 @@ +#pragma once +#include +#include "src/utils/pch.h" + +namespace utils { + +auto ExtrinsicsJsonToCameraToRobot(nlohmann::json extrinsics_json) + -> frc::Transform3d; +} diff --git a/src/utils/intrinsics_from_json.cc b/src/utils/intrinsics_from_json.cc index 60c88f6..187482a 100644 --- a/src/utils/intrinsics_from_json.cc +++ b/src/utils/intrinsics_from_json.cc @@ -1,5 +1,6 @@ #include "src/utils/intrinsics_from_json.h" #include "third_party/971apriltag/971apriltag.h" +namespace utils { template <> auto camera_matrix_from_json( nlohmann::json intrinsics) -> frc971::apriltag::CameraMatrix { @@ -48,3 +49,4 @@ auto camera_matrix_from_json(nlohmann::json intrinsics) intrinsics["cy"], 0, 0, 1; return K; } +} // namespace utils diff --git a/src/utils/intrinsics_from_json.h b/src/utils/intrinsics_from_json.h index 0344c8d..44a9522 100644 --- a/src/utils/intrinsics_from_json.h +++ b/src/utils/intrinsics_from_json.h @@ -2,8 +2,11 @@ #include "src/utils/pch.h" +namespace utils { + template auto camera_matrix_from_json(nlohmann::json intrinsics) -> T; template auto distortion_coefficients_from_json(nlohmann::json intrinsics) -> T; +} // namespace utils