diff --git a/AGENTS.md b/AGENTS.md index a55ea71..157130a 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -13,3 +13,8 @@ build-rmcs --packages-up-to rmcs_auto_aim_v2 cmake -B build cmake --build build -j ``` + + +2. 规范 + +- 项目头文件遵循最小引入原则，如果某个类型通过头文件间接引入了，那就不需要再引入该头文件 diff --git a/src/kernel/auto_aim.cpp b/src/kernel/auto_aim.cpp index 93ace4e..c744cbe 100644 --- a/src/kernel/auto_aim.cpp +++ b/src/kernel/auto_aim.cpp @@ -142,11 +142,11 @@ struct AutoAim::Impl { cmd.pitch = result->pitch; cmd.robot_center = result->center_position; - if (result->feedforward.has_value()) { - cmd.yaw_rate = result->feedforward->yaw_rate; - cmd.pitch_rate = result->feedforward->pitch_rate; - cmd.yaw_acc = result->feedforward->yaw_acc; - cmd.pitch_acc = result->feedforward->pitch_acc; + if (auto feedforward = result->feedforward) { + cmd.yaw_rate = feedforward->yaw_rate; + cmd.pitch_rate = feedforward->pitch_rate; + cmd.yaw_acc = feedforward->yaw_acc; + cmd.pitch_acc = feedforward->pitch_acc; } auto impact_armors = snapshot->predicted_armors(result->impact_time); @@ -154,6 +154,14 @@ struct AutoAim::Impl { visual.update_aiming_direction(cmd.yaw, cmd.pitch); visual.update_mpc_plan(cmd.yaw, cmd.pitch, cmd.yaw_rate, cmd.pitch_rate, cmd.yaw_acc, cmd.pitch_acc); + + if (auto aim_2d = estimator.make_point2d(result->aim_point)) { + visual.draw_later(Canvas::Point { + .origin = aim_2d->make(), + .radius = 5, + .color = result->shoot_permitted ? kRed : kGreen, + }); + } } } visual.draw_later( diff --git a/src/kernel/fire_control.cpp b/src/kernel/fire_control.cpp index d12e8bc..3eebee4 100644 --- a/src/kernel/fire_control.cpp +++ b/src/kernel/fire_control.cpp @@ -162,6 +162,7 @@ struct FireControl::Impl { .feedforward = feedforward, .shoot_permitted = shoot_permitted, .center_position = Point3d { center_position }, + .aim_point = Point3d { aim_point_position }, .impact_time = target_solution.impact_time, }; } diff --git a/src/kernel/fire_control.hpp b/src/kernel/fire_control.hpp index 9863343..67facda 100644 --- a/src/kernel/fire_control.hpp +++ b/src/kernel/fire_control.hpp @@ -30,6 +30,7 @@ class FireControl { std::optional feedforward; bool shoot_permitted; Point3d center_position; + Point3d aim_point; TimePoint impact_time; }; diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index fe04e31..fc37af2 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -117,11 +117,11 @@ struct PoseEstimator::Impl { auto transformed = armor; - auto position = Eigen::Vector3d {}; + auto position = Eigen::Vector3d { }; transformed.translation.copy_to(position); transformed.translation = camera_orientation.inverse() * (position - camera_translation); - auto quat = Eigen::Quaterniond {}; + auto quat = Eigen::Quaterniond { }; transformed.orientation.copy_to(quat); transformed.orientation = camera_orientation.inverse() * quat; @@ -271,6 +271,21 @@ struct PoseEstimator::Impl { } return armor3ds; } + + auto make_point2d(const Point3d& point_odom) const -> std::optional { + const auto t = camera_feature.translation.make(); + const auto q = camera_feature.orientation.make(); + + // odom (ROS) -> camera (OpenCV) + const auto point_ros_odom = point_odom.make(); + const auto point_ros_camera = Eigen::Vector3d { q.inverse() * (point_ros_odom - t) }; + + const auto point_opencv = ros2opencv_position(point_ros_camera); + const auto point_cv = Point3d { point_opencv }; + + // reproject + return util::reproject_point(point_cv, camera_feature); + } }; auto PoseEstimator::initialize(const YAML::Node& yaml) noexcept @@ -298,6 +313,10 @@ auto PoseEstimator::update_camera_transform(const Transform& transform) -> void return pimpl->update_camera_transform(transform); } +auto PoseEstimator::make_point2d(const Point3d& point_odom) const -> std::optional { + return pimpl->make_point2d(point_odom); +} + PoseEstimator::PoseEstimator() noexcept : pimpl { std::make_unique() } { } diff --git a/src/kernel/pose_estimator.hpp b/src/kernel/pose_estimator.hpp index 8cd5e31..cfda6ec 100644 --- a/src/kernel/pose_estimator.hpp +++ b/src/kernel/pose_estimator.hpp @@ -33,6 +33,8 @@ class PoseEstimator { auto estimate_armor(const std::vector&) const -> Armor3ds; auto estimate_armor(const std::vector&, Image&) const -> Armor3ds; + auto make_point2d(const Point3d& point_odom) const -> std::optional; + auto addition() -> const Addition&; }; diff --git a/src/module/predictor/model/outpost.cpp b/src/module/predictor/model/outpost.cpp new file mode 100644 index 0000000..17fcaea --- /dev/null +++ b/src/module/predictor/model/outpost.cpp @@ -0,0 +1,191 @@ +#include "outpost.hpp" + +#include "utility/math/angle.hpp" +#include "utility/robot/constant.hpp" + +#include + +using namespace rmcs; + +struct OutpostModel::Impl { + using StateVector = Eigen::Matrix; + using Covariance = Eigen::Matrix; + using ProcessNoise = Eigen::Matrix; + using ObservationNoise = Eigen::Matrix; + using ObservationVector = Eigen::Matrix; + using ObservationJacobian = Eigen::Matrix; + using KalmanGain = Eigen::Matrix; + + static auto observation_jacobian() { + auto jacobian = ObservationJacobian { }; + // clang-format off + jacobian << + 1, 0, 0, 0, 0, + 0, 1, 0, 0, 0, + 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1; + // clang-format on + return jacobian; + } + + struct Context { + StateVector posteriors_state = StateVector::Zero(); + Covariance posteriors_covariance = Covariance::Identity(); + ProcessNoise noise_process = ProcessNoise::Zero(); + ObservationNoise noise_observation = ObservationNoise::Zero(); + + Context() noexcept { + noise_process.diagonal() << 1e-4, 1e-4, 1e-4, 1e-2, 1e-2; + noise_observation.diagonal() << 0.09, 0.09, 0.09, 9e-2; + } + + static_assert(sizeof(State) == sizeof(posteriors_state)); + auto state() noexcept -> State& { + auto* data_pointer = &posteriors_state; + return *reinterpret_cast(data_pointer); + } + auto state() const noexcept -> const State& { + const auto* data_pointer = &posteriors_state; + return *reinterpret_cast(data_pointer); + } + + auto update_center(const Eigen::Vector3d& point) noexcept { + state().x = point.x(); + state().y = point.y(); + state().z = point.z(); + } + } context; + + auto initialize_from(const Armor3d& armor) noexcept -> void { + constexpr auto pitch = kPredictedOutpostArmorPitch; + constexpr auto radius = kOutpostRadius; + + const auto translation = armor.translation.make(); + const auto orientation = armor.orientation.make(); + + const auto backward = Eigen::Vector3d { orientation * Eigen::Vector3d::UnitX() }; + const auto armor_to_center = Eigen::Vector3d { + Eigen::AngleAxisd { -pitch, orientation * Eigen::Vector3d::UnitY() } * backward + }; + const auto center = Eigen::Vector3d { translation + radius * armor_to_center }; + + context.update_center(center); + context.state().rotation_speed = 0.0; + context.state().rotation_angle = + util::normalize_angle(std::atan2(-armor_to_center.y(), -armor_to_center.x())); + + auto initial_covariance_diagonal = Eigen::Matrix { }; + initial_covariance_diagonal << 1.0, 1.0, 1.0, 64.0, 0.4; + + context.posteriors_covariance = initial_covariance_diagonal.asDiagonal(); + } + + static auto predict_state(double dt, const State& last) -> State { + auto next = State { last }; + + next.rotation_angle = util::normalize_angle(last.rotation_angle + last.rotation_speed * dt); + return next; + } + + auto predict_covariance(double dt, const Covariance& covariance) const -> Covariance { + auto jacobian = Covariance { Covariance::Identity() }; + jacobian(4, 3) = dt; + return jacobian * covariance * jacobian.transpose() + context.noise_process; + } + + static auto measure_innovation(const ObservationVector& observation, const State& prior_state) + -> ObservationVector { + auto predicted_observation = ObservationVector { }; + predicted_observation << prior_state.x, prior_state.y, prior_state.z, + prior_state.rotation_angle; + + auto innovation = ObservationVector { observation - predicted_observation }; + innovation.coeffRef(3) = util::normalize_angle(innovation.coeff(3)); + return innovation; + } + + auto calculate_kalman_gain(const Covariance& prior_covariance) const -> KalmanGain { + const auto jacobian = observation_jacobian(); + + const auto innovation_covariance = + jacobian * prior_covariance * jacobian.transpose() + context.noise_observation; + + return prior_covariance * jacobian.transpose() + * innovation_covariance.ldlt().solve(ObservationNoise::Identity()); + } + + auto a_posteriori_update(const State& prior_state, const Covariance& prior_covariance, + const KalmanGain& kalman_gain, const ObservationVector& innovation, + const ObservationJacobian& jacobian) const -> std::pair { + + auto posterior_state = State { prior_state }; + reinterpret_cast(posterior_state).noalias() += kalman_gain * innovation; + posterior_state.rotation_angle = util::normalize_angle(posterior_state.rotation_angle); + + const auto complement = Covariance::Identity() - kalman_gain * jacobian; + auto posterior_covariance = Covariance { }; + posterior_covariance.noalias() = complement * prior_covariance * complement.transpose(); + posterior_covariance += + (kalman_gain * context.noise_observation * kalman_gain.transpose()).eval(); + posterior_covariance = 0.5 * (posterior_covariance + posterior_covariance.transpose()); + + return { posterior_state, posterior_covariance }; + } + + auto predict(double dt) noexcept -> void { + const auto prior_state = predict_state(dt, context.state()); + const auto prior_covariance = predict_covariance(dt, context.posteriors_covariance); + + context.state() = prior_state; + context.posteriors_covariance = prior_covariance; + } + + auto correct(const Armor3d& armor) noexcept -> void { + { // 切板检测 + } + + constexpr auto pitch = kPredictedOutpostArmorPitch; + + const auto orientation = armor.orientation.make(); + const auto translation = armor.translation.make(); + + const auto backward = Eigen::Vector3d { orientation * Eigen::Vector3d::UnitX() }; + const auto armor_to_center = Eigen::Vector3d { + Eigen::AngleAxisd { -pitch, orientation * Eigen::Vector3d::UnitY() } * backward + }; + + const auto center = Eigen::Vector3d { translation + kOutpostRadius * armor_to_center }; + + const auto top_armor_yaw = + util::normalize_angle(std::atan2(-armor_to_center.y(), -armor_to_center.x())); + + auto observation = ObservationVector { }; + observation << center.x(), center.y(), center.z(), top_armor_yaw; + + const auto prior_state = context.state(); + const auto prior_covariance = context.posteriors_covariance; + + const auto innovation = measure_innovation(observation, prior_state); + const auto kalman_gain = calculate_kalman_gain(prior_covariance); + const auto jacobian = observation_jacobian(); + + const auto [posterior_state, posterior_covariance] = + a_posteriori_update(prior_state, prior_covariance, kalman_gain, innovation, jacobian); + + context.state() = posterior_state; + context.posteriors_covariance = posterior_covariance; + } +}; + +OutpostModel::OutpostModel(const Armor3d& armor) noexcept + : pimpl { std::make_unique() } { + pimpl->initialize_from(armor); +} + +OutpostModel::~OutpostModel() noexcept = default; + +auto OutpostModel::predict(double dt) noexcept -> void { pimpl->predict(dt); } + +auto OutpostModel::correct(const Armor3d& armor) noexcept -> void { pimpl->correct(armor); } + +auto OutpostModel::state() noexcept -> State { return pimpl->context.state(); } diff --git a/src/module/predictor/model/outpost.hpp b/src/module/predictor/model/outpost.hpp new file mode 100644 index 0000000..ea0f03e --- /dev/null +++ b/src/module/predictor/model/outpost.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "utility/pimpl.hpp" +#include "utility/robot/armor.hpp" + +namespace rmcs { + +class OutpostModel { + RMCS_PIMPL_DEFINITION(OutpostModel) + +public: + struct State { + // 前哨站旋转中心在世界坐标系下的位置 + double x; + double y; + double z; + + // 以参考装甲板朝向为基准的角速度与 yaw 角 + double rotation_speed; + double rotation_angle; + }; + + explicit OutpostModel(const Armor3d& armor) noexcept; + + auto predict(double dt) noexcept -> void; + auto correct(const Armor3d& armor) noexcept -> void; + auto state() noexcept -> State; +}; + +} diff --git a/src/module/predictor/model/robot.hpp b/src/module/predictor/model/robot.hpp new file mode 100644 index 0000000..91df6ce --- /dev/null +++ b/src/module/predictor/model/robot.hpp @@ -0,0 +1,7 @@ +#pragma once + +namespace rmcs { + +class RobotModel { }; + +} diff --git a/src/module/predictor/outpost/ekf_parameter.hpp b/src/module/predictor/outpost/ekf_parameter.hpp index 0b085f4..098a4bc 100644 --- a/src/module/predictor/outpost/ekf_parameter.hpp +++ b/src/module/predictor/outpost/ekf_parameter.hpp @@ -35,13 +35,13 @@ struct OutpostEKFParameters { auto const center_x = obs.xyz[0] + kOutpostRadius * std::cos(yaw); auto const center_y = obs.xyz[1] + kOutpostRadius * std::sin(yaw); - auto x = EKF::XVec {}; + auto x = EKF::XVec { }; x << center_x, 0.0, center_y, 0.0, obs.xyz[2], yaw; return x; } static auto P_initial_dig() -> EKF::PDig { - auto P_dig = EKF::PDig {}; + auto P_dig = EKF::PDig { }; P_dig << 1.0, 64.0, 1.0, 64.0, 1.0, 0.4; return P_dig; } @@ -69,7 +69,7 @@ struct OutpostEKFParameters { const auto ypd = util::xyz2ypd(xyz); const auto yaw = armor_yaw(x, phase_offset); - auto z = EKF::ZVec {}; + auto z = EKF::ZVec { }; z << ypd[0], ypd[1], ypd[2], yaw; return z; } @@ -89,7 +89,7 @@ struct OutpostEKFParameters { } static auto F(double dt) -> EKF::AMat { - auto F = EKF::AMat {}; + auto F = EKF::AMat { }; // clang-format off F << 1, dt, 0, 0, 0, 0, @@ -117,7 +117,7 @@ struct OutpostEKFParameters { const auto b = dt * dt * dt / 2.0; const auto c = dt * dt; - auto Q = EKF::QMat {}; + auto Q = EKF::QMat { }; // clang-format off Q << a * v1, b * v1, 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, @@ -151,7 +151,7 @@ struct OutpostEKFParameters { } static auto z(ArmorObservation const& obs) -> EKF::ZVec { - auto z = EKF::ZVec {}; + auto z = EKF::ZVec { }; z << obs.ypd[0], obs.ypd[1], obs.ypd[2], obs.ypr[0]; return z; } @@ -161,7 +161,7 @@ struct OutpostEKFParameters { const auto delta_yaw = util::normalize_angle(obs.ypr[0] - center_yaw); const auto distance = obs.ypd[2]; - auto R_dig = EKF::RDig {}; + auto R_dig = EKF::RDig { }; // clang-format off R_dig << 4e-3, 4e-3, std::log(std::abs(delta_yaw) + 1.0) + 1.0, std::log(std::abs(distance) + 1.0) / 200.0 + 9e-2; @@ -177,7 +177,7 @@ struct OutpostEKFParameters { const auto dx_da = kOutpostRadius * sin_phase; const auto dy_da = -kOutpostRadius * cos_phase; - auto H_armor_xyza = Eigen::Matrix {}; + auto H_armor_xyza = Eigen::Matrix { }; // clang-format off H_armor_xyza << 1, 0, 0, 0, 0, dx_da, diff --git a/src/utility/math/corners_optimizor.cpp b/src/utility/math/corners_optimizor.cpp index 5ef13b5..7c50eac 100644 --- a/src/utility/math/corners_optimizor.cpp +++ b/src/utility/math/corners_optimizor.cpp @@ -129,7 +129,11 @@ namespace { } +/// @FIXME: 角点优化的实现存在问题，等待后续修复再开启 + auto optimize_corners(const Image& image, Lightbar2d& lightbar) -> void { + return; // @FIXME: + const auto& mat = image.details().mat; if (mat.empty()) return; @@ -188,6 +192,8 @@ auto optimize_corners(const Image& image, Lightbar2d& lightbar) -> void { } auto optimize_corners(const Image& image, Armor2ds& armors) -> void { + return; // @FIXME: + for (auto& armor : armors) { auto left = Lightbar2d { .color = armor.color, diff --git a/src/utility/math/outpost.cpp b/src/utility/math/outpost.cpp index ac7c7cd..02f1074 100644 --- a/src/utility/math/outpost.cpp +++ b/src/utility/math/outpost.cpp @@ -1,23 +1,51 @@ #include "utility/math/outpost.hpp" #include "utility/robot/constant.hpp" + #include namespace rmcs::util { -auto NeighborBarSolution::solve() -> void { +namespace details { + + using ArmorLevel = OutpostSolution::ArmorLevel; + + constexpr auto get_transform(ArmorLevel from, ArmorLevel to) -> std::tuple { + constexpr auto step = kOutpostArmorHeightStep; + constexpr auto turn = 2.0 * std::numbers::pi / 3.0; + + if (from == to) return { 0.0, 0.0 }; + + if (from == ArmorLevel::UPPER && to == ArmorLevel::MIDDLE) return { -1 * step, +turn }; + if (from == ArmorLevel::MIDDLE && to == ArmorLevel::LOWER) return { -1 * step, +turn }; + if (from == ArmorLevel::LOWER && to == ArmorLevel::UPPER) return { +2. * step, +turn }; + + if (from == ArmorLevel::UPPER && to == ArmorLevel::LOWER) return { -2. * step, -turn }; + if (from == ArmorLevel::MIDDLE && to == ArmorLevel::UPPER) return { +1 * step, -turn }; + if (from == ArmorLevel::LOWER && to == ArmorLevel::MIDDLE) return { +1 * step, -turn }; + + return { 0.0, 0.0 }; + } + + constexpr auto get_level(bool is_right, bool is_upper) -> std::tuple { + if (is_right && is_upper) return { ArmorLevel::LOWER, ArmorLevel::UPPER }; + if (is_right && !is_upper) return { ArmorLevel::UPPER, ArmorLevel::MIDDLE }; + if (!is_right && is_upper) return { ArmorLevel::MIDDLE, ArmorLevel::UPPER }; + return { ArmorLevel::UPPER, ArmorLevel::LOWER }; + } + +} + +auto OutpostSolution::solve() -> void { const auto radius = kOutpostRadius + input.armor_thickness; const auto pitch = kPredictedOutpostArmorPitch; - const auto& armor = input.source; - - const auto q = armor.orientation.make(); - const auto t = armor.translation.make(); + const auto q = input.orientation.make(); + const auto t = input.translation.make(); // 先求出旋转中心，按照装甲板的朝向反推即可，注意 15 度的倾角 const auto backward = Eigen::Vector3d { q * Eigen::Vector3d::UnitX() }; const auto armor2center = Eigen::Vector3d { Eigen::AngleAxisd { -pitch, q * Eigen::Vector3d::UnitY() } * backward }; - const auto rotation_center = Eigen::Vector3d { t + radius * armor2center }; // 前哨站向上的单位向量，即水平向量绕 Y 轴朝向旋转 90 度 @@ -25,27 +53,47 @@ auto NeighborBarSolution::solve() -> void { Eigen::AngleAxisd { -0.5 * std::numbers::pi, q * Eigen::Vector3d::UnitY() } * armor2center }; + const auto [height_diff, rotate_angle] = details::get_transform(input.source, input.target); + // 逆时针旋转为正，所以在右边需要 +，旋转 1/3 个圆 - const auto rotate_sign = input.in_right ? +1 : -1; - const auto rotate2next = - Eigen::AngleAxisd { rotate_sign * 2 * std::numbers::pi / 3., vertical }; + const auto rotate2target = Eigen::AngleAxisd { rotate_angle, vertical }; + const auto target_center = Eigen::Vector3d { rotation_center + + rotate2target * (-armor2center * radius) + vertical * height_diff }; + const auto toward = Eigen::Vector3d { rotate2target * q * Eigen::Vector3d::UnitX() }; - const auto next_center = - Eigen::Vector3d { rotation_center + rotate2next * (-armor2center * radius) }; + const auto x_axis = Eigen::Vector3d { toward.normalized() }; + const auto y_axis = Eigen::Vector3d { vertical.cross(x_axis).normalized() }; + const auto z_axis = Eigen::Vector3d { x_axis.cross(y_axis).normalized() }; - const auto steps = std::array { - (input.in_right ? +2 : +1) * kOutpostArmorHeightStep, - (input.in_right ? -1 : -2) * kOutpostArmorHeightStep, - }; - const auto flags = std::array { true, false }; - for (auto&& [step, is_upper] : std::views::zip(steps, flags)) { - const auto center = Eigen::Vector3d { next_center + vertical * step }; - const auto toward = Eigen::Vector3d { rotate2next - * armor.orientation.make() * Eigen::Vector3d::UnitX() }; + auto rotation_matrix = Eigen::Matrix3d { }; + rotation_matrix << x_axis, y_axis, z_axis; + + result.center = Point3d { rotation_center }; + result.translation = Translation { target_center }; + result.orientation = Orientation { Eigen::Quaterniond { rotation_matrix }.normalized() }; +} + +auto NeighborBarSolution::solve() -> void { + const auto& armor = input.source; + + auto solution = OutpostSolution { }; + + solution.input.translation = armor.translation; + solution.input.orientation = armor.orientation; + solution.input.armor_thickness = input.armor_thickness; + + for (const auto is_upper : { true, false }) { + const auto [source, target] = details::get_level(input.in_right, is_upper); + + solution.input.source = source; + solution.input.target = target; + solution.solve(); + + const auto center = solution.result.translation.make(); + const auto q = solution.result.orientation.make(); - const auto x_axis = Eigen::Vector3d { toward.normalized() }; - const auto y_axis = Eigen::Vector3d { vertical.cross(x_axis).normalized() }; - const auto z_axis = Eigen::Vector3d { x_axis.cross(y_axis).normalized() }; + const auto y_axis = Eigen::Vector3d { q * Eigen::Vector3d::UnitY() }; + const auto z_axis = Eigen::Vector3d { q * Eigen::Vector3d::UnitZ() }; const auto y_step = 0.5 * kSmallArmorWidth; const auto z_step = 0.5 * kLightBarHeight; @@ -76,7 +124,7 @@ auto NeighborBarSolution::solve() -> void { (is_upper ? result.upper_near : result.lower_near) = near; (is_upper ? result.upper_away : result.lower_away) = away; - result.center = rotation_center; + result.center = solution.result.center; } } diff --git a/src/utility/math/outpost.hpp b/src/utility/math/outpost.hpp index 81a86dc..063aded 100644 --- a/src/utility/math/outpost.hpp +++ b/src/utility/math/outpost.hpp @@ -3,6 +3,32 @@ namespace rmcs::util { +class OutpostSolution { +public: + enum class ArmorLevel { UPPER, MIDDLE, LOWER }; + + /// Level 按照下面的顺序排列，其旋向确定 + /// [ UPPER ] + /// [ MIDDLE ] + /// [ LOWER ] + struct Input { + Translation translation; + Orientation orientation; + ArmorLevel source; + ArmorLevel target; + + double armor_thickness = 0; + } input; + + struct Result { + Point3d center; + Translation translation; + Orientation orientation; + } result; + + auto solve() -> void; +}; + class NeighborBarSolution { public: struct Input { diff --git a/src/utility/math/reprojection.cpp b/src/utility/math/reprojection.cpp index 9ba34b8..5c75c3f 100644 --- a/src/utility/math/reprojection.cpp +++ b/src/utility/math/reprojection.cpp @@ -2,8 +2,6 @@ #include -#include - namespace rmcs::details { auto project_points::impl(std::span object_points, const cv::Mat& intrinsic, @@ -19,3 +17,30 @@ auto project_points::impl(std::span object_points, const cv:: } } + +namespace rmcs::util { + +auto reproject_point(const Point3d& point_camera, const util::CameraFeature& camera) + -> std::optional { + + auto projected = std::vector { }; + { + const auto object_points = std::vector { + cv::Point3f { + static_cast(point_camera.x), + static_cast(point_camera.y), + static_cast(point_camera.z), + }, + }; + const auto zero = cv::Vec3d { 0, 0, 0 }; + cv::projectPoints( + object_points, zero, zero, camera.intrinsic(), camera.distortion(), projected); + } + + if (projected.empty()) { + return std::nullopt; + } + return Point2d { projected[0] }; +} + +} diff --git a/src/utility/math/reprojection.hpp b/src/utility/math/reprojection.hpp index 27dbc9e..5be9788 100644 --- a/src/utility/math/reprojection.hpp +++ b/src/utility/math/reprojection.hpp @@ -63,4 +63,8 @@ struct ReprojectionSolution { } }; +namespace util { + auto reproject_point(const Point3d& point_camera, const util::CameraFeature& camera) + -> std::optional; +} } diff --git a/tool/cxx/CMakeLists.txt b/tool/cxx/CMakeLists.txt index 107fc37..fe5dae3 100644 --- a/tool/cxx/CMakeLists.txt +++ b/tool/cxx/CMakeLists.txt @@ -67,6 +67,21 @@ target_link_libraries( ${geometry_msgs_LIBRARIES} ) +add_executable( + test_outpost_ekf + ${TOOL_DIR}/test_outpost_ekf.cpp + ${RMCS_SRC_DIR}/module/predictor/model/outpost.cpp + ${RMCS_SRC_DIR}/utility/math/outpost.cpp + ${RMCS_SRC_DIR}/utility/panic.cpp + ${RMCS_SRC_DIR}/utility/rclcpp/node.cpp +) +target_link_libraries( + test_outpost_ekf + rclcpp::rclcpp + ${visualization_msgs_LIBRARIES} + ${geometry_msgs_LIBRARIES} +) + add_executable( ov-model-status ${TOOL_DIR}/ov_model_status.cpp @@ -99,6 +114,7 @@ if(HIKCAMERA_AVAILABLE) hikcamera ${TOOL_DIR}/hikcamera.cpp ${RMCS_SRC_DIR}/utility/image/recorder.cpp + ${RMCS_SRC_DIR}/utility/singleton/running.cpp ${RMCS_SRC_DIR}/module/debug/visualization/stream_session.cpp ${RMCS_SRC_DIR}/module/debug/visualization/stream_context.cpp ) @@ -107,6 +123,7 @@ if(HIKCAMERA_AVAILABLE) tool_terminal ${OpenCV_LIBS} ${hikcamera_LIBRARIES} + rclcpp::rclcpp ) # Hikcamera Test diff --git a/tool/cxx/test_outpost_ekf.cpp b/tool/cxx/test_outpost_ekf.cpp new file mode 100644 index 0000000..7e4e1f5 --- /dev/null +++ b/tool/cxx/test_outpost_ekf.cpp @@ -0,0 +1,256 @@ +#include "module/predictor/model/outpost.hpp" +#include "utility/math/angle.hpp" +#include "utility/math/linear.hpp" +#include "utility/rclcpp/node.details.hpp" +#include "utility/robot/armor.hpp" +#include "utility/robot/constant.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace rmcs; +using namespace rmcs::util; + +auto main() -> int { + rclcpp::init(0, nullptr); + + auto node = RclcppNode { "test_outpost_ekf" }; + node.set_pub_topic_prefix("/rmcs/auto_aim/outpost_test/"); + + auto marker_publisher = node.details->make_pub( + "/rmcs/auto_aim/outpost_test/ekf", qos::debug); + + const auto true_center = Eigen::Vector3d { 2.0, 0.0, 2.0 }; + constexpr auto angular_speed = kOutpostAngularSpeed; // +0.8 pi, CCW + constexpr auto radius = kOutpostRadius; + constexpr auto pitch = kPredictedOutpostArmorPitch; + constexpr auto dt = 0.01; + + constexpr auto noise_sigma = 0.30; + constexpr auto outlier_probability = 0.05; + constexpr auto outlier_sigma = 1.0; + constexpr auto orientation_sigma = 0.05; + constexpr auto observation_cloud_capacity = 50; + + auto noise_engine = std::mt19937 { 42 }; + auto noise_distribution = std::normal_distribution { 0.0, noise_sigma }; + auto outlier_distribution = std::normal_distribution { 0.0, outlier_sigma }; + auto orientation_noise_distribution = + std::normal_distribution { 0.0, orientation_sigma }; + auto uniform_distribution = std::uniform_real_distribution { 0.0, 1.0 }; + + auto model = std::unique_ptr { }; + auto elapsed = 0.0; + auto frame_index = 0; + auto observation_cloud = std::deque { }; + + const auto start_time = std::chrono::steady_clock::now(); + + while (rclcpp::ok()) { + elapsed += dt; + const auto stamp = node.details->rclcpp->now(); + + const auto angle = angular_speed * elapsed; + const auto true_position = Eigen::Vector3d { + true_center.x() + radius * std::cos(angle), + true_center.y() + radius * std::sin(angle), + true_center.z(), + }; + const auto direction = true_position - true_center; + const auto yaw_rotation = + Eigen::AngleAxisd { std::atan2(direction.y(), direction.x()) + std::numbers::pi, + Eigen::Vector3d::UnitZ() }; + const auto pitch_rotation = Eigen::AngleAxisd { pitch, Eigen::Vector3d::UnitY() }; + const auto true_orientation = Eigen::Quaterniond { yaw_rotation * pitch_rotation }; + + auto distance_direction = Eigen::Vector3d { true_position.normalized() }; + auto noisy_position = Eigen::Vector3d { true_position + + distance_direction * noise_distribution(noise_engine) }; + + auto is_outlier = uniform_distribution(noise_engine) < outlier_probability; + if (is_outlier) { + noisy_position += distance_direction * outlier_distribution(noise_engine); + } + + observation_cloud.push_back(noisy_position); + if (observation_cloud.size() > observation_cloud_capacity) { + observation_cloud.pop_front(); + } + + const auto orientation_perturbation = + Eigen::AngleAxisd { orientation_noise_distribution(noise_engine), + Eigen::Vector3d::UnitZ() }; + const auto noisy_orientation = + Eigen::Quaterniond { orientation_perturbation * true_orientation }; + + auto noisy_armor = Armor3d { + .genre = DeviceId::OUTPOST, + .color = ArmorColor::BLUE, + .id = 0, + .translation = Translation { noisy_position }, + .orientation = Orientation { noisy_orientation }, + }; + + if (!model) { + model = std::make_unique(noisy_armor); + } else { + model->predict(dt); + model->correct(noisy_armor); + } + + const auto estimated_state = model->state(); + const auto estimated_center = Eigen::Vector3d { + estimated_state.x, + estimated_state.y, + estimated_state.z, + }; + const auto estimated_armor_position = Eigen::Vector3d { + estimated_center.x() - radius * std::cos(estimated_state.rotation_angle), + estimated_center.y() - radius * std::sin(estimated_state.rotation_angle), + estimated_center.z(), + }; + const auto estimated_orientation = Eigen::Quaterniond { + Eigen::AngleAxisd { estimated_state.rotation_angle, Eigen::Vector3d::UnitZ() } + * Eigen::AngleAxisd { pitch, Eigen::Vector3d::UnitY() } + }; + + auto marker_array = visualization_msgs::msg::MarkerArray { }; + auto marker_index = 0; + + auto make_marker = [&](const std::string& namespace_name) { + auto marker = visualization_msgs::msg::Marker { }; + marker.header.frame_id = "camera_link"; + marker.header.stamp = stamp; + marker.ns = namespace_name; + marker.id = marker_index++; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + return marker; + }; + + auto make_point = [](const Eigen::Vector3d& position) { + auto point = geometry_msgs::msg::Point { }; + point.x = position.x(); + point.y = position.y(); + point.z = position.z(); + return point; + }; + + auto set_pose = [](visualization_msgs::msg::Marker& marker, const Eigen::Vector3d& position, + const Eigen::Quaterniond& orientation) { + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = position.z(); + marker.pose.orientation.w = orientation.w(); + marker.pose.orientation.x = orientation.x(); + marker.pose.orientation.y = orientation.y(); + marker.pose.orientation.z = orientation.z(); + }; + + { + auto marker = make_marker("true_armor"); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale.x = 0.003; + marker.scale.y = 0.135; + marker.scale.z = 0.056; + marker.color.g = 1.0; + marker.color.a = 1.0; + set_pose(marker, true_position, true_orientation); + marker_array.markers.emplace_back(std::move(marker)); + } + + { + auto marker = make_marker("observed_armor"); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale.x = 0.003; + marker.scale.y = 0.135; + marker.scale.z = 0.056; + marker.color.r = 1.0; + marker.color.b = 1.0; + marker.color.a = 0.7; + set_pose(marker, noisy_position, noisy_orientation); + marker_array.markers.emplace_back(std::move(marker)); + } + + { + auto marker = make_marker("noisy_observation"); + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.color.r = 1.0; + marker.color.a = 0.6; + for (const auto& position : observation_cloud) { + marker.points.emplace_back(make_point(position)); + } + marker_array.markers.emplace_back(std::move(marker)); + } + + { + auto marker = make_marker("estimated_center"); + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.scale.x = 0.06; + marker.scale.y = 0.06; + marker.scale.z = 0.06; + marker.color.b = 1.0; + marker.color.a = 1.0; + set_pose(marker, estimated_center, Eigen::Quaterniond::Identity()); + marker_array.markers.emplace_back(std::move(marker)); + } + + { + auto marker = make_marker("estimated_armor"); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale.x = 0.003; + marker.scale.y = 0.135; + marker.scale.z = 0.056; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.a = 1.0; + set_pose(marker, estimated_armor_position, estimated_orientation); + marker_array.markers.emplace_back(std::move(marker)); + } + + { + auto marker = make_marker("estimated_circle"); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.scale.x = 0.005; + marker.color.b = 1.0; + marker.color.a = 0.5; + for (int step = 0; step <= 36; ++step) { + const auto phi = 2.0 * std::numbers::pi * step / 36.0; + marker.points.emplace_back(make_point(Eigen::Vector3d { + estimated_center.x() + radius * std::cos(phi), + estimated_center.y() + radius * std::sin(phi), + estimated_center.z(), + })); + } + marker_array.markers.emplace_back(std::move(marker)); + } + + marker_publisher->publish(marker_array); + + if (frame_index % 10 == 0) { + const auto center_error = (estimated_center - true_center).norm(); + const auto true_yaw = util::normalize_angle(angle + std::numbers::pi); + const auto angle_error = + std::abs(util::normalize_angle(estimated_state.rotation_angle - true_yaw)); + std::cout << std::format("elapsed={:6.2f}s center_err={:.4f}m omega_est={:.4f} " + "omega_true={:.4f} angle_err={:.4f}rad{}\n", + elapsed, center_error, estimated_state.rotation_speed, angular_speed, angle_error, + is_outlier ? " [OUTLIER]" : ""); + } + + ++frame_index; + rclcpp::sleep_for(std::chrono::milliseconds { 10 }); + } + + return 0; +}