diff --git a/CMakeLists.txt b/CMakeLists.txt index d7214ce5..eeedfc7c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,6 +38,7 @@ find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) find_package(vpi REQUIRED) find_package(libuvc REQUIRED) +find_package(XAD REQUIRED) add_subdirectory(third_party/json) add_subdirectory(third_party/971apriltag) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 2f9ff596..81e5bafc 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -1,215 +1,536 @@ #include "src/localization/joint_solver.h" +#include #include #include +#include "src/localization/position_solver.h" #include "src/utils/camera_utils.h" #include "src/utils/constants_from_json.h" #include "src/utils/transform.h" namespace localization { -using data_point_t = struct DataPoint { - Eigen::Vector2d undistorted_point; - std::string name; - Eigen::Vector4d field_to_tag_corner_homogenous; -}; +using frc::AprilTagFieldLayout; +using utils::CameraMatrixFromJson; +using utils::DistortionCoefficientsFromJson; +using utils::ExtrinsicsJsonToCameraToRobot; +using utils::ReadExtrinsics; +using utils::ReadIntrinsics; + +// clang-format off +const Eigen::Matrix PI = + (Eigen::Matrix() << + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0).finished(); -constexpr auto sq(double num) -> double { - return num * num; +const Eigen::Matrix rotate_yaw = + (Eigen::Matrix() << + -1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1).finished(); +// clang-format on + +constexpr auto square(double x) -> double { + return x * x; +} +auto JointSolver::DifferentiableTransform3d::ToEigen() -> Eigen::Matrix4d { + const auto matrix = ToMatrix(); + Eigen::Matrix4d out = Eigen::Matrix4d::Zero(); + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + out(i, j) = value(matrix[i][j]); + } + } + return out; } -using frc::AprilTagFieldLayout; +auto JointSolver::DifferentiableTransform3d::ToMatrix() + -> std::array, 4> { + AD cos_x = xad::cos(r_x); + AD cos_y = xad::cos(r_y); + AD cos_z = xad::cos(r_z); + + AD sin_x = xad::sin(r_x); + AD sin_y = xad::sin(r_y); + AD sin_z = xad::sin(r_z); + + // Rotation + std::array, 4> matrix; + matrix[0][0] = cos_z * cos_y; + matrix[0][1] = cos_z * sin_y * sin_x - sin_z * cos_x; + matrix[0][2] = cos_z * sin_y * cos_x + sin_z * sin_x; + + matrix[1][0] = sin_z * cos_y; + matrix[1][1] = sin_z * sin_y * sin_x + cos_z * cos_x; + matrix[1][2] = sin_z * sin_y * cos_x - cos_z * sin_x; + + matrix[2][0] = -sin_y; + matrix[2][1] = cos_y * sin_x; + matrix[2][2] = cos_y * cos_x; + + // Translation + matrix[0][3] = t_x; + matrix[1][3] = t_y; + matrix[2][3] = t_z; + + matrix[3][0] = 0; + matrix[3][1] = 0; + matrix[3][3] = 1; + matrix[3][2] = 0; + return matrix; +} +void JointSolver::DifferentiableTransform3d::Update( + const Eigen::VectorXd& update) { + CHECK(update.size() == 6); + t_x += update[0]; + t_y += update[1]; + t_z += update[2]; + + r_x += update[3]; + r_y += update[4]; + r_z += update[5]; +} + +auto UndistortPixelPoint(const cv::Point2f& distortedPoint, + const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs) + -> cv::Point2f { + std::vector src = {distortedPoint}; + std::vector dst; + cv::undistortPoints(src, dst, cameraMatrix, distCoeffs, cv::noArray(), + cameraMatrix); + return dst[0]; +} + +auto NormalizePoint(const cv::Point2d& image_point, + const camera::camera_constant_t& camera_constant, + const cv::Mat& camera_matrix, + const cv::Mat& distortion_coefficients) -> Eigen::Vector3d { + cv::Point2f undistorted_point = + UndistortPixelPoint(image_point, camera_matrix, distortion_coefficients); + // clang-format off + return (Eigen::Vector3d() << + 1, + undistorted_point.x / camera_constant.frame_width.value(), + undistorted_point.y / camera_constant.frame_height.value()) + .finished(); + // clang-format on +} + +auto ProjectPoints(const Eigen::MatrixXd& A, const Eigen::MatrixXd& correction, + const Eigen::Vector4d& x) -> Eigen::Vector3d { + + Eigen::Vector3d projected_point = A * correction * x; + auto normalized_point = projected_point / projected_point[0]; + return normalized_point; +} + +auto NormalizeCameraMatrix(Eigen::Matrix3d camera_matrix, + const camera::camera_constant_t& camera_constant) + -> Eigen::Matrix3d { + camera_matrix(1, 0) /= camera_constant.frame_width.value(); + + camera_matrix(2, 0) /= camera_constant.frame_height.value(); + camera_matrix(2, 2) /= camera_constant.frame_height.value(); + camera_matrix(1, 1) /= camera_constant.frame_width.value(); + return camera_matrix; +} + +auto CreateTransformationMatrix(const Eigen::VectorXd& params) + -> Eigen::Matrix4d { + double tx = params[0]; + double ty = params[1]; + double tz = params[2]; + + double rx = params[3]; + double ry = params[4]; + double rz = params[5]; + + const double cos_x = std::cos(rx); + const double cos_y = std::cos(ry); + const double cos_z = std::cos(rz); + + const double sin_x = std::sin(rx); + const double sin_y = std::sin(ry); + const double sin_z = std::sin(rz); -JointSolver::JointSolver( - const std::vector& camera_constants_, - const AprilTagFieldLayout& layout) - : robot_to_field_(Eigen::Matrix4d::Identity()) { // clang-format off - const Eigen::Matrix4d rotate_yaw_cv = (Eigen::Matrix4d() << - -1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, -1, 0, - 0, 0, 0, 1).finished(); + return (Eigen::Matrix() << + cos_z * cos_y, cos_z * sin_y * sin_x - sin_z * cos_x, cos_z * sin_y * cos_x + sin_z * sin_x, tx, + sin_z * cos_y, sin_z * sin_y * sin_x + cos_z * cos_x, sin_z * sin_y * cos_x - cos_z * sin_x, ty, + -sin_y, cos_y * sin_x, cos_y * cos_x, tz, + 0, 0, 0, 1) + .finished(); // clang-format on - for (const frc::AprilTag& tag : layout.GetTags()) { - Eigen::Matrix4d field_to_tag = tag.pose.ToMatrix(); - utils::ChangeBasis(field_to_tag, utils::WPI_TO_CV); - std::array field_relative_corners; - for (size_t i = 0; i < kapriltag_corners_eigen.size(); i++) { - field_relative_corners[i] = field_to_tag * rotate_yaw_cv * - utils::Homogenize(kapriltag_corners_eigen[i]); +} + +auto JointSolver::Multiply(const std::array, 4>& a, + const Eigen::Matrix4d& b) -> Eigen::Matrix4d { + Eigen::Matrix4d out = Eigen::Matrix4d::Zero(); + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + for (int k = 0; k < 4; ++k) { + out(i, j) += value(a[i][k]) * b(k, j); + } + } + } + return out; +} + +auto JointSolver::Multiply(const std::array, 4>& a, + const Eigen::Vector4d& b) -> std::array { + std::array out{}; + for (int i = 0; i < 4; i++) { + out[i] = 0; + for (int j = 0; j < 4; j++) { + out[i] += a[i][j] * b[j]; + } + } + return out; +} +auto JointSolver::Multiply(const Eigen::Matrix& a, + const std::array& b) -> std::array { + std::array out{}; + for (int i = 0; i < 3; i++) { + out[i] = 0; + for (int j = 0; j < 4; j++) { + out[i] += a(i, j) * b[j]; + } + } + return out; +} + +void JointSolver::SaveJacobian(Eigen::MatrixXd& J, + const differentiable_transform3d_t& correction, + int index) { + const int offset = index * 2; + J(0 + offset, 0) = correction.t_x.getAdjoint()[0]; + J(0 + offset, 1) = correction.t_y.getAdjoint()[0]; + J(0 + offset, 2) = correction.t_z.getAdjoint()[0]; + J(0 + offset, 3) = correction.r_x.getAdjoint()[0]; + J(0 + offset, 4) = correction.r_y.getAdjoint()[0]; + J(0 + offset, 5) = correction.r_z.getAdjoint()[0]; + J(1 + offset, 0) = correction.t_x.getAdjoint()[1]; + J(1 + offset, 1) = correction.t_y.getAdjoint()[1]; + J(1 + offset, 2) = correction.t_z.getAdjoint()[1]; + J(1 + offset, 3) = correction.r_x.getAdjoint()[1]; + J(1 + offset, 4) = correction.r_y.getAdjoint()[1]; + J(1 + offset, 5) = correction.r_z.getAdjoint()[1]; +} + +void JointSolver::SaveResidual(Eigen::VectorXd& residual, double u_residual, + double v_residual, int index) { + const int offset = index * 2; + residual[0 + offset] = u_residual; + residual[1 + offset] = v_residual; +} + +auto JointSolver::CalculateUpdate(const Eigen::MatrixXd& J, + const Eigen::VectorXd& residual, + double lambda) -> Eigen::VectorXd { + constexpr double kEpsilon = 1e-12; + + const Eigen::MatrixXd j_t_j = J.transpose() * J; + Eigen::VectorXd diagonal = j_t_j.diagonal(); + for (int i = 0; i < diagonal.size(); ++i) { + if (diagonal[i] <= kEpsilon) { + diagonal[i] = kEpsilon; } - tag_corners_[tag.ID] = field_relative_corners; } - Eigen::Matrix pi = Eigen::Matrix::Zero(); - pi.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); - for (const camera::camera_constant_t& camera_constant : camera_constants_) { - const nlohmann::json intrinsics_json = - utils::ReadIntrinsics(camera_constant.intrinsics_path.value()); - const auto camera_matrix = - utils::CameraMatrixFromJson(intrinsics_json); - const Eigen::Matrix image_to_camera = camera_matrix * pi; - const Eigen::Matrix4d camera_to_robot = - utils::ExtrinsicsJsonToCameraToRobot( - utils::ReadExtrinsics(camera_constant.extrinsics_path.value())) - .ToMatrix(); - const Eigen::Matrix image_to_robot = - image_to_camera * camera_to_robot; - camera_matrices_.insert( - {camera_constant.name, - {.image_to_robot = image_to_robot, - .distortion_coefficients = - utils::DistortionCoefficientsFromJson(intrinsics_json), - .camera_matrix = utils::EigenToCvMat(camera_matrix)}}); + + const Eigen::MatrixXd damping = + (lambda * diagonal).asDiagonal().toDenseMatrix(); + const Eigen::MatrixXd damped_hessian = + j_t_j + damping + + kEpsilon * Eigen::MatrixXd::Identity(J.cols(), J.cols()); + const Eigen::VectorXd gradient = J.transpose() * residual; + + const Eigen::LDLT solver(damped_hessian); + if (solver.info() != Eigen::Success) { + return Eigen::VectorXd::Zero(J.cols()); + } + + return solver.solve(-gradient); +} + +auto JointSolver::CalculateResidualLoss( + const Eigen::Matrix4d& correction_matrix, + const std::vector& data_points) -> double { + + double loss = 0.0; + for (const auto& data_point : data_points) { + const Eigen::Vector3d projected_point = + ProjectPoints(data_point.A, correction_matrix, data_point.x); + const double u_residual = + projected_point[1] - data_point.normalized_image_point[1]; + const double v_residual = + projected_point[2] - data_point.normalized_image_point[2]; + loss += u_residual * u_residual + v_residual * v_residual; + } + + const double residual_count = + static_cast(std::max(1, data_points.size() * 2)); + return loss / residual_count; +} + +JointSolver::JointSolver( + const std::vector& camera_constants, + AprilTagFieldLayout layout) + : layout_(std::move(layout)), camera_constants_(camera_constants) { + for (size_t i = 0; i < camera_constants.size(); i++) { + camera_name_to_index[camera_constants[i].name] = i; + + distortion_coefficients_.emplace_back( + DistortionCoefficientsFromJson( + ReadIntrinsics(camera_constants[i].intrinsics_path.value()))); + + camera_matrix_.emplace_back(CameraMatrixFromJson( + ReadIntrinsics(camera_constants[i].intrinsics_path.value()))); + + normalized_camera_matrix_.emplace_back(NormalizeCameraMatrix( + CameraMatrixFromJson( + ReadIntrinsics(camera_constants[i].intrinsics_path.value())), + camera_constants[i])); + + robot_to_camera_.emplace_back( + ExtrinsicsJsonToCameraToRobot( + ReadExtrinsics(camera_constants[i].extrinsics_path.value())) + .ToMatrix() + .inverse()); } } auto JointSolver::EstimatePosition( const std::map>& - all_cam_detections, - const frc::Pose3d& starting_pose) -> position_estimate_t { - if (all_cam_detections.empty()) { - return {}; + camera_detections, + std::optional initial_pose_maybe) -> position_estimate_t { + std::vector tag_ids; + std::vector data_points; + double average_timestamp = 0; + + const auto initial_pose = + initial_pose_maybe.value_or(frc::Pose3d(position_receiver_.Get())); + const Eigen::Matrix4d field_to_robot = initial_pose.ToMatrix(); + const Eigen::Matrix4d robot_to_feild = field_to_robot.inverse(); + + CreateDataPoints(initial_pose, camera_detections, data_points, + average_timestamp, tag_ids); + + const int total_detections = data_points.size() / 4; + + if (data_points.size() < 3) { + return position_estimate_t{ + .tag_ids = tag_ids, + .rejected_tag_ids = {}, + .pose = {}, + .variance = 1, + .timestamp = average_timestamp, + .num_tags = total_detections, + .avg_tag_dist = -1, + .latency = -1, + .invalid = true, + .loss = -1, + }; } - robot_to_field_ = starting_pose.ToMatrix().inverse(); - utils::ChangeBasis(robot_to_field_, utils::WPI_TO_CV); - std::vector data_points; - for (const auto& pair : all_cam_detections) { - const CameraMatrices& camera_mats = camera_matrices_.at(pair.first); - for (const tag_detection_t& detection : pair.second) { - std::vector undistorted_corners; - cv::undistortImagePoints(detection.corners, undistorted_corners, - camera_mats.camera_matrix, - camera_mats.distortion_coefficients); - for (size_t i = 0; i < undistorted_corners.size(); i++) { - Eigen::Vector2d undistorted_image_point; - undistorted_image_point << undistorted_corners[i].x, - undistorted_corners[i].y; - const data_point_t datapoint = { - .undistorted_point = undistorted_image_point, - .name = pair.first, - .field_to_tag_corner_homogenous = - tag_corners_[detection.tag_id].value()[i]}; - data_points.push_back(datapoint); + + differentiable_transform3d_t correction{}; + + double checkpoint_loss = + CalculateResidualLoss(correction.ToEigen(), data_points); + double lambda = 1e-6; + constexpr double kMinLambda = 1e-16; + constexpr double kMaxLambda = 1e16; + constexpr double kMinUpdateNorm = 1e-18; + constexpr double kMinLossImprovement = 1e-17; + constexpr int kMaxAttemptsPerEpoch = 40; + constexpr int kMaxStaleEpochs = 400; + constexpr int kepochs = 100; + int stale_epochs = 0; + + for (int epoch = 0; epoch < kepochs; epoch++) { + Eigen::MatrixXd J(data_points.size() * 2, 6); + Eigen::VectorXd residual(data_points.size() * 2); + + tape_.newRecording(); + tape_.registerInput(correction.t_x); + tape_.registerInput(correction.t_y); + tape_.registerInput(correction.t_z); + tape_.registerInput(correction.r_x); + tape_.registerInput(correction.r_y); + tape_.registerInput(correction.r_z); + int index = 0; + for (auto const& data_point : data_points) { + const auto correction_matrix = correction.ToMatrix(); + auto a = Multiply(correction_matrix, data_point.x); + std::array projected_point = Multiply(data_point.A, a); + AD u = projected_point[1] / projected_point[0]; + AD v = projected_point[2] / projected_point[0]; + tape_.registerOutput(u); + tape_.registerOutput(v); + u.setAdjoint({1.0, 0.0}); + v.setAdjoint({0.0, 1.0}); + tape_.computeAdjoints(); + SaveJacobian(J, correction, index); + SaveResidual(residual, u.value() - data_point.normalized_image_point[1], + v.value() - data_point.normalized_image_point[2], index); + + index++; + } + const double loss = residual.squaredNorm(); + bool accepted = false; + double best_candidate_loss = checkpoint_loss; + Eigen::VectorXd accepted_update = Eigen::VectorXd::Zero(6); + double trial_lambda = lambda; + + for (int attempt = 0; attempt < kMaxAttemptsPerEpoch; ++attempt) { + const Eigen::VectorXd update = CalculateUpdate(J, residual, trial_lambda); + if (update.norm() < kMinUpdateNorm) { + break; + } + + differentiable_transform3d_t candidate = correction; + candidate.Update(update); + const double candidate_loss = + CalculateResidualLoss(candidate.ToEigen(), data_points); + + if (candidate_loss < checkpoint_loss) { + accepted = true; + accepted_update = update; + best_candidate_loss = candidate_loss; + lambda = std::max(kMinLambda, trial_lambda * 0.5); + break; } + + trial_lambda = std::min(kMaxLambda, trial_lambda * 2.0); } - } - double loss = std::numeric_limits::infinity(); - int counter = 0; - - const double step = 1e-7; - - utils::TransformValues translation_and_rotation = - utils::ExtractTranslationAndRotation(robot_to_field_); - utils::TransformDecomposition decomposed_robot_to_field; - - while (loss > kacceptable_reprojection_error && counter < 1000000) { - bool printed = false; - for (const data_point_t& data_point : data_points) { - decomposed_robot_to_field = utils::SeparateTranslationAndRotationMatrices( - translation_and_rotation); - const Eigen::Matrix& image_to_robot = - camera_matrices_.at(data_point.name).image_to_robot; - - const Eigen::Vector4d Rx_activation = - decomposed_robot_to_field.Rx * - data_point.field_to_tag_corner_homogenous; - const Eigen::Vector4d Ry_activation = - decomposed_robot_to_field.Ry * Rx_activation; - const Eigen::Vector4d Rz_activation = - decomposed_robot_to_field.Rz * Ry_activation; - - Eigen::Vector3d projection = image_to_robot * - decomposed_robot_to_field.translation * - Rz_activation; - const double lambda = projection(2); - projection /= lambda; - - const Eigen::Vector2d projection_error( - projection.x() - data_point.undistorted_point.x(), - projection.y() - data_point.undistorted_point.y()); - loss = 0.5 * projection_error.squaredNorm(); - - if (counter % 10000 == 0 && !printed) { - printed = true; - std::cout << "loss: " << loss << std::endl; + VLOG(1) << "--------------------------------------"; + VLOG(1) << "Residual\n" << residual; + VLOG(1) << "Loss: " << loss; + VLOG(1) << "Checkpoint Loss: " << checkpoint_loss; + VLOG(1) << "Candidate Loss: " << best_candidate_loss; + VLOG(1) << "lambda: " << lambda; + VLOG(1) << "update\n" << accepted_update; + VLOG(1) << "correction\n" << correction; + if (accepted) { + const double loss_improvement = checkpoint_loss - best_candidate_loss; + correction.Update(accepted_update); + checkpoint_loss = best_candidate_loss; + if (loss_improvement < kMinLossImprovement) { + stale_epochs++; + } else { + stale_epochs = 0; + } + } else { + stale_epochs++; + lambda = std::min(kMaxLambda, trial_lambda); + if (lambda >= kMaxLambda) { + break; } + } + + if (stale_epochs >= kMaxStaleEpochs) { + break; + } + } + + Eigen::Matrix4d tmp = + (Multiply(correction.ToMatrix(), robot_to_feild)).inverse(); + tmp(3, 0) = 0; + tmp(3, 1) = 0; + tmp(3, 2) = 0; + tmp(3, 3) = 1; + frc::Pose3d pose(tmp); + + return position_estimate_t{ + .tag_ids = tag_ids, + .rejected_tag_ids = {}, + .pose = pose, + .variance = 1, + .timestamp = average_timestamp, + .num_tags = total_detections, + .avg_tag_dist = -1, + .latency = -1, + .invalid = false, + .loss = checkpoint_loss, + }; +} + +auto JointSolver::CalculateResidualLoss( + frc::Pose3d pose, const std::map>& + camera_detections) const -> double { + std::vector tag_ids; + std::vector data_points; + double average_timestamp = 0; + + CreateDataPoints(pose, camera_detections, data_points, average_timestamp, + tag_ids); + + differentiable_transform3d_t correction{}; + + double checkpoint_loss = + CalculateResidualLoss(correction.ToEigen(), data_points); + + return checkpoint_loss; +} - const Eigen::Vector3d d_projection( - projection_error.x() / lambda, projection_error.y() / lambda, - -(projection_error.x() * projection.x() + - projection_error.y() * projection.y()) / - lambda); - - Eigen::Vector4d accumulated_gradient = - image_to_robot.transpose() * d_projection; - - const Eigen::Matrix4d d_translation = - accumulated_gradient * Rz_activation.transpose(); - - accumulated_gradient = decomposed_robot_to_field.translation.transpose() * - accumulated_gradient; - const Eigen::Matrix4d d_Rz = - accumulated_gradient * Ry_activation.transpose(); - - accumulated_gradient = - decomposed_robot_to_field.Rz.transpose() * accumulated_gradient; - const Eigen::Matrix4d d_Ry = - accumulated_gradient * Rx_activation.transpose(); - - accumulated_gradient = - decomposed_robot_to_field.Ry.transpose() * accumulated_gradient; - const Eigen::Matrix4d d_Rx = - accumulated_gradient * - data_point.field_to_tag_corner_homogenous.transpose(); - - // clang-format off - const Eigen::Matrix4d d_Rz_d_rz = - (Eigen::Matrix4d() << - -sin(translation_and_rotation.rz), -cos(translation_and_rotation.rz), 0, 0, - cos(translation_and_rotation.rz), -sin(translation_and_rotation.rz), 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0).finished(); - const Eigen::Matrix4d d_Ry_d_ry = - (Eigen::Matrix4d() << - -sin(translation_and_rotation.ry), 0, cos(translation_and_rotation.ry), 0, - 0, 0, 0, 0, - -cos(translation_and_rotation.ry), 0, -sin(translation_and_rotation.ry), 0, - 0, 0, 0, 0).finished(); - const Eigen::Matrix4d d_Rx_d_rx = - (Eigen::Matrix4d() << - 0, 0, 0, 0, - 0, -sin(translation_and_rotation.rx), -cos(translation_and_rotation.rx), 0, - 0, cos(translation_and_rotation.rx), -sin(translation_and_rotation.rx), 0, - 0, 0, 0, 0).finished(); - // clang-format on - - translation_and_rotation.rx -= - step * (d_Rx_d_rx.transpose() * d_Rx).trace(); - translation_and_rotation.ry -= - step * (d_Ry_d_ry.transpose() * d_Ry).trace(); - translation_and_rotation.rz -= - step * (d_Rz_d_rz.transpose() * d_Rz).trace(); - - translation_and_rotation.x -= step * d_translation(0, 3); - translation_and_rotation.y -= step * d_translation(1, 3); - translation_and_rotation.z -= step * d_translation(2, 3); +void JointSolver::CreateDataPoints( + const frc::Pose3d& initial_pose, + const std::map>& + camera_detections, + std::vector& data_points, double& average_timestamp, + std::vector& tag_ids) const { + + average_timestamp = 0; + const Eigen::Matrix4d field_to_robot = initial_pose.ToMatrix(); + const Eigen::Matrix4d robot_to_feild = field_to_robot.inverse(); + + for (const auto& [camera_name, tag_detections] : camera_detections) { + int index = camera_name_to_index.at(camera_name); + for (const auto& tag_detection : tag_detections) { + tag_ids.push_back(tag_detection.tag_id); + average_timestamp += tag_detection.timestamp; + int corner_index = 0; + for (const auto& corner : tag_detection.corners) { + const auto field_to_tag = + layout_.GetTagPose(tag_detection.tag_id)->ToMatrix(); + const auto homogenized_apriltag_corner = + localization::kapriltag_corners_eigen_homogenized[corner_index]; + const auto normalized_camera_matrix = normalized_camera_matrix_[index]; + const auto camera_to_robot = robot_to_camera_[index].inverse(); + data_points.push_back(datapoint_t{ + .normalized_image_point = NormalizePoint( + corner, camera_constants_[index], camera_matrix_[index], + distortion_coefficients_[index]), + .normalized_camera_matrix = normalized_camera_matrix, + .camera_to_robot = camera_to_robot, + .feild_to_tag = field_to_tag, + .homogenized_apriltag_corner = homogenized_apriltag_corner, + .x = robot_to_feild * field_to_tag * rotate_yaw * + homogenized_apriltag_corner, + .A = normalized_camera_matrix * PI * camera_to_robot}); + corner_index++; + } } - counter++; } - Eigen::Matrix4d field_to_robot = - (decomposed_robot_to_field.translation * decomposed_robot_to_field.Rz * - decomposed_robot_to_field.Ry * decomposed_robot_to_field.Rx) - .inverse(); + if (data_points.size() == 0) { + return; + } - utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), - "Field to robot cv"); - utils::ChangeBasis(field_to_robot, utils::CV_TO_WPI); - utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), - "Field to robot wpi"); + average_timestamp /= data_points.size() / 4; +} - return {.pose = frc::Pose3d(field_to_robot), .variance = 0, .timestamp = 0}; +auto operator<<(std::ostream& os, + const JointSolver::differentiable_transform3d_t& t) + -> std::ostream& { + os << "t_x=" << t.t_x << "\n" + << "t_y=" << t.t_y << "\n" + << "t_z=" << t.t_z << "\n" + << "r_x=" << t.r_x << "\n" + << "r_y=" << t.r_y << "\n" + << "r_z=" << t.r_z << ""; + return os; } } // namespace localization diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 7dfb91f1..34dd982d 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -1,33 +1,108 @@ #pragma once #include +#include +#include #include "nlohmann/json.hpp" #include "src/camera/camera_constants.h" +#include "src/localization/position_receiver.h" #include "src/localization/position_solver.h" #include "src/localization/square_solver.h" -using json = nlohmann::json; - namespace localization { -struct CameraMatrices { - Eigen::Matrix image_to_robot; - cv::Mat distortion_coefficients; - cv::Mat camera_matrix; -}; -static constexpr int kmax_tags = 50; + class JointSolver { public: - JointSolver(const std::vector& camera_constants_, - const frc::AprilTagFieldLayout& layout = kapriltag_layout); + using mode = xad::adj; + using tape_type = mode::tape_type; + using AD = mode::active_type; + + using datapoint_t = struct DataPoint { + Eigen::Vector3d normalized_image_point; + Eigen::Matrix3d normalized_camera_matrix; + Eigen::Matrix4d camera_to_robot; + Eigen::Matrix4d feild_to_tag; + Eigen::Vector4d homogenized_apriltag_corner; + Eigen::Vector4d x; + Eigen::MatrixXd A; + }; + + using differentiable_transform3d_t = struct DifferentiableTransform3d { + AD t_x = 0; + AD t_y = 0; + AD t_z = 0; + AD r_x = 0; + AD r_y = 0; + AD r_z = 0; + + auto ToEigen() -> Eigen::Matrix4d; + auto ToMatrix() -> std::array, 4>; + void Update(const Eigen::VectorXd& update); + }; + + public: + auto static Multiply(const std::array, 4>& a, + const Eigen::Vector4d& b) -> std::array; + + auto static Multiply(const std::array, 4>& a, + const Eigen::Matrix4d& b) -> Eigen::Matrix4d; + + auto static Multiply(const Eigen::Matrix& a, + const std::array& b) -> std::array; + + // Jacobian should be data_points_.size() x 6(number of params in correction) + // index is should be from 1...data_points_.size() + void static SaveJacobian(Eigen::MatrixXd& J, + const differentiable_transform3d_t& correction, + int index); + + void static SaveResidual(Eigen::VectorXd& residual, double u_residual, + double v_residual, int index); + + auto static CalculateUpdate(const Eigen::MatrixXd& J, + const Eigen::VectorXd& residual, double lambda) + -> Eigen::VectorXd; + + auto static CalculateResidualLoss(const Eigen::Matrix4d& correction_matrix, + const std::vector& data_points) + -> double; + + public: + JointSolver(const std::vector& camera_constants, + frc::AprilTagFieldLayout layout = kapriltag_layout); auto EstimatePosition( const std::map>& - all_cam_detections, - const frc::Pose3d& starting_pose) -> position_estimate_t; - Eigen::Matrix4d robot_to_field_; + camera_detections, + std::optional intial_pose_maybe = std::nullopt) + -> position_estimate_t; + + auto CalculateResidualLoss( + frc::Pose3d pose, + const std::map>& + camera_detections) const -> double; private: - static constexpr double kacceptable_reprojection_error = 0.005; - std::map camera_matrices_; - std::array>, kmax_tags> - tag_corners_; + void CreateDataPoints( + const frc::Pose3d& initial_pose, + const std::map>& + camera_detections, + std::vector& data_points, double& average_timestamp, + std::vector& tag_ids) const; + + private: + std::unordered_map camera_name_to_index; + frc::AprilTagFieldLayout layout_; + std::vector camera_matrix_; + std::vector distortion_coefficients_; + std::vector normalized_camera_matrix_; + std::vector robot_to_camera_; + std::vector camera_constants_; + + PositionReceiver position_receiver_; + tape_type tape_; }; + +auto operator<<(std::ostream& os, + const JointSolver::DifferentiableTransform3d& t) + -> std::ostream&; } // namespace localization +// diff --git a/src/localization/multi_tag_solver.cc b/src/localization/multi_tag_solver.cc index 1c5ab9f6..8795878a 100644 --- a/src/localization/multi_tag_solver.cc +++ b/src/localization/multi_tag_solver.cc @@ -4,6 +4,8 @@ #include "src/utils/constants_from_json.h" #include "src/utils/transform.h" +#include + static const cv::Mat zero_vec = (cv::Mat_(3, 1) << 0, 0, 0); namespace localization { @@ -114,6 +116,19 @@ auto MultiTagSolver::EstimatePosition( cv::solvePnP(object_points, image_points, camera_matrix_, distortion_coefficients_, rvec, tvec, false, cv::SOLVEPNP_SQPNP); + + std::vector reprojected_points; + cv::projectPoints(object_points, rvec, tvec, camera_matrix_, + distortion_coefficients_, reprojected_points); + double squared_error_sum = 0.0; + for (size_t i = 0; i < image_points.size(); ++i) { + const cv::Point2d delta = image_points[i] - reprojected_points[i]; + squared_error_sum += delta.dot(delta); + } + const double reprojection_error = + std::sqrt(squared_error_sum / image_points.size()); + LOG(INFO) << "MultiTagSolver reprojection RMSE (px): " + << reprojection_error; } catch (const std::exception& e) { LOG(WARNING) << "Caught solve pnp exception:\n" << e.what(); return {}; diff --git a/src/localization/position_receiver.cc b/src/localization/position_receiver.cc index 5624159a..f31fec41 100644 --- a/src/localization/position_receiver.cc +++ b/src/localization/position_receiver.cc @@ -20,4 +20,4 @@ auto PositionReceiver::Get() -> frc::Pose2d { return pose2d_subscriber_.Get(); } -} // namespace localization \ No newline at end of file +} // namespace localization diff --git a/src/localization/position_receiver.h b/src/localization/position_receiver.h index 890a28bd..b168d08d 100644 --- a/src/localization/position_receiver.h +++ b/src/localization/position_receiver.h @@ -19,4 +19,4 @@ class PositionReceiver { std::mutex mutex_; }; -} // namespace localization \ No newline at end of file +} // namespace localization diff --git a/src/localization/position_solver.h b/src/localization/position_solver.h index 70ebae2b..0de7723e 100644 --- a/src/localization/position_solver.h +++ b/src/localization/position_solver.h @@ -21,6 +21,13 @@ const std::vector kapriltag_corners_eigen = { {ktag_size / 2, -ktag_size / 2, 0}, {-ktag_size / 2, -ktag_size / 2, 0}}; +const std::vector kapriltag_corners_eigen_homogenized = { + {0, ktag_size / 2, -ktag_size / 2, 1}, // Bottom left + {0, -ktag_size / 2, -ktag_size / 2, 1}, // Bottom right + {0, -ktag_size / 2, ktag_size / 2, 1}, // Top right + {0, ktag_size / 2, ktag_size / 2, 1}, // Top left +}; + const frc::AprilTagFieldLayout kapriltag_layout = frc::AprilTagFieldLayout::LoadField( frc::AprilTagField::k2026RebuiltAndyMark); diff --git a/src/localization/square_solver.cc b/src/localization/square_solver.cc index d8da439d..bf5a5d2b 100644 --- a/src/localization/square_solver.cc +++ b/src/localization/square_solver.cc @@ -1,4 +1,7 @@ #include "src/localization/square_solver.h" + +#include + #include #include #include "Eigen/Dense" diff --git a/src/test/unit_test/CMakeLists.txt b/src/test/unit_test/CMakeLists.txt index 6b9ce1b2..5d65206c 100644 --- a/src/test/unit_test/CMakeLists.txt +++ b/src/test/unit_test/CMakeLists.txt @@ -1,9 +1,6 @@ include(GoogleTest) enable_testing() -add_executable(joint_solve_test joint_solve_test.cc) -target_link_libraries(joint_solve_test PRIVATE localization utils GTest::gtest GTest::gtest_main) - add_executable(multi_tag_test multi_tag_test.cc) target_link_libraries(multi_tag_test PRIVATE localization utils GTest::gtest_main) @@ -13,9 +10,12 @@ target_link_libraries(square_solve_test PRIVATE localization utils GTest::gtest_ add_executable(general_solver_test general_solver_test.cc) target_link_libraries(general_solver_test PRIVATE localization utils GTest::gtest_main) -add_executable(matrix matrix.cc) +add_executable(matrix matrix.cc) target_link_libraries(matrix PRIVATE localization utils GTest::gtest_main) +add_executable(joint_solve_test joint_solve_test.cc) +target_link_libraries(joint_solve_test PRIVATE localization utils GTest::gtest_main XAD::xad) + add_executable(pathing_test pathing_test.cc) target_link_libraries(pathing_test PRIVATE pathing utils nlohmann_json::nlohmann_json GTest::gtest_main) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc new file mode 100644 index 00000000..b2569841 --- /dev/null +++ b/src/test/unit_test/forward_test.cc @@ -0,0 +1,473 @@ +#include +#include +#include +#include +#include "src/localization/joint_solver.h" +#include "src/localization/joint_solver.h" // NO LINT +#include "src/localization/opencv_apriltag_detector.h" +#include "src/localization/square_solver.h" +#include "src/utils/camera_utils.h" +#include "src/utils/constants_from_json.h" +#include "src/utils/timer.h" +#include "src/utils/transform.h" + +#define IMAGE_STRIDE 4 +#define LOG_PATH "/bos-logs/log181/right" +#define TRANSLATION_LR 1 +#define ROTATION_LR 0.01 +#define LR 0.1 +#define EPOCHS 1000 +#define BETA_1 0.99 +#define BETA_2 0.990 +#define EPSILON 1e-9 + +namespace fs = std::filesystem; + +using camera::camera_constant_t; +using camera::GetCameraConstants; +using camera::timestamped_frame_t; +using localization::JointSolver; +using localization::kapriltag_layout; +using localization::OpenCVAprilTagDetector; +using localization::position_estimate_t; +using localization::SquareSolver; +using localization::tag_detection_t; +using utils::CameraMatrixFromJson; +using utils::ReadIntrinsics; + +using mode = xad::adj; +using tape_type = mode::tape_type; +using AD = mode::active_type; + +// clang-format off +const Eigen::Matrix PI = + (Eigen::Matrix() << + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0).finished(); + +const Eigen::Matrix rotate_yaw = + (Eigen::Matrix() << + -1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1).finished(); +// clang-format on + +class ForwardTest : public ::testing::Test { + protected: + void SetUp() override { + camera_constant_right_ = GetCameraConstants().at("second_bot_right"); + square_solver_right_ = + std::make_unique(camera_constant_right_); + detector_right_ = std::make_unique( + camera_constant_right_.frame_width.value(), + camera_constant_right_.frame_height.value(), + ReadIntrinsics(camera_constant_right_.intrinsics_path.value())); + camera_matrix_right_ = CameraMatrixFromJson( + ReadIntrinsics(camera_constant_right_.intrinsics_path.value())); + normalized_camera_matrix_right_ = JointSolver::NormalizeCameraMatrix( + camera_matrix_right_, camera_constant_right_); + camera_to_robot_right_ = + utils::ExtrinsicsJsonToCameraToRobot( + utils::ReadExtrinsics( + camera_constant_right_.extrinsics_path.value())) + .ToMatrix(); + + camera_constant_left_ = GetCameraConstants().at("second_bot_left"); + square_solver_left_ = std::make_unique(camera_constant_left_); + detector_left_ = std::make_unique( + camera_constant_left_.frame_width.value(), + camera_constant_left_.frame_height.value(), + ReadIntrinsics(camera_constant_left_.intrinsics_path.value())); + camera_matrix_left_ = CameraMatrixFromJson( + ReadIntrinsics(camera_constant_left_.intrinsics_path.value())); + normalized_camera_matrix_left_ = JointSolver::NormalizeCameraMatrix( + camera_matrix_left_, camera_constant_left_); + camera_to_robot_left_ = + utils::ExtrinsicsJsonToCameraToRobot( + utils::ReadExtrinsics( + camera_constant_left_.extrinsics_path.value())) + .ToMatrix(); + } + + void TearDown() override {} + + std::unique_ptr detector_right_; + camera_constant_t camera_constant_right_; + std::unique_ptr square_solver_right_; + Eigen::Matrix3d camera_matrix_right_; + Eigen::Matrix3d normalized_camera_matrix_right_; + Eigen::Matrix4d camera_to_robot_right_; + + std::unique_ptr detector_left_; + camera_constant_t camera_constant_left_; + std::unique_ptr square_solver_left_; + Eigen::Matrix3d camera_matrix_left_; + Eigen::Matrix3d normalized_camera_matrix_left_; + Eigen::Matrix4d camera_to_robot_left_; +}; + +// TODO: Tolerance is quite high. Find out what causes the loss in precision +void CheckIsEqual(cv::Point2d image_point, Eigen::Vector3d projected_points, + double tolerance) { + EXPECT_NEAR(image_point.x, projected_points[1], tolerance); + EXPECT_NEAR(image_point.y, projected_points[2], tolerance); +} + +void CheckIsEqual(Eigen::Vector3d image_point, Eigen::Vector3d projected_points, + double tolerance) { + EXPECT_NEAR(image_point[1], projected_points[1], tolerance); + EXPECT_NEAR(image_point[2], projected_points[2], tolerance); +} + +TEST_F(ForwardTest, TestForward) { // NOLINT + // [u, v] = camera_matrix * PI * camera_to_tag * tag_corners + // + // feild_to_camera = feild_to_tag * 180_yaw * camera_to_tag.inv + // feild_to_camera * camera_to_tag = feild_to_tag * 180_yaw + // camera_to_tag = feild_to_camera.inv * feild_to_tag * 180_yaw + // + // feild_to_robot = feild_to_tag * 180_yaw * camera_to_tag.inv * camera_to_robot + // feild_to_robot * camera_to_robot.inv = feild_to_tag * 180_yaw * camera_to_tag.inv + // feild_to_robot * camera_to_robot.inv * camera_to_tag = feild_to_tag * 180_yaw + // camera_to_robot.inv * camera_to_tag = feild_to_robot.inv * feild_to_tag * 180_yaw + // camera_to_tag = camera_to_robot * feild_to_robot.inv * feild_to_tag * 180_yaw + std::vector image_paths; + for (const auto& file : fs::directory_iterator(LOG_PATH)) { + + image_paths.push_back(file.path()); + } + std::sort(image_paths.begin(), image_paths.end()); + + for (size_t i = 0; i < image_paths.size(); i += IMAGE_STRIDE) { + cv::Mat image = cv::imread(image_paths[i]); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_right_->GetTagDetections(timestamped_frame); + if (detections.empty()) { + continue; + } + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_right_->EstimatePosition({detection})[0]; + + // for (int j = 0; j < 4; j++) { + // auto projected_points = JointSolver::ProjectPoints( + // square_solver_solution.pose, + // kapriltag_layout.GetTagPose(detection.tag_id).value(), + // camera_matrix_right_, camera_to_robot_right_, j); + // + // auto projected_points_normalized = JointSolver::ProjectPoints( + // square_solver_solution.pose, + // kapriltag_layout.GetTagPose(detection.tag_id).value(), + // normalized_camera_matrix_right_, camera_to_robot_right_, j); + // + // CheckIsEqual(detection.corners[j], projected_points, 1); + // CheckIsEqual(JointSolver::NormalizePoint(detection.corners[j], + // camera_constant_right_), + // projected_points_normalized, 0.001); + // } + } +} + +TEST_F(ForwardTest, TestDerrivative) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_right_->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_right_->EstimatePosition({detection})[0]; + + auto feild_to_tag = + kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); + + Eigen::Matrix4d robot_to_feild = + square_solver_solution.pose.ToMatrix().inverse(); + + // Noise + robot_to_feild(0, 0) += 0.1; + robot_to_feild(0, 3) += 0.1; + robot_to_feild(0, 2) += 0.1; + robot_to_feild(1, 3) += 0.1; + + double loss = 0; + for (int epoch = 0; epoch < EPOCHS; epoch++) { + Eigen::Matrix4d robot_to_feild_d = Eigen::Matrix4d::Zero(); + loss = 0; + for (int corner_index = 0; corner_index < 4; corner_index++) { + + auto image_point = JointSolver::NormalizePoint( + detection.corners[corner_index], camera_constant_right_); + + robot_to_feild_d += JointSolver::CalculateDerivative( + robot_to_feild, feild_to_tag, camera_to_robot_right_, + normalized_camera_matrix_right_, image_point, corner_index); + + loss += JointSolver::CalculateLoss( + robot_to_feild, feild_to_tag, camera_to_robot_right_, + normalized_camera_matrix_right_, image_point, corner_index); + } + robot_to_feild -= robot_to_feild_d * 0.01; + } + ASSERT_LT(loss, 0.001); +} + +TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_right_->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_right_->EstimatePosition({detection})[0]; + + JointSolver::DifferentiableTransform3d transform_from_pose( + square_solver_solution.pose); + JointSolver::DifferentiableTransform3d transform_from_eigen( + square_solver_solution.pose.ToMatrix()); + + const double tolerance = 1e-7; + // clang-format off + ASSERT_NEAR(transform_from_pose.scaler.value(), transform_from_eigen.scaler.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.theta.value(), transform_from_eigen.theta.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.t_z.value(), transform_from_eigen.t_z.value(), + tolerance); + + ASSERT_NEAR(transform_from_pose.r_x.value(), transform_from_eigen.r_x.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.r_y.value(), transform_from_eigen.r_y.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.r_z.value(), transform_from_eigen.r_z.value(), + tolerance); + // clang-format on +} + +TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_right_->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_right_->EstimatePosition({detection})[0]; + + JointSolver::DifferentiableTransform3d transform(square_solver_solution.pose); + transform.CalculateMatrix(); + + EXPECT_TRUE(square_solver_solution.pose.ToMatrix().isApprox( + transform.ToEigen(), 1e-9)); + + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + EXPECT_NEAR(square_solver_solution.pose.ToMatrix()(i, j), + transform.matrix[i][j].value(), 1e-9); + } + } +} + +TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/6.767740.jpg"); + // cv::Mat image = cv::imread("/bos-logs/log181/right/17.047751.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_right_->GetTagDetections(timestamped_frame); + + LOG(INFO) << detections.size(); + // PCHECK(detections.size() > 1) + // << "Need at least two detections to test differnt tag positions"; + + auto square_solver_solution = + square_solver_right_->EstimatePosition(detections)[0]; + + auto feild_to_robot = square_solver_solution.pose.ToMatrix(); + + const auto robot_to_feild = feild_to_robot.inverse(); + JointSolver::DifferentiableTransform3d T(frc::Pose3d{}); + + T.CalculateMatrix(); + LOG(INFO) << "robot_to_feild_before\n" << robot_to_feild; + + // Noise + // robot_to_feild.scaler += 0.2; + // robot_to_feild.theta += 0.2; + // robot_to_feild.t_z += 0.1; + + // robot_to_feild.r_x += 0.05; + // robot_to_feild.r_y += 0.05; + // robot_to_feild.r_z += 0.1; + + tape_type tape; + + double loss = 0; + utils::Timer solve_timer("solve", true); + for (int epoch = 0; epoch < EPOCHS; epoch++) { + loss = 0; + JointSolver::transform3d_derivative_t derrivative; + for (const auto& detection : detections) { + auto feild_to_tag = + kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); + + for (int corner_index = 0; corner_index < 4; corner_index++) { + T.CalculateMatrix(); + T.RegisterInputs(tape); + + auto image_point = JointSolver::NormalizePoint( + detection.corners[corner_index], camera_constant_right_); + + auto robot_to_feild_d = JointSolver::CalculateDerivative( + robot_to_feild, feild_to_tag, camera_to_robot_right_, + normalized_camera_matrix_right_, image_point, corner_index); + + T.RegisterOutputs(tape); + derrivative = derrivative + T.BackPropagate(robot_to_feild_d, tape); + + loss += JointSolver::CalculateLoss( + robot_to_feild, feild_to_tag, camera_to_robot_right_, + normalized_camera_matrix_right_, image_point, corner_index); + + tape.newRecording(); + } + } + LOG(INFO) << "loss " << loss; + } +} + +// TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT +// cv::Mat image_right = cv::imread("/bos-logs/log181/right/20.411762.jpg"); +// +// cv::Mat image_left = cv::imread("/bos-logs/log181/left/20.935361.jpg"); +// std::map> detections; +// { +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image_right), .timestamp = 0, .invalid = false}; +// detections[camera_constant_right_.name] = +// detector_right_->GetTagDetections(timestamped_frame); +// CHECK(!detections[camera_constant_right_.name].empty()); +// } +// { +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image_left), .timestamp = 0, .invalid = false}; +// detections[camera_constant_left_.name] = +// detector_left_->GetTagDetections(timestamped_frame); +// CHECK(!detections[camera_constant_left_.name].empty()); +// } +// +// auto square_solver_solution = square_solver_right_->EstimatePosition( +// detections[camera_constant_right_.name])[0]; +// +// auto feild_to_robot = square_solver_solution.pose.ToMatrix(); +// JointSolver::DifferntiableTransform3d robot_to_feild( +// feild_to_robot.inverse()); +// +// // Noise +// robot_to_feild.t_x += 0.1; +// robot_to_feild.t_y += 0.1; +// robot_to_feild.t_z += 0.1; +// +// robot_to_feild.r_x += 0.1; +// robot_to_feild.r_y += 0.1; +// robot_to_feild.r_z += 0.1; +// +// tape_type tape; +// +// double loss = 0; +// std::vector camera_constants{camera_constant_left_, +// camera_constant_right_}; +// utils::Timer solve_timer("solve", false); +// JointSolver::transform3d_derrivative_t velocity; +// for (int epoch = 0; epoch < EPOCHS; epoch++) { +// loss = 0; +// JointSolver::transform3d_derrivative_t derrivative; +// for (const auto& camera_constant : camera_constants) { +// +// auto camera_to_robot = +// utils::ExtrinsicsJsonToCameraToRobot( +// utils::ReadExtrinsics(camera_constant.extrinsics_path.value())) +// .ToMatrix(); +// +// auto camera_matrix = CameraMatrixFromJson( +// ReadIntrinsics(camera_constant.intrinsics_path.value())); +// Eigen::Matrix3d normalized_camera_matrix = camera_matrix / NORMALIZATION; +// normalized_camera_matrix(0, 0) = 1; +// +// for (const auto& detection : detections[camera_constant.name]) { +// auto feild_to_tag = +// kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); +// +// for (int corner_index = 0; corner_index < 4; corner_index++) { +// robot_to_feild.CalculateMatrix(); +// robot_to_feild.RegisterInputs(tape); +// const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); +// auto image_point = (Eigen::Vector3d() << 1, +// detection.corners[corner_index].x / NORMALIZATION, +// detection.corners[corner_index].y / NORMALIZATION) +// .finished(); +// +// auto robot_to_feild_d = JointSolver::CalculateDerivative( +// robot_to_feild_eigen, feild_to_tag, camera_to_robot, +// normalized_camera_matrix, image_point, corner_index); +// +// robot_to_feild.RegisterOutputs(tape); +// derrivative = derrivative + +// robot_to_feild.BackPropagate(robot_to_feild_d, tape); +// +// loss += JointSolver::CalculateLoss( +// robot_to_feild_eigen, feild_to_tag, camera_to_robot, +// normalized_camera_matrix, image_point, corner_index); +// +// tape.newRecording(); +// } +// } +// } +// velocity = (velocity * BETA) + derrivative; +// robot_to_feild.Apply(velocity, LR); +// } +// ASSERT_LT(solve_timer.Stop(), 0.1); // The test is very unoptimized +// ASSERT_LT(loss, 0.01); +// } + +// TEST_F(ForwardTest, JointSolverTest) { // NOLINT +// cv::Mat image_right = cv::imread("/bos-logs/log181/right/20.411762.jpg"); +// +// cv::Mat image_left = cv::imread("/bos-logs/log181/left/20.935361.jpg"); +// std::map> detections; +// +// { +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image_right), .timestamp = 0, .invalid = false}; +// detections[camera_constant_right_.name] = +// detector_right_->GetTagDetections(timestamped_frame); +// CHECK(!detections[camera_constant_right_.name].empty()); +// } +// { +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image_left), .timestamp = 0, .invalid = false}; +// detections[camera_constant_left_.name] = +// detector_left_->GetTagDetections(timestamped_frame); +// CHECK(!detections[camera_constant_left_.name].empty()); +// } +// +// auto square_solver_solution = square_solver_left_->EstimatePosition( +// detections[camera_constant_left_.name])[0]; +// +// std::vector camera_constants{camera_constant_left_, +// camera_constant_right_}; +// +// JointSolver joint_solver(camera_constants); +// position_estimate_t joint_solver_solution = +// joint_solver.EstimatePosition(detections, square_solver_solution.pose); +// +// // LOG(INFO) << "square_solver_solution_pose\n" +// // << square_solver_solution.pose.ToMatrix(); +// // +// // LOG(INFO) << "joint_solver_solution_pose\n" +// // << joint_solver_solution.pose.ToMatrix(); +// } diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 0bc15d77..9df60819 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -1,41 +1,163 @@ #include +#include +#include +#include #include "src/localization/joint_solver.h" +#include "src/localization/multi_tag_solver.h" +#include "src/localization/opencv_apriltag_detector.h" #include "src/localization/square_solver.h" #include "src/utils/camera_utils.h" #include "src/utils/constants_from_json.h" +#include "src/utils/timer.h" #include "src/utils/transform.h" -std::vector detections( - {{.tag_id = 31, - .corners = {cv::Point2d(100.0, 200.0), cv::Point2d(200.0, 200.0), - cv::Point2d(200.0, 100.0), cv::Point2d(100.0, 100.0)}, - .timestamp = 0.0, - .confidence = 0.0}}); - -TEST(JointSolveTest, EstimatePosition) { // NOLINT - camera::camera_constant_t camera_constant = - camera::GetCameraConstants().at("dev_orin"); - localization::JointSolver joint_solver({camera_constant}); - localization::SquareSolver square_solver(camera_constant); - - auto square_solver_solution = square_solver.EstimatePosition(detections)[0]; - utils::PrintTransformationMatrix( - utils::EigenToCvMat(square_solver_solution.pose.ToMatrix())); - std::cout << square_solver_solution.pose.ToMatrix() << std::endl; - - frc::Transform3d noise( - frc::Translation3d(units::meter_t{0.067}, units::meter_t{0.1}, - units::meter_t{-0.1}), - {}); - - square_solver_solution.pose = square_solver_solution.pose.TransformBy(noise); - - std::map> - associated_detections; - associated_detections.insert({camera_constant.name, detections}); - auto joint_solver_solution = joint_solver.EstimatePosition( - associated_detections, square_solver_solution.pose); - std::cout << "Joint solver solution: " << joint_solver_solution << std::endl; - std::cout << "Square solver solution: " << square_solver_solution - << std::endl; +namespace fs = std::filesystem; + +using camera::camera_constant_t; +using camera::GetCameraConstants; +using camera::timestamped_frame_t; +using localization::JointSolver; +using localization::kapriltag_layout; +using localization::MultiTagSolver; +using localization::OpenCVAprilTagDetector; +using localization::position_estimate_t; +using localization::SquareSolver; +using localization::tag_detection_t; +using utils::CameraMatrixFromJson; +using utils::ReadIntrinsics; + +TEST(JointSolveTest, TestJointSolve) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/15.739774.jpg"); + auto camera_constant = GetCameraConstants().at("second_bot_right"); + auto square_solver = std::make_unique(camera_constant); + auto joint_solver = + std::make_unique(std::vector{camera_constant}); + auto detector = std::make_unique( + camera_constant.frame_width.value(), camera_constant.frame_height.value(), + ReadIntrinsics(camera_constant.intrinsics_path.value())); + + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = square_solver->EstimatePosition({detection})[0]; + const frc::Pose3d expected_pose = square_solver_solution.pose; + + const std::map> camera_detections{ + {camera_constant.name, {detection}}, + }; + + square_solver_solution.pose = + square_solver_solution.pose.TransformBy(frc::Transform3d( + frc::Translation3d(units::meter_t{0.2}, units::meter_t{-0.15}, + units::meter_t{0.05}), + frc::Rotation3d(units::radian_t{0.06}, units::radian_t{-0.05}, + units::radian_t{0.04}))); + + auto joint_solve_solution = joint_solver->EstimatePosition( + camera_detections, square_solver_solution.pose); + + ASSERT_LT(joint_solve_solution.loss, 1e-7); + ASSERT_TRUE(joint_solve_solution.pose.ToMatrix().isApprox( + expected_pose.ToMatrix(), 0.01)); +} + +TEST(JointSolveTest, TestJointSolveMultipleInputImages) { // NOLINT + const auto camera_constant_right = + GetCameraConstants().at("second_bot_right"); + const auto camera_constant_left = GetCameraConstants().at("second_bot_left"); + + cv::Mat image_right = cv::imread("/bos-logs/log181/right/11.067757.jpg"); + cv::Mat image_left = cv::imread("/bos-logs/log181/left/11.031403.jpg"); + ASSERT_FALSE(image_right.empty()); + ASSERT_FALSE(image_left.empty()); + + auto square_solver = std::make_unique(camera_constant_right); + auto joint_solver = std::make_unique( + std::vector{camera_constant_right, camera_constant_left}); + + auto detector_right = std::make_unique( + camera_constant_right.frame_width.value(), + camera_constant_right.frame_height.value(), + ReadIntrinsics(camera_constant_right.intrinsics_path.value())); + auto detector_left = std::make_unique( + camera_constant_left.frame_width.value(), + camera_constant_left.frame_height.value(), + ReadIntrinsics(camera_constant_left.intrinsics_path.value())); + + timestamped_frame_t timestamped_frame_right{ + .frame = std::move(image_right), .timestamp = 0, .invalid = false}; + timestamped_frame_t timestamped_frame_left{ + .frame = std::move(image_left), .timestamp = 0, .invalid = false}; + + auto detections_right = + detector_right->GetTagDetections(timestamped_frame_right); + auto detections_left = + detector_left->GetTagDetections(timestamped_frame_left); + + ASSERT_FALSE(detections_right.empty()); + ASSERT_FALSE(detections_left.empty()); + + auto square_solution_right = + square_solver->EstimatePosition(detections_right)[0]; + + const std::map> camera_detections{ + {camera_constant_right.name, detections_right}, + // {camera_constant_left.name, detections_left}, + }; + + auto fudged_pose = square_solution_right.pose.TransformBy(frc::Transform3d( + frc::Translation3d(units::meter_t{1.25}, units::meter_t{-0.2}, + units::meter_t{0.98}), + frc::Rotation3d(units::radian_t{0.58}, units::radian_t{-0.07}, + units::radian_t{0.06}))); + + auto joint_solve_solution = + joint_solver->EstimatePosition(camera_detections, fudged_pose); + + ASSERT_LT(joint_solve_solution.loss, 1e-8); + ASSERT_FALSE(joint_solve_solution.invalid); +} + +TEST(JointSolveTest, TestJointSolveLoss) { // NOLINT + const auto camera_constant_right = + GetCameraConstants().at("second_bot_right"); + + cv::Mat image_right = cv::imread("/bos-logs/log181/right/17.979762.jpg"); + ASSERT_FALSE(image_right.empty()); + + auto square_solver = std::make_unique(camera_constant_right); + auto joint_solver = + std::make_unique(std::vector{camera_constant_right}); + + auto detector_right = std::make_unique( + camera_constant_right.frame_width.value(), + camera_constant_right.frame_height.value(), + ReadIntrinsics(camera_constant_right.intrinsics_path.value())); + + timestamped_frame_t timestamped_frame_right{ + .frame = std::move(image_right), .timestamp = 0, .invalid = false}; + + auto detections_right = + detector_right->GetTagDetections(timestamped_frame_right); + + ASSERT_GE(detections_right.size(), 4); + + const auto single_tag_detection = std::vector{detections_right[0]}; + + std::vector square_solutions = + square_solver->EstimatePosition(detections_right); + + ASSERT_FALSE(square_solutions.empty()); + + const std::map> camera_detections{ + {camera_constant_right.name, detections_right}, + }; + + for (const auto& square_solution : square_solutions) { + double square_loss = joint_solver->CalculateResidualLoss( + square_solution.pose, camera_detections); + ASSERT_LE(square_loss, 1e-3); + } } diff --git a/src/test/unit_test/multi_tag_test.cc b/src/test/unit_test/multi_tag_test.cc index 1a4d283f..2fc6e328 100644 --- a/src/test/unit_test/multi_tag_test.cc +++ b/src/test/unit_test/multi_tag_test.cc @@ -1,24 +1,50 @@ #include #include "src/localization/multi_tag_solver.h" +#include "src/localization/opencv_apriltag_detector.h" #include "src/localization/square_solver.h" +#include "src/utils/camera_utils.h" +#include "src/utils/constants_from_json.h" + +using camera::camera_constant_t; +using camera::GetCameraConstants; +using camera::timestamped_frame_t; +using localization::kapriltag_layout; +using localization::MultiTagSolver; +using localization::OpenCVAprilTagDetector; +using localization::position_estimate_t; +using localization::SquareSolver; +using localization::tag_detection_t; +using utils::CameraMatrixFromJson; +using utils::ReadIntrinsics; TEST(MultiTagTest, EqualToSquareSolve) { // NOLINT - camera::camera_constant_t camera_constant = - camera::GetCameraConstants().at("dev_orin"); + const auto camera_constant = GetCameraConstants().at("second_bot_right"); + + cv::Mat image = cv::imread("/bos-logs/log181/right/15.739774.jpg"); + + auto multi_tag_solver = std::make_unique(camera_constant); + auto square_solver = std::make_unique(camera_constant); + + auto detector = std::make_unique( + camera_constant.frame_width.value(), camera_constant.frame_height.value(), + ReadIntrinsics(camera_constant.intrinsics_path.value())); + + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; - std::vector detections( - {{.tag_id = 25, - .corners = {cv::Point2d(100.0f, 100.0f), cv::Point2d(200.0f, 100.0f), - cv::Point2d(200.0f, 200.0f), cv::Point2d(100.0f, 200.0f)}, - .timestamp = 0.0, - .confidence = 0.0}}); + auto detections = detector->GetTagDetections(timestamped_frame); + auto detection = std::vector{detections[0]}; - localization::SquareSolver square_solver(camera_constant); - localization::MultiTagSolver multi_tag_solver(camera_constant); + position_estimate_t multi_tag_solution = + multi_tag_solver->EstimatePosition(detection)[0]; + position_estimate_t square_solution = + square_solver->EstimatePosition(detection)[0]; - auto square_solver_solution = square_solver.EstimatePosition(detections)[0]; - auto multi_tag_solution = square_solver.EstimatePosition(detections)[0]; + // LOG(INFO) << multi_tag_solution.pose.ToMatrix(); + // LOG(INFO) << square_solution.pose.ToMatrix(); + // LOG(INFO) << multi_tag_solution; + // LOG(INFO) << square_solution; - ASSERT_TRUE(square_solver_solution.pose.ToMatrix() == - multi_tag_solution.pose.ToMatrix()); + ASSERT_TRUE(multi_tag_solution.pose.ToMatrix().isApprox( + square_solution.pose.ToMatrix(), 0.1)); } diff --git a/src/test/unit_test/xad_test.cc b/src/test/unit_test/xad_test.cc new file mode 100644 index 00000000..60a34072 --- /dev/null +++ b/src/test/unit_test/xad_test.cc @@ -0,0 +1,117 @@ +#include +#include +#include +#include + +double x0 = 1.0; +double x1 = 1.5; +double x2 = 1.3; +double x3 = 1.2; + +template +auto f2(const T& x0_in, const T& x1_in, const T& x2_in, const T& x3_in) + -> std::pair { + T y1 = x0_in * x1_in + x2_in; + T y2 = x0_in + x2_in * x3_in; + return {y1, y2}; +} +TEST(XadTest, Add) { // NOLINT + + using mode = xad::fwd; + using AD = mode::active_type; + + AD x0_ad = x0; + AD x1_ad = x1; + AD x2_ad = x2; + AD x3_ad = x3; + + AD sum = x0_ad + x1_ad + x2_ad + x3_ad; + EXPECT_DOUBLE_EQ(sum.value(), x0 + x1 + x2 + x3); +} + +TEST(XadTest, Vector) { // NOLINT + // tape and active data type for 1st order adjoint computation with 2-vector mode + using mode = xad::adj; + using tape_type = mode::tape_type; + using AD = mode::active_type; + + // initialize tape + tape_type tape; + + // set independent variables + AD x0_ad = x0; + AD x1_ad = x1; + AD x2_ad = x2; + AD x3_ad = x3; + + // and register them + tape.registerInput(x0_ad); + tape.registerInput(x1_ad); + tape.registerInput(x2_ad); + tape.registerInput(x3_ad); + + // start recording derivatives + tape.newRecording(); + + // run function with 2 outputs + std::pair y = f2(x0_ad, x1_ad, x2_ad, x3_ad); + + // register and seed adjoint of outputs + tape.registerOutput(y.first); + tape.registerOutput(y.second); + y.first.setAdjoint({1.0, 0.0}); + y.second.setAdjoint({0.0, 1.0}); + + // compute all other adjoints + tape.computeAdjoints(); + + // output results + auto y1 = value(y.first); + auto y2 = value(y.second); + auto dy1_dx0 = x0_ad.getAdjoint()[0]; + auto dy1_dx1 = x1_ad.getAdjoint()[0]; + auto dy1_dx2 = x2_ad.getAdjoint()[0]; + auto dy1_dx3 = x3_ad.getAdjoint()[0]; + auto dy2_dx0 = x0_ad.getAdjoint()[1]; + auto dy2_dx1 = x1_ad.getAdjoint()[1]; + auto dy2_dx2 = x2_ad.getAdjoint()[1]; + auto dy2_dx3 = x3_ad.getAdjoint()[1]; + + tape.newRecording(); + tape.registerInput(x0_ad); + tape.registerInput(x1_ad); + tape.registerInput(x2_ad); + tape.registerInput(x3_ad); + tape.registerOutput(y.first); + tape.registerOutput(y.second); + y = f2(x0_ad, x1_ad, x2_ad, x3_ad); + y.first.setAdjoint({1.0, 0.0}); + y.second.setAdjoint({0.0, 1.0}); + tape.computeAdjoints(); + + ASSERT_DOUBLE_EQ(y1, value(y.first)); + ASSERT_DOUBLE_EQ(y2, value(y.second)); + + EXPECT_DOUBLE_EQ(dy1_dx0, x0_ad.getAdjoint()[0]); + EXPECT_DOUBLE_EQ(dy1_dx1, x1_ad.getAdjoint()[0]); + EXPECT_DOUBLE_EQ(dy1_dx2, x2_ad.getAdjoint()[0]); + EXPECT_DOUBLE_EQ(dy1_dx3, x3_ad.getAdjoint()[0]); + + EXPECT_DOUBLE_EQ(dy2_dx0, x0_ad.getAdjoint()[1]); + EXPECT_DOUBLE_EQ(dy2_dx1, x1_ad.getAdjoint()[1]); + EXPECT_DOUBLE_EQ(dy2_dx2, x2_ad.getAdjoint()[1]); + EXPECT_DOUBLE_EQ(dy2_dx3, x3_ad.getAdjoint()[1]); + + std::cout << "y1 = " << value(y.first) << "\n" + << "y2 = " << value(y.second) << "\n" + << "\nfirst order derivatives of y1:\n" + << "dy1/dx0 = " << x0_ad.getAdjoint()[0] << "\n" + << "dy1/dx1 = " << x1_ad.getAdjoint()[0] << "\n" + << "dy1/dx2 = " << x2_ad.getAdjoint()[0] << "\n" + << "dy1/dx3 = " << x3_ad.getAdjoint()[0] << "\n" + << "\nfirst order derivatives of y2:\n" + << "dy2/dx0 = " << x0_ad.getAdjoint()[1] << "\n" + << "dy2/dx1 = " << x1_ad.getAdjoint()[1] << "\n" + << "dy2/dx2 = " << x2_ad.getAdjoint()[1] << "\n" + << "dy2/dx3 = " << x3_ad.getAdjoint()[1] << "\n"; +} diff --git a/src/utils/constants_from_json.cc b/src/utils/constants_from_json.cc index 7b8b601a..21277f5b 100644 --- a/src/utils/constants_from_json.cc +++ b/src/utils/constants_from_json.cc @@ -45,8 +45,12 @@ template <> auto CameraMatrixFromJson(nlohmann::json intrinsics) -> Eigen::Matrix3d { Eigen::Matrix3d K; - K << intrinsics["fx"], 0, intrinsics["cx"], 0, intrinsics["fy"], - intrinsics["cy"], 0, 0, 1; + // clang-format off + K << + 1, 0, 0, + static_cast(intrinsics["cx"]), -static_cast(intrinsics["fx"]), 0, + static_cast(intrinsics["cy"]), 0, -static_cast(intrinsics["fy"]); + // clang-format on return K; }