refactor: ekf and debug tool#62
Merged
Merged
Conversation
Contributor
|
Warning Review limit reached
More reviews will be available in 21 minutes and 30 seconds. Learn how PR review limits work. Your organization has used up its prepaid credits, and credit purchases are no longer available. Enable the review add-on in the billing tab to keep reviews running — you're only billed for reviews past your plan's rate limits ($0.25/file). ⌛ How to resolve this issue?After more reviews become available, a review can be triggered using the To avoid repeated limits, reduce automatic review volume by pausing incremental auto-reviews earlier, using label-based review opt-in, excluding WIP or generated PR titles, or requesting reviews manually when the PR is ready. If your team needs uninterrupted high-volume reviews, an organization admin can enable usage-based credits. 🚦 How do rate limits work?CodeRabbit enforces per-developer PR review limits for each organization. Most developers receive the normal plan refill rate. For paid Pro and Pro+ PR reviews, CodeRabbit uses adaptive limits for sustained high-volume activity. When a developer's recent PR review activity reaches the 95th percentile or higher among CodeRabbit users, the refill rate gradually slows as usage increases. The highest same-day bursts are limited more strictly. Please see our Fair Usage Limits Policy for further information. ℹ️ Review info⚙️ Run configurationConfiguration used: Organization UI Review profile: CHILL Plan: Pro Run ID: 📒 Files selected for processing (2)
Walkthrough新增前哨站 EKF 预测模型 Changes前哨站 EKF 预测器与瞄准点可视化
Sequence Diagram(s)sequenceDiagram
participant AutoAim
participant FireControl
participant PoseEstimator
participant util_reproject_point as util::reproject_point
AutoAim->>FireControl: solve(armors, ...)
FireControl-->>AutoAim: Result{..., aim_point}
AutoAim->>PoseEstimator: make_point2d(result->aim_point)
PoseEstimator->>PoseEstimator: 变换 odom→相机坐标系
PoseEstimator->>util_reproject_point: reproject_point(point_camera, camera)
util_reproject_point->>util_reproject_point: cv::projectPoints()
util_reproject_point-->>PoseEstimator: optional<Point2d>
PoseEstimator-->>AutoAim: optional<Point2d>
AutoAim->>AutoAim: 绘制彩色圆点标记（颜色取决于 shoot_permitted）
Estimated code review effort🎯 4 (Complex) | ⏱️ ~60 minutes Possibly related PRs
Suggested labels
Poem
🚥 Pre-merge checks | ✅ 4 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (4 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches🧪 Generate unit tests (beta)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
Contributor
There was a problem hiding this comment.
Actionable comments posted: 3
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
src/kernel/fire_control.cpp (1)
118-165:⚠️ Potential issue | 🟡 Minor | ⚡ Quick win用弹道解算后的目标点填充
aim_point。Line 165 输出的是 Line 104 在粗选阶段缓存的
aim_point_position；但返回的yaw/pitch来自target_solution。若两次预测时间不同，可视化点会和实际解算姿态不一致。建议修复
last_selected_armor_id = solution->candidate.armor.id; target_solution = std::move(*solution); + target_solution.candidate.armor.translation.copy_to(aim_point_position); }这样射击评估和
Result::aim_point都使用同一个解算结果目标点。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@src/kernel/fire_control.cpp` around lines 118 - 165, The issue is that the `aim_point_position` being returned in the Result on line 165 comes from the rough selection phase cached value (from line 104), but the actual `yaw` and `pitch` returned are derived from `target_solution` after trajectory planning and offset correction. This causes inconsistency when visualization points don't match the actual solved attitude. Fix this by calculating or deriving the `aim_point` from the same `target_solution` that produces the final `yaw` and `pitch` values, rather than using the earlier cached `aim_point_position` value, ensuring both the shoot evaluation and Result use the same ballistic solution target point.
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@src/kernel/pose_estimator.cpp`:
- Around line 279-287: The code is projecting points without validating their
visibility and validity, allowing points with z <= 0 (behind camera) or
non-finite coordinates to be reprojected, resulting in misleading debug markers.
Before calling util::reproject_point with point_cv, add a validation check to
ensure the z coordinate is positive and all coordinates are finite; if either
condition fails, return std::nullopt instead of attempting reprojection.
In `@src/module/predictor/model/outpost.cpp`:
- Around line 42-50: Remove the unsafe `reinterpret_cast` usage in the const and
non-const `state()` methods which directly reinterpret `posteriors_state` as
`State`. Replace these methods with explicit conversion functions that safely
map between `StateVector` (Eigen::Matrix<double, 5, 1>) and `State`. Create two
helper functions to handle the bidirectional conversion instead of using type
punning through reinterpret_cast, and apply the same fix to any other locations
(such as Line 122) where similar reinterpret_cast conversions occur between
these types.
In `@src/utility/math/reprojection.cpp`:
- Around line 25-41: The reproject_point function does not validate that the
input point has positive depth before attempting projection, which allows points
behind or at the camera (point_camera.z <= 0) to be processed and generate
misleading 2D projections. Add an early validation check at the beginning of the
reproject_point function that returns std::nullopt if point_camera.z is less
than or equal to zero. Additionally, ensure the file includes the <cmath> header
if it is not already present.
---
Outside diff comments:
In `@src/kernel/fire_control.cpp`:
- Around line 118-165: The issue is that the `aim_point_position` being returned
in the Result on line 165 comes from the rough selection phase cached value
(from line 104), but the actual `yaw` and `pitch` returned are derived from
`target_solution` after trajectory planning and offset correction. This causes
inconsistency when visualization points don't match the actual solved attitude.
Fix this by calculating or deriving the `aim_point` from the same
`target_solution` that produces the final `yaw` and `pitch` values, rather than
using the earlier cached `aim_point_position` value, ensuring both the shoot
evaluation and Result use the same ballistic solution target point.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: 0624faef-aef9-474c-90f7-7415fe83f565
📒 Files selected for processing (16)
AGENTS.mdsrc/kernel/auto_aim.cppsrc/kernel/fire_control.cppsrc/kernel/fire_control.hppsrc/kernel/pose_estimator.cppsrc/kernel/pose_estimator.hppsrc/module/predictor/model/outpost.cppsrc/module/predictor/model/outpost.hppsrc/module/predictor/model/robot.hppsrc/module/predictor/outpost/ekf_parameter.hppsrc/utility/math/outpost.cppsrc/utility/math/outpost.hppsrc/utility/math/reprojection.cppsrc/utility/math/reprojection.hpptool/cxx/CMakeLists.txttool/cxx/test_outpost_ekf.cpp
Comment on lines
+279
to
+287
| // odom (ROS) -> camera (OpenCV) | ||
| const auto point_ros_odom = point_odom.make<Eigen::Vector3d>(); | ||
| 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); |
Contributor
There was a problem hiding this comment.
在重投影前过滤相机后方/非法点
Line 281 之后直接投影会把 z <= 0 或非有限坐标也转成 2D 点，调试层会出现误导性的 aim_point 标记。建议在调用 util::reproject_point 前先做可见性与有限值检查，不满足时返回 std::nullopt。
建议修复
// odom (ROS) -> camera (OpenCV)
const auto point_ros_odom = point_odom.make<Eigen::Vector3d>();
const auto point_ros_camera = Eigen::Vector3d { q.inverse() * (point_ros_odom - t) };
+ if (!point_ros_camera.allFinite() || point_ros_camera.z() <= 0.0) {
+ return std::nullopt;
+ }
const auto point_opencv = ros2opencv_position(point_ros_camera);
const auto point_cv = Point3d { point_opencv };🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/kernel/pose_estimator.cpp` around lines 279 - 287, The code is projecting
points without validating their visibility and validity, allowing points with z
<= 0 (behind camera) or non-finite coordinates to be reprojected, resulting in
misleading debug markers. Before calling util::reproject_point with point_cv,
add a validation check to ensure the z coordinate is positive and all
coordinates are finite; if either condition fails, return std::nullopt instead
of attempting reprojection.
Comment on lines
+42
to
+50
| static_assert(sizeof(State) == sizeof(posteriors_state)); | ||
| auto state() noexcept -> State& { | ||
| auto* data_pointer = &posteriors_state; | ||
| return *reinterpret_cast<State*>(data_pointer); | ||
| } | ||
| auto state() const noexcept -> const State& { | ||
| const auto* data_pointer = &posteriors_state; | ||
| return *reinterpret_cast<const State*>(data_pointer); | ||
| } |
Contributor
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
rg -nP 'reinterpret_cast<\s*(const\s+)?State(\s*[*&])|reinterpret_cast<\s*(const\s+)?StateVector(\s*[*&])' src/module/predictor/model/outpost.cpp
rg -nP 'static_assert\s*\(\s*sizeof\(State\)\s*==\s*sizeof\(posteriors_state\)\s*\)' src/module/predictor/model/outpost.cppRepository: Alliance-Algorithm/rmcs_auto_aim_v2
Length of output: 379
🏁 Script executed:
cat -n src/module/predictor/model/outpost.cpp | head -160Repository: Alliance-Algorithm/rmcs_auto_aim_v2
Length of output: 7918
🏁 Script executed:
cat -n src/module/predictor/model/outpost.hppRepository: Alliance-Algorithm/rmcs_auto_aim_v2
Length of output: 867
移除 Lines 42-50 与 Line 122 的 reinterpret_cast 状态互转
直接重解释 State 与 Eigen::Matrix<double, 5, 1> 违反了严格别名规则（strict aliasing），即使通过 static_assert(sizeof(...)) 检查，也无法保证对象布局和内存访问安全，属于未定义行为。建议改为显式转换函数处理 StateVector 与 State 之间的映射。
建议修复示例
- static_assert(sizeof(State) == sizeof(posteriors_state));
- auto state() noexcept -> State& {
- auto* data_pointer = &posteriors_state;
- return *reinterpret_cast<State*>(data_pointer);
- }
- auto state() const noexcept -> const State& {
- const auto* data_pointer = &posteriors_state;
- return *reinterpret_cast<const State*>(data_pointer);
- }
+ static auto to_state(const StateVector& v) noexcept -> State {
+ return State {
+ .x = v[0],
+ .y = v[1],
+ .z = v[2],
+ .rotation_speed = v[3],
+ .rotation_angle = v[4],
+ };
+ }
+ static auto from_state(const State& s) noexcept -> StateVector {
+ auto v = StateVector { };
+ v << s.x, s.y, s.z, s.rotation_speed, s.rotation_angle;
+ return v;
+ }
@@
- auto posterior_state = State { prior_state };
- reinterpret_cast<StateVector&>(posterior_state).noalias() += kalman_gain * innovation;
+ auto posterior_vector = Context::from_state(prior_state);
+ posterior_vector.noalias() += kalman_gain * innovation;
+ auto posterior_state = Context::to_state(posterior_vector);🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/module/predictor/model/outpost.cpp` around lines 42 - 50, Remove the
unsafe `reinterpret_cast` usage in the const and non-const `state()` methods
which directly reinterpret `posteriors_state` as `State`. Replace these methods
with explicit conversion functions that safely map between `StateVector`
(Eigen::Matrix<double, 5, 1>) and `State`. Create two helper functions to handle
the bidirectional conversion instead of using type punning through
reinterpret_cast, and apply the same fix to any other locations (such as Line
122) where similar reinterpret_cast conversions occur between these types.
Comment on lines
+25
to
+41
| auto reproject_point(const Point3d& point_camera, const util::CameraFeature& camera) | ||
| -> std::optional<Point2d> { | ||
|
|
||
| auto object_points = std::vector<cv::Point3f> { | ||
| cv::Point3f { | ||
| static_cast<float>(point_camera.x), | ||
| static_cast<float>(point_camera.y), | ||
| static_cast<float>(point_camera.z), | ||
| }, | ||
| }; | ||
| auto projected = std::vector<cv::Point2f> { }; | ||
|
|
||
| cv::projectPoints(object_points, cv::Vec3d { 0.0, 0.0, 0.0 }, cv::Vec3d { 0.0, 0.0, 0.0 }, | ||
| camera.intrinsic(), camera.distortion(), projected); | ||
|
|
||
| if (projected.empty()) return std::nullopt; | ||
| return Point2d { projected[0] }; |
Contributor
There was a problem hiding this comment.
投影前先拒绝非正深度点。
point_camera.z <= 0 表示目标不在相机前方；当前只检查 projected.empty()，会把相机后方/近平面的点继续交给可视化链路，生成误导性的 2D 点。
建议修复
auto reproject_point(const Point3d& point_camera, const util::CameraFeature& camera)
-> std::optional<Point2d> {
+ if (!std::isfinite(point_camera.x) || !std::isfinite(point_camera.y)
+ || !std::isfinite(point_camera.z) || point_camera.z <= 0.0) {
+ return std::nullopt;
+ }
+
auto object_points = std::vector<cv::Point3f> {
cv::Point3f {
static_cast<float>(point_camera.x),
static_cast<float>(point_camera.y),
static_cast<float>(point_camera.z),
@@
if (projected.empty()) return std::nullopt;
- return Point2d { projected[0] };
+ if (!std::isfinite(projected.front().x) || !std::isfinite(projected.front().y)) {
+ return std::nullopt;
+ }
+ return Point2d { projected.front() };
}如果本文件尚未包含 <cmath>，需要同步补上。
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/utility/math/reprojection.cpp` around lines 25 - 41, The reproject_point
function does not validate that the input point has positive depth before
attempting projection, which allows points behind or at the camera
(point_camera.z <= 0) to be processed and generate misleading 2D projections.
Add an early validation check at the beginning of the reproject_point function
that returns std::nullopt if point_camera.z is less than or equal to zero.
Additionally, ensure the file includes the <cmath> header if it is not already
present.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
变更概览
本PR为RMC自动瞄准系统引入了前哨站扩展卡尔曼滤波（EKF）实现和相关的瞄准点目标可视化功能。
核心新增功能
前哨站EKF模型（OutpostModel）
OutpostModel类，实现5维状态的卡尔曼滤波预测和校正逻辑瞄准点可视化
aim_point字段，用于存储瞄准目标点的三维坐标坐标系变换与投影
PoseEstimator中新增make_point2d方法，实现从里程计坐标系到相机坐标系的转换和二维投影reproject_point，基于相机内参和畸变参数进行投影计算前哨站几何求解重构
OutpostSolution类，将前哨站中心、位置和姿态的计算从装甲灯条解析中分离出来测试与调试工具
test_outpost_ekf.cpp测试程序，用于验证EKF实现代码质量改进
AGENTS.md明确头文件引入规范（最小引入原则）影响范围
OutpostModel、OutpostSolution、reproject_point、PoseEstimator::make_point2dFireControl::Result结构体新增aim_point字段