refactor: identifier and visualization pipeline#56
Merged
Conversation
Contributor
Walkthrough本PR统一Armor/Lightbar类型命名（大写→小写 d），重构绿灯与邻接光条检测接口与实现，新增 CameraFeature.from 与 PoseEstimator::configure_camera，推出 drawable/Canvas 与可视化 Armors 发布器，并将运行时调用流水线迁移为 draw_later/publish 模式。 Changes统一类型与系统管道重构
Estimated code review effort: Possibly related PRs:
✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
|
![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/utility/robot/id.hpp (1)
58-76: 🛠️ Refactor suggestion | 🟠 Major | ⚡ Quick win
to_string对非法DeviceId的处理没有 fail-fast。Line 75 当前返回
"UNREACHABLE"，会把非法参数静默传递到后续逻辑。建议改为显式错误路径（如断言/终止并附带错误信息），避免继续使用伪值。As per coding guidelines "Use fail-fast strategy by default: perform comprehensive verification during construction/initialization phase, report explicit errors for illegal parameters instead of using default values".
🤖 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/robot/id.hpp` around lines 58 - 76, The to_string(DeviceId) function currently returns the silent sentinel "UNREACHABLE" for an invalid DeviceId; change this to a fail-fast behavior: replace the final return with an explicit abort/assertion that reports the illegal DeviceId (include the enum value cast to its underlying integer) so callers cannot continue with a bogus string. Update to_string to call an assertion or std::terminate with a clear message (and add the needed include for <cassert> or <cstdlib>), keeping the failure path in the function handling of DeviceId.
🧹 Nitpick comments (11)
src/module/predictor/outpost/robot_state.hpp (2)
13-29: ⚡ Quick win
RMCS_PIMPL_DEFINITION建议放到类定义顶部。这里宏位于类体尾部，和项目约定不一致，建议与其他模块保持同一布局风格。
♻️ 建议修改
class OutpostRobotState { + RMCS_PIMPL_DEFINITION(OutpostRobotState) + public: using EKF = OutpostEKFParameters::EKF; @@ - RMCS_PIMPL_DEFINITION(OutpostRobotState) };As per coding guidelines,
RMCS_PIMPL_DEFINITION macro should be placed at the top of class/struct definitions.🤖 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/outpost/robot_state.hpp` around lines 13 - 29, Move the RMCS_PIMPL_DEFINITION macro to the top of the OutpostRobotState class definition to match project convention; specifically, place RMCS_PIMPL_DEFINITION(OutpostRobotState) immediately after the opening of class OutpostRobotState { and before the public: section so the PIMPL macro appears at the top of the class alongside other classes that follow the same layout.
3-10: ⚡ Quick win请统一为“本地头文件优先”的 include 顺序。
当前顺序是标准库在前、本地头文件在后，和仓库约定不一致。
♻️ 建议修改
-#include <chrono> -#include <span> - `#include` "module/predictor/outpost/ekf_parameter.hpp" `#include` "module/predictor/snapshot.hpp" `#include` "utility/clock.hpp" `#include` "utility/pimpl.hpp" + +#include <chrono> +#include <span>As per coding guidelines,
Header files should follow the order: local headers, standard library, third-party libraries, with blank lines separating these groups (can be merged into one block if few headers).🤖 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/outpost/robot_state.hpp` around lines 3 - 10, The include order in robot_state.hpp currently places standard headers (<chrono>, <span>) before project headers; reorder them to follow the repo convention: list local/project headers first ("module/predictor/outpost/ekf_parameter.hpp", "module/predictor/snapshot.hpp", "utility/clock.hpp", "utility/pimpl.hpp"), then a blank line, then standard library headers (<chrono>, <span>), ensuring the groups are separated by a blank line and preserving existing includes.src/kernel/tracker.hpp (1)
3-10: ⚡ Quick win头文件分组顺序与项目规范不一致。
建议将本地头文件放在最前，再放标准库、最后第三方库，以保持全仓一致性。
♻️ 建议修改
-#include <expected> -#include <span> -#include <yaml-cpp/yaml.h> - `#include` "module/tracker/decider.hpp" `#include` "utility/clock.hpp" `#include` "utility/pimpl.hpp" `#include` "utility/robot/armor.hpp" + +#include <expected> +#include <span> + +#include <yaml-cpp/yaml.h>As per coding guidelines,
Header files should follow the order: local headers, standard library, third-party libraries, with blank lines separating these groups (can be merged into one block if few headers).🤖 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/tracker.hpp` around lines 3 - 10, The include order in the file is not following project guidelines: put local project headers first (e.g., "module/tracker/decider.hpp", "utility/clock.hpp", "utility/pimpl.hpp", "utility/robot/armor.hpp"), then standard library headers (<expected>, <span>), and finally third‑party headers (<yaml-cpp/yaml.h>), with a blank line between each group; reorder the includes accordingly so the header groups and spacing match the coding standard.test/aim_point_chooser.cpp (1)
1-16: ⚡ Quick win测试文件 include 分组顺序建议按仓库规范调整。
建议先放本地头文件，再标准库，最后第三方库，减少风格不一致。
♻️ 建议修改
+#include "module/fire_control/aim_point_chooser.hpp" +#include "utility/math/angle.hpp" + `#include` <array> `#include` <cmath> `#include` <initializer_list> `#include` <memory> `#include` <optional> `#include` <random> `#include` <span> `#include` <string> `#include` <vector> `#include` <eigen3/Eigen/Geometry> `#include` <gtest/gtest.h> - -#include "module/fire_control/aim_point_chooser.hpp" -#include "utility/math/angle.hpp"As per coding guidelines,
Header files should follow the order: local headers, standard library, third-party libraries, with blank lines separating these groups (can be merged into one block if few headers).🤖 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 `@test/aim_point_chooser.cpp` around lines 1 - 16, Reorder the include block in test/aim_point_chooser.cpp to follow the project guideline: place local headers first (e.g., "module/fire_control/aim_point_chooser.hpp" and "utility/math/angle.hpp"), then standard library headers (array, cmath, initializer_list, memory, optional, random, span, string, vector), and finally third‑party headers (Eigen and gtest). Ensure blank lines separate these groups and keep the same header names so functions/classes referenced by the test (like symbols from aim_point_chooser.hpp and angle.hpp) remain resolvable.src/module/fire_control/aim_point_chooser.cpp (1)
1-8: ⚡ Quick win请按仓库约定重排 include 分组顺序。
当前把本地头文件拆成了两段（标准库前后各一段），建议合并为“本地头文件 -> 标准库 -> 第三方库”的顺序，减少后续维护时的风格漂移。
♻️ 建议修改
`#include` "aim_point_chooser.hpp" +#include "utility/math/conversion.hpp" +#include "utility/serializable.hpp" `#include` <cmath> `#include` <tuple> `#include` <vector> - -#include "utility/math/conversion.hpp" -#include "utility/serializable.hpp"As per coding guidelines,
Header files should follow the order: local headers, standard library, third-party libraries, with blank lines separating these groups (can be merged into one block if few headers).🤖 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/fire_control/aim_point_chooser.cpp` around lines 1 - 8, 把 include 分组并按仓库约定重排：将所有本地/项目头（例如 "aim_point_chooser.hpp"、"utility/math/conversion.hpp"、"utility/serializable.hpp"）合并到一个连续块（首组），然后空一行，接着放标准库头（例如 <cmath>, <tuple>, <vector>）作为第二组；确保没有将本地头分散到标准库前后，保持三组（本地 -> 标准 -> 第三方）或合并为两组时本地在前。src/module/tracker/armor_filter.hpp (1)
3-4: ⚡ Quick win为公开接口补齐直接依赖头文件。
当前头文件在公开签名中使用
std::span/std::vector，但未直接包含<span>与<vector>；这会使该头文件依赖传递包含，后续上游头文件调整时容易触发编译回归。♻️ 建议修改
`#pragma` once `#include` "utility/pimpl.hpp" `#include` "utility/robot/armor.hpp" +#include <span> +#include <vector> namespace rmcs::tracker {As per coding guidelines
**/*.{h,hpp,cpp,cc,cxx}: Header files should follow the order: local headers, standard library, third-party libraries, with blank lines separating these groups.Also applies to: 14-14
🤖 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/tracker/armor_filter.hpp` around lines 3 - 4, 头文件中公开签名使用了 std::span 与 std::vector，但 armor_filter.hpp 只包含本地头 "utility/pimpl.hpp" 和 "utility/robot/armor.hpp"；请在该头文件直接添加缺失的标准库包含（#include <span> 和 `#include` <vector>），并按项目约定把本地头、标准库、第三方库分别分组且用空行分隔（即保留 "utility/pimpl.hpp" 和 "utility/robot/armor.hpp" 为本地组，并将 <span> 和 <vector> 放入标准库组），以消除对传递包含的依赖并避免将来编译回归。src/module/predictor/snapshot.hpp (1)
3-11: ⚡ Quick win调整头文件分组顺序以符合仓库约定。
Line 3-11 当前是“第三方/标准库在前，本地头在后”，与仓库规定不一致；建议改为“本地头 → 标准库 → 第三方库”，并保留分组空行。
建议修改
-#include <eigen3/Eigen/Core> -#include <memory> -#include <vector> - `#include` "utility/clock.hpp" `#include` "utility/math/kalman_filter/ekf.hpp" `#include` "utility/robot/armor.hpp" `#include` "utility/robot/id.hpp" + +#include <memory> +#include <vector> + +#include <eigen3/Eigen/Core>As per coding guidelines
**/*.{h,hpp,cpp,cc,cxx}: Header files should follow the order: local headers, standard library, third-party libraries, with blank lines separating these groups.🤖 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/snapshot.hpp` around lines 3 - 11, Reorder the includes in snapshot.hpp so local headers come first, then standard library headers, then third-party headers: group and place "utility/clock.hpp", "utility/math/kalman_filter/ekf.hpp", "utility/robot/armor.hpp", "utility/robot/id.hpp" before <memory> and <vector>, and put <eigen3/Eigen/Core> last; keep a single blank line between each group to match the repository header ordering convention.src/kernel/identifier.hpp (1)
20-22: ⚡ Quick win补齐
<vector>直接依赖，避免头文件对传递包含的耦合
Result::areas在 Line 22 直接使用了std::vector，建议在头文件显式#include <vector>，避免未来上游头文件变动导致编译脆弱性。建议修改
`#include` <expected> `#include` <optional> +#include <vector> `#include` <opencv2/core/types.hpp> `#include` <yaml-cpp/node/node.h>As per coding guidelines "Header files should follow the order: local headers, standard library, third-party libraries, with blank lines separating these groups (can be merged into one block if few headers)".
🤖 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/identifier.hpp` around lines 20 - 22, The header is using std::vector for Result::areas but doesn't directly include <vector>, making compilation fragile; add an explicit `#include` <vector> at the top of this header and ensure includes follow the project's ordering (local headers, blank line, standard library like <vector>, then third-party headers) so Result::areas resolves without relying on transitive includes.src/utility/image/drawable.hpp (1)
1-4: ⚡ Quick win为头文件补齐标准库依赖，避免隐式传递包含
当前声明直接依赖了
std::string / std::uint8_t / std::declval / std::move，建议显式包含对应标准头，减少编译脆弱性。建议修改
`#pragma` once `#include` "utility/image/image.hpp" `#include` "utility/robot/armor.hpp" + +#include <concepts> +#include <cstdint> +#include <string> +#include <utility>As per coding guidelines "Header files should follow the order: local headers, standard library, third-party libraries, with blank lines separating these groups (can be merged into one block if few headers)".
Also applies to: 7-29
🤖 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/image/drawable.hpp` around lines 1 - 4, The header currently relies on standard types/functions (std::string, std::uint8_t, std::move, std::declval) without explicitly including their headers; update drawable.hpp to explicitly include the needed standard headers (<string>, <cstdint>, <utility>) and reorder headers to follow the guideline (local headers first, then a blank line, then standard library headers), ensuring includes for "utility/image/image.hpp" and "utility/robot/armor.hpp" remain present and the new standard headers are added immediately after with a separating blank line.src/kernel/pose_estimator.hpp (1)
35-35: 💤 Low value考虑将
addition()声明为const成员函数该方法仅返回内部数据的 const 引用，不修改对象状态，声明为
const更符合语义且允许在 const 上下文中调用。♻️ 建议修改
- auto addition() -> const Addition&; + auto addition() const -> const Addition&;🤖 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.hpp` at line 35, 将成员函数 addition() 从非 const 声明改为 const 成员函数：在声明 auto addition() -> const Addition&; 后加上 const（即 auto addition() const -> const Addition&;），并同步更新对应的定义/实现（pose_estimator::addition）以在签名中包含尾随 const，以便方法可在 const 对象上调用且语义正确。src/kernel/visualization.hpp (1)
3-11: ⚡ Quick win头文件包含顺序与编码规范不符
根据编码规范，头文件应按以下顺序排列：本地头文件、标准库、第三方库，并用空行分隔。当前顺序为标准库 → 第三方 → 本地，建议调整。
♻️ 建议修改
`#pragma` once +#include "utility/image/drawable.hpp" +#include "utility/image/image.hpp" +#include "utility/robot/armor.hpp" + `#include` <expected> `#include` <span> `#include` <string> `#include` <yaml-cpp/yaml.h> - -#include "utility/image/drawable.hpp" -#include "utility/image/image.hpp" -#include "utility/robot/armor.hpp"As per coding guidelines: "Header files should follow the order: local headers, standard library, third-party libraries, with blank lines separating these groups"
🤖 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/visualization.hpp` around lines 3 - 11, Reorder the `#include` block in visualization.hpp to follow the project's header ordering: first local headers ("utility/image/drawable.hpp", "utility/image/image.hpp", "utility/robot/armor.hpp"), then standard library headers (<expected>, <span>, <string>), then third-party headers (<yaml-cpp/yaml.h>), and insert a blank line between each group; update the include sequence in the file accordingly so the file matches the coding guideline.
🤖 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/identifier.cpp`:
- Around line 69-78: 当前逻辑只用 result.green_light->y 作为阈值并且仅过滤
ArmorGenre::OUTPOST/BASE，导致低于绿灯矩形下边缘的误检和其它类型装甲未被过滤；请在检测到 result.green_light
时把阈值改为 result.green_light->y + result.green_light->height，并修改用于
std::erase_if(detected, pred) 的 pred（或替换该谓词）为针对所有 Armor 项比较 armor.bl.y <
(green_light->y + green_light->height) 的条件，移除对 ArmorGenre::OUTPOST/BASE
的专门判断以对所有装甲生效。
In `@src/module/identifier/armor_detection.hpp`:
- Line 19: The current sync_detect(const Image&) noexcept ->
std::vector<Armor2d> signature swallows all error cases (empty image, ROI
failure from generate_openvino_request(), explain_infer_result() returning
nullopt) into an indistinguishable empty vector; change the API to preserve an
explicit failure channel (e.g. return std::expected<std::vector<Armor2d>,
std::string> or similar) so callers can tell “no armor” from “detection failed”,
update sync_detect's implementation in armor_detection.cpp to propagate and
return errors from generate_openvino_request() and explain_infer_result() rather
than returning a default empty vector, and adjust callers to handle the error
variant accordingly.
In `@src/utility/rclcpp/visual/armor.cpp`:
- Line 59: 当前 ARROW 的 lifetime 在 marker.lifetime 处被设为 1s，导致与 CUBE（之前在文件中以 0.1s
设置）不一致并在短暂停更时出现残留箭头；将 ARROW 的 lifetime 与 CUBE 统一即可。修复方法：在更新 marker.lifetime（标识
ARROW 的赋值处）改为与 CUBE 相同的值（0.1s），或更好地：提取一个共享常量（例如 kMarkerLifetime 或
marker_lifetime）并在设置 ARROW 和 CUBE 时都使用该常量，从而确保两者保持一致且便于将来调整。
---
Outside diff comments:
In `@src/utility/robot/id.hpp`:
- Around line 58-76: The to_string(DeviceId) function currently returns the
silent sentinel "UNREACHABLE" for an invalid DeviceId; change this to a
fail-fast behavior: replace the final return with an explicit abort/assertion
that reports the illegal DeviceId (include the enum value cast to its underlying
integer) so callers cannot continue with a bogus string. Update to_string to
call an assertion or std::terminate with a clear message (and add the needed
include for <cassert> or <cstdlib>), keeping the failure path in the function
handling of DeviceId.
---
Nitpick comments:
In `@src/kernel/identifier.hpp`:
- Around line 20-22: The header is using std::vector for Result::areas but
doesn't directly include <vector>, making compilation fragile; add an explicit
`#include` <vector> at the top of this header and ensure includes follow the
project's ordering (local headers, blank line, standard library like <vector>,
then third-party headers) so Result::areas resolves without relying on
transitive includes.
In `@src/kernel/pose_estimator.hpp`:
- Line 35: 将成员函数 addition() 从非 const 声明改为 const 成员函数：在声明 auto addition() ->
const Addition&; 后加上 const（即 auto addition() const -> const
Addition&;），并同步更新对应的定义/实现（pose_estimator::addition）以在签名中包含尾随 const，以便方法可在 const
对象上调用且语义正确。
In `@src/kernel/tracker.hpp`:
- Around line 3-10: The include order in the file is not following project
guidelines: put local project headers first (e.g., "module/tracker/decider.hpp",
"utility/clock.hpp", "utility/pimpl.hpp", "utility/robot/armor.hpp"), then
standard library headers (<expected>, <span>), and finally third‑party headers
(<yaml-cpp/yaml.h>), with a blank line between each group; reorder the includes
accordingly so the header groups and spacing match the coding standard.
In `@src/kernel/visualization.hpp`:
- Around line 3-11: Reorder the `#include` block in visualization.hpp to follow
the project's header ordering: first local headers
("utility/image/drawable.hpp", "utility/image/image.hpp",
"utility/robot/armor.hpp"), then standard library headers (<expected>, <span>,
<string>), then third-party headers (<yaml-cpp/yaml.h>), and insert a blank line
between each group; update the include sequence in the file accordingly so the
file matches the coding guideline.
In `@src/module/fire_control/aim_point_chooser.cpp`:
- Around line 1-8: 把 include 分组并按仓库约定重排：将所有本地/项目头（例如
"aim_point_chooser.hpp"、"utility/math/conversion.hpp"、"utility/serializable.hpp"）合并到一个连续块（首组），然后空一行，接着放标准库头（例如
<cmath>, <tuple>, <vector>）作为第二组；确保没有将本地头分散到标准库前后，保持三组（本地 -> 标准 ->
第三方）或合并为两组时本地在前。
In `@src/module/predictor/outpost/robot_state.hpp`:
- Around line 13-29: Move the RMCS_PIMPL_DEFINITION macro to the top of the
OutpostRobotState class definition to match project convention; specifically,
place RMCS_PIMPL_DEFINITION(OutpostRobotState) immediately after the opening of
class OutpostRobotState { and before the public: section so the PIMPL macro
appears at the top of the class alongside other classes that follow the same
layout.
- Around line 3-10: The include order in robot_state.hpp currently places
standard headers (<chrono>, <span>) before project headers; reorder them to
follow the repo convention: list local/project headers first
("module/predictor/outpost/ekf_parameter.hpp", "module/predictor/snapshot.hpp",
"utility/clock.hpp", "utility/pimpl.hpp"), then a blank line, then standard
library headers (<chrono>, <span>), ensuring the groups are separated by a blank
line and preserving existing includes.
In `@src/module/predictor/snapshot.hpp`:
- Around line 3-11: Reorder the includes in snapshot.hpp so local headers come
first, then standard library headers, then third-party headers: group and place
"utility/clock.hpp", "utility/math/kalman_filter/ekf.hpp",
"utility/robot/armor.hpp", "utility/robot/id.hpp" before <memory> and <vector>,
and put <eigen3/Eigen/Core> last; keep a single blank line between each group to
match the repository header ordering convention.
In `@src/module/tracker/armor_filter.hpp`:
- Around line 3-4: 头文件中公开签名使用了 std::span 与 std::vector，但 armor_filter.hpp 只包含本地头
"utility/pimpl.hpp" 和 "utility/robot/armor.hpp"；请在该头文件直接添加缺失的标准库包含（#include
<span> 和 `#include` <vector>），并按项目约定把本地头、标准库、第三方库分别分组且用空行分隔（即保留
"utility/pimpl.hpp" 和 "utility/robot/armor.hpp" 为本地组，并将 <span> 和 <vector>
放入标准库组），以消除对传递包含的依赖并避免将来编译回归。
In `@src/utility/image/drawable.hpp`:
- Around line 1-4: The header currently relies on standard types/functions
(std::string, std::uint8_t, std::move, std::declval) without explicitly
including their headers; update drawable.hpp to explicitly include the needed
standard headers (<string>, <cstdint>, <utility>) and reorder headers to follow
the guideline (local headers first, then a blank line, then standard library
headers), ensuring includes for "utility/image/image.hpp" and
"utility/robot/armor.hpp" remain present and the new standard headers are added
immediately after with a separating blank line.
In `@test/aim_point_chooser.cpp`:
- Around line 1-16: Reorder the include block in test/aim_point_chooser.cpp to
follow the project guideline: place local headers first (e.g.,
"module/fire_control/aim_point_chooser.hpp" and "utility/math/angle.hpp"), then
standard library headers (array, cmath, initializer_list, memory, optional,
random, span, string, vector), and finally third‑party headers (Eigen and
gtest). Ensure blank lines separate these groups and keep the same header names
so functions/classes referenced by the test (like symbols from
aim_point_chooser.hpp and angle.hpp) remain resolvable.
🪄 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: e867de64-fad2-450e-90b3-865e035d4d28
📒 Files selected for processing (64)
config/config.yamlsrc/kernel/capturer.hppsrc/kernel/identifier.cppsrc/kernel/identifier.hppsrc/kernel/pose_estimator.cppsrc/kernel/pose_estimator.hppsrc/kernel/tracker.cppsrc/kernel/tracker.hppsrc/kernel/visualization.cppsrc/kernel/visualization.hppsrc/module/debug/visualization/armor_visualizer.cppsrc/module/debug/visualization/armor_visualizer.hppsrc/module/fire_control/aim_point_chooser.cppsrc/module/fire_control/aim_point_chooser.hppsrc/module/identifier/adjacency_lightbar.cppsrc/module/identifier/adjacency_lightbar.hppsrc/module/identifier/armor_detection.cppsrc/module/identifier/armor_detection.hppsrc/module/identifier/green_light.cppsrc/module/identifier/green_light.hppsrc/module/identifier/green_light_detection.cppsrc/module/identifier/green_light_detection.hppsrc/module/identifier/green_light_locator.cppsrc/module/predictor/backend/robot_state_backend.cppsrc/module/predictor/backend/robot_state_backend.hppsrc/module/predictor/backend/snapshot_backend.hppsrc/module/predictor/outpost/ekf_parameter.hppsrc/module/predictor/outpost/robot_state.cppsrc/module/predictor/outpost/robot_state.hppsrc/module/predictor/outpost/snapshot.cppsrc/module/predictor/regular/ekf_parameter.hppsrc/module/predictor/regular/robot_state.cppsrc/module/predictor/regular/robot_state.hppsrc/module/predictor/regular/snapshot.cppsrc/module/predictor/robot_state.cppsrc/module/predictor/robot_state.hppsrc/module/predictor/snapshot.cppsrc/module/predictor/snapshot.hppsrc/module/tracker/armor_filter.cppsrc/module/tracker/armor_filter.hppsrc/module/tracker/decider.cppsrc/module/tracker/decider.hppsrc/runtime.cppsrc/utility/image/armor.cppsrc/utility/image/armor.hppsrc/utility/image/drawable.cppsrc/utility/image/drawable.hppsrc/utility/image/green_light.cppsrc/utility/image/green_light.hppsrc/utility/math/camera.cppsrc/utility/math/camera.hppsrc/utility/math/outpost.cppsrc/utility/math/outpost.hppsrc/utility/math/solve_pnp/outpost_distance_optimizer.cppsrc/utility/math/solve_pnp/outpost_distance_optimizer.hppsrc/utility/math/solve_pnp/yaw_optimizer.cppsrc/utility/rclcpp/visual/armor.cppsrc/utility/rclcpp/visual/armor.hppsrc/utility/robot/armor.hppsrc/utility/robot/id.hpptest/aim_point_chooser.cpptest/model_infer.cpptest/solve_pnp.cpptool/cxx/see_outpost.cpp
💤 Files with no reviewable changes (10)
- src/utility/image/green_light.hpp
- src/module/debug/visualization/armor_visualizer.cpp
- src/utility/image/armor.hpp
- src/module/identifier/green_light_detection.hpp
- src/utility/image/green_light.cpp
- src/kernel/capturer.hpp
- src/module/identifier/green_light_locator.cpp
- src/module/debug/visualization/armor_visualizer.hpp
- src/module/identifier/green_light_detection.cpp
- src/utility/image/armor.cpp
Contributor
There was a problem hiding this comment.
Actionable comments posted: 1
🧹 Nitpick comments (3)
src/kernel/pose_estimator.cpp (1)
149-180: 💤 Low value建议清理注释掉的代码
这段被注释的
yaw_optimizer相关代码已经不再使用，且有 TODO 标记表明需要清理。建议在本次 PR 或后续 PR 中移除，以保持代码整洁。🤖 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 149 - 180, Remove the dead/commented yaw-optimizer block to clean up the codebase: delete the commented lines referencing config.yaw_optimizer, the block that sets input.armor_shape / input.xyz_in_world / input.center_yaw / input.genre and the std::ranges::copy into input.detected_corners, plus the assignment armor_3d.orientation = yaw_optimizer.solve().orientation; keep the surrounding PNP logic (pnp_solution, armor_3d population) intact so only the unused yaw_optimizer-related comments and TODO are removed.src/utility/math/solve_pnp/pnp_solution.cpp (1)
4-18: 💤 Low value头文件顺序不符合规范
根据编码规范，头文件顺序应为：本地头文件、标准库、第三方库，各组之间用空行分隔。当前顺序将标准库头文件放在了第三方库之后。
建议的头文件顺序调整
`#include` "pnp_solution.hpp" `#include` "utility/math/angle.hpp" `#include` "utility/math/conversion.hpp" `#include` "utility/math/reprojection.hpp" +#include <array> +#include <cmath> +#include <limits> +#include <optional> +#include <ranges> +#include <stdexcept> +#include <vector> + `#include` <eigen3/Eigen/Geometry> `#include` <opencv2/calib3d.hpp> `#include` <opencv2/core/eigen.hpp> - -#include <array> -#include <cmath> -#include <limits> -#include <optional> -#include <ranges> -#include <stdexcept> -#include <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/utility/math/solve_pnp/pnp_solution.cpp` around lines 4 - 18, Reorder the include blocks in pnp_solution.cpp so local project headers ("utility/math/angle.hpp", "utility/math/conversion.hpp", "utility/math/reprojection.hpp") come first, followed by standard library headers (<array>, <cmath>, <limits>, <optional>, <ranges>, <stdexcept>, <vector>), and finally third‑party headers (<eigen3/Eigen/Geometry>, <opencv2/calib3d.hpp>, <opencv2/core/eigen.hpp>), with a blank line between each group to comply with the header ordering convention.Source: Coding guidelines
src/utility/math/solve_pnp/yaw_optimizer.cpp (1)
103-106: 💤 Low value
ReprojectionOptimizer::solve()是空实现该方法目前始终返回
false，如果有调用方依赖此功能，将无法正常工作。请确认这是有意为之的占位实现，还是遗漏了实现逻辑。需要我协助实现此优化器的逻辑吗？
🤖 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/solve_pnp/yaw_optimizer.cpp` around lines 103 - 106, ReprojectionOptimizer::solve() is a stub that always returns false which breaks callers expecting actual optimization; replace the empty implementation in ReprojectionOptimizer::solve with the real optimizer logic (run the yaw/pose optimization steps, update internal state and return true on success/false on failure) using existing optimizer helpers in this class, or if the algorithm isn't ready, make the unimplemented state explicit by throwing a descriptive exception (e.g., std::logic_error("ReprojectionOptimizer::solve not implemented")) or logging a clear TODO and returning a failure code, so callers can detect the intentional absence; locate and modify the ReprojectionOptimizer::solve method to implement one of these fixes.
🤖 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/utility/math/solve_pnp/pnp_solution.cpp`:
- Around line 228-232: The loop uses std::views::iota(0, steps + 1) but steps is
computed as a double from std::round, causing a type mismatch and possible wrong
iteration count; fix by making the loop bounds an integer: convert or store
steps as an integer (e.g., int steps_int = static_cast<int>(std::round(...)))
and use std::views::iota(0, steps_int + 1) when iterating over the yaw search
that updates optimized_yaw/optimized_error so the iteration count is
well-defined.
---
Nitpick comments:
In `@src/kernel/pose_estimator.cpp`:
- Around line 149-180: Remove the dead/commented yaw-optimizer block to clean up
the codebase: delete the commented lines referencing config.yaw_optimizer, the
block that sets input.armor_shape / input.xyz_in_world / input.center_yaw /
input.genre and the std::ranges::copy into input.detected_corners, plus the
assignment armor_3d.orientation = yaw_optimizer.solve().orientation; keep the
surrounding PNP logic (pnp_solution, armor_3d population) intact so only the
unused yaw_optimizer-related comments and TODO are removed.
In `@src/utility/math/solve_pnp/pnp_solution.cpp`:
- Around line 4-18: Reorder the include blocks in pnp_solution.cpp so local
project headers ("utility/math/angle.hpp", "utility/math/conversion.hpp",
"utility/math/reprojection.hpp") come first, followed by standard library
headers (<array>, <cmath>, <limits>, <optional>, <ranges>, <stdexcept>,
<vector>), and finally third‑party headers (<eigen3/Eigen/Geometry>,
<opencv2/calib3d.hpp>, <opencv2/core/eigen.hpp>), with a blank line between each
group to comply with the header ordering convention.
In `@src/utility/math/solve_pnp/yaw_optimizer.cpp`:
- Around line 103-106: ReprojectionOptimizer::solve() is a stub that always
returns false which breaks callers expecting actual optimization; replace the
empty implementation in ReprojectionOptimizer::solve with the real optimizer
logic (run the yaw/pose optimization steps, update internal state and return
true on success/false on failure) using existing optimizer helpers in this
class, or if the algorithm isn't ready, make the unimplemented state explicit by
throwing a descriptive exception (e.g.,
std::logic_error("ReprojectionOptimizer::solve not implemented")) or logging a
clear TODO and returning a failure code, so callers can detect the intentional
absence; locate and modify the ReprojectionOptimizer::solve method to implement
one of these fixes.
🪄 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: 40ef6233-ff7d-4538-be7d-870f4cf8f702
📒 Files selected for processing (7)
src/kernel/pose_estimator.cppsrc/kernel/pose_estimator.hppsrc/utility/math/reprojection.hppsrc/utility/math/solve_pnp/pnp_solution.cppsrc/utility/math/solve_pnp/pnp_solution.hppsrc/utility/math/solve_pnp/yaw_optimizer.cppsrc/utility/math/solve_pnp/yaw_optimizer.hpp
💤 Files with no reviewable changes (1)
- src/kernel/pose_estimator.hpp
Contributor
There was a problem hiding this comment.
Actionable comments posted: 1
🧹 Nitpick comments (7)
src/utility/math/outpost.cpp (1)
18-20: ⚡ Quick winvertical_axis 计算的归一化是冗余的。
Line 19 已经对
armor.orientation * UnitZ的结果调用了.normalized()，因为四元数旋转向量保持长度，UnitZ 长度为1，所以旋转后的向量长度仍为1，归一化是多余的操作。♻️ 建议的优化
- const auto vertical_axis = Eigen::Vector3d { - (armor.orientation.make<Eigen::Quaterniond>() * Eigen::Vector3d::UnitZ()).normalized() - }; + const auto vertical_axis = Eigen::Vector3d { + armor.orientation.make<Eigen::Quaterniond>() * Eigen::Vector3d::UnitZ() + };🤖 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/outpost.cpp` around lines 18 - 20, The expression computing vertical_axis redundantly calls .normalized() on the rotated UnitZ vector; remove the unnecessary normalization so vertical_axis is assigned directly from (armor.orientation.make<Eigen::Quaterniond>() * Eigen::Vector3d::UnitZ()), keeping the existing use of Eigen::Vector3d and preserving the vertical_axis identifier and the armor.orientation.make<Eigen::Quaterniond>() * Eigen::Vector3d::UnitZ() operation.src/kernel/pose_estimator.cpp (4)
211-212: ⚡ Quick win移除或说明注释代码的保留意图。
Lines 211-212 的注释代码
result.armor.orientation = outpost_in_camera.orientation;如果是临时调试代码应删除；如果是为后续优化保留的备选方案，应添加明确的说明注释（如// TODO: 考虑是否需要同时优化旋转或// 当前仅优化距离，旋转保持不变）。🗑️ 建议的改动
如果确定不需要，直接删除：
if (outpost_optimizer.solve()) { addition.origin.push_back(*outpost3d); auto& result = outpost_optimizer.result; - // 只优化距离，旋转保持原来的 - // result.armor.orientation = outpost_in_camera.orientation; - outpost_in_camera = result.armor;如果需要保留作为参考，改为：
- // 只优化距离，旋转保持原来的 - // result.armor.orientation = outpost_in_camera.orientation; + // NOTE: 当前仅优化距离，不修改 orientation。 + // 如需同时优化旋转，取消下行注释： + // result.armor.orientation = outpost_in_camera.orientation;🤖 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 211 - 212, The commented-out assignment result.armor.orientation = outpost_in_camera.orientation is left ambiguous—either remove it if it was temporary, or keep it but replace the bare comment with an explicit note; update the surrounding code in the pose estimation routine (where result and outpost_in_camera are used) to either delete that line entirely or change the comment to a clear intent like "// TODO: consider optimizing rotation as well" or "// Currently only optimizing distance; orientation intentionally unchanged" so future readers know why orientation is not modified.
238-269: ⚖️ Poor tradeoff重投影循环较长，建议提取为辅助方法。
Lines 238-269 的 for 循环包含坐标转换、ReprojectionSolution 配置、投影求解和 Lightbar2d 构造等多个步骤，认知复杂度较高。建议将此循环体提取为
Impl的私有辅助方法（如project_lightbar_to_2d），参数化 bar/color，返回std::optional<Lightbar2d>，失败时返回std::nullopt。♻️ 可选的重构示例
// 在 Impl 中添加私有方法： auto project_lightbar_to_2d(const Lightbar3d& bar, const cv::Scalar& color) const -> std::optional<Lightbar2d> { const auto upper_opencv = ros2opencv_position(bar.upper.make<Eigen::Vector3d>()); const auto lower_opencv = ros2opencv_position(bar.lower.make<Eigen::Vector3d>()); auto projection = ReprojectionSolution<2> { }; projection.input.camera = camera_feature; projection.input.object_points = { cv::Point3f { static_cast<float>(upper_opencv[0]), static_cast<float>(upper_opencv[1]), static_cast<float>(upper_opencv[2]) }, cv::Point3f { static_cast<float>(lower_opencv[0]), static_cast<float>(lower_opencv[1]), static_cast<float>(lower_opencv[2]) }, }; projection.input.image_points = { cv::Point2f { }, cv::Point2f { } }; if (!projection.solve()) return std::nullopt; return Lightbar2d { .color = bar.color, .upper = Point2d { projection.result.projected_points[0] }, .lower = Point2d { projection.result.projected_points[1] }, .draw_color = color, }; } // 在原处简化为： for (const auto& [bar, color] : std::views::zip(bars, draw_color)) { if (auto lightbar_2d = project_lightbar_to_2d(bar, color)) { addition.detected_2d.push_back(*lightbar_2d); } }🤖 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 238 - 269, The for-loop performing ros2opencv_position, ReprojectionSolution setup/solve, and Lightbar2d construction is too long—extract it into a private Impl helper named project_lightbar_to_2d that takes (const Lightbar3d& bar, const cv::Scalar& color) and returns std::optional<Lightbar2d>; inside the helper call ros2opencv_position for upper/lower, populate projection.input.camera = camera_feature and projection.input.object_points, call projection.solve() and return std::nullopt on failure, otherwise construct and return the Lightbar2d; then replace the loop body with: if (auto lb = project_lightbar_to_2d(bar, color)) addition.detected_2d.push_back(*lb).
234-236: 💤 Low value颜色计算表达式不够清晰。
Lines 234-235 使用
cv::Scalar {...} * 1.0和cv::Scalar {...} * 0.5生成不同亮度的颜色。虽然功能正确，但* 1.0是冗余操作，且意图不够明确。建议用更具语义的变量名或注释说明1.0表示 "full brightness"，0.5表示 "half brightness"。♻️ 可选的改进方案
const auto color = ArmorVisualColor { lightbar.color }; + constexpr auto kFullBrightness = 1.0; + constexpr auto kHalfBrightness = 0.5; const auto draw_color = std::array { - cv::Scalar { color.b() * 255, color.g() * 255, color.r() * 255 } * 1.0, - cv::Scalar { color.b() * 255, color.g() * 255, color.r() * 255 } * 0.5, + cv::Scalar { color.b() * 255, color.g() * 255, color.r() * 255 } * kFullBrightness, + cv::Scalar { color.b() * 255, color.g() * 255, color.r() * 255 } * kHalfBrightness, };🤖 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 234 - 236, The color brightness arithmetic is unclear and uses a redundant "* 1.0"; replace the inline multiplications with clearly named variables or a short comment: compute a base cv::Scalar from color (e.g., baseColor), then derive full brightness and half brightness as fullColor (baseColor) and halfColor (baseColor * 0.5) and use those instead of cv::Scalar{...} * 1.0 / * 0.5; reference the existing color variable and the cv::Scalar constructions in the pose estimation code to locate where to introduce baseColor/fullColor/halfColor and add a one-line comment indicating that fullColor is "full brightness" and halfColor is "half brightness".
179-181: 🏗️ Heavy liftFIXME 注释指出关键缺陷，建议开启 Issue 跟踪。
注释中提到两个严重问题：
- 坐标系变换导致 Pitch 随观测角度变化
- 前哨站 Yaw 在大角度时出现二义性翻转
这些是稳定性和准确性方面的潜在 Critical 问题。建议创建 Issue 进行跟踪，并考虑在当前 PR 中至少添加日志或警告，当检测到异常 Pitch/Yaw 变化时提示开发者。
您是否需要我为此创建 Issue 模板或生成异常检测代码？
🤖 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 179 - 181, Create an Issue tracking the two FIXME items and add runtime anomaly detection and logging around the coordinate transform and yaw-disambiguation code: in the PoseEstimator::estimatePose (or the code block performing the coordinate frame transform, e.g., transformCoordinateFrame) add a check comparing current Pitch and Yaw to the previous values (or to a smooth/expected trajectory) and if deltaPitch > configurable_threshold_deg or deltaYaw > configurable_threshold_deg emit a warning log with the raw and transformed vectors and the camera/observer angle; additionally, in the outpost/front-station yaw handling (e.g., resolveYawAmbiguity or the branch that picks between flipped yaw solutions) add detection for large-angle ambiguity flips and log the candidate yaw pair and chosen solution, and optionally reject/flag the frame for further inspection—make thresholds configurable and ensure logs include enough context (frame id, timestamp, input angles) so the Issue can be debugged.src/utility/math/reprojection.cpp (1)
12-13: ⚡ Quick win补充文档说明 object_points 的坐标系假设。
cv::projectPoints使用零旋转向量和零平移向量，这意味着object_points必须已经位于相机坐标系中。建议在函数或结构体的文档注释中明确说明此前置条件，避免调用方误用。📝 建议添加的注释
在
reprojection.hpp的 Line 16 之前添加：/// `@brief` Projects 3D points (already in camera frame) to 2D image plane. /// `@note` object_points must be in camera coordinate system (rvec=0, tvec=0 is used).🤖 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 12 - 13, 说明当前调用 cv::projectPoints 时传入了 rvec=0 和 tvec=0，从而要求 object_points 已经位于相机坐标系；请在相关接口的文档注释中明确这一前置条件（例如在 reprojection.hpp 的注释区域/函数注释中），用类似 “Projects 3D points (already in camera frame) to 2D image plane. `@note` object_points must be in camera coordinate system (rvec=0, tvec=0 is used).” 的说明标注 cv::projectPoints、object_points 和零旋转/零平移的假设，以避免调用方误用。src/utility/math/reprojection.hpp (1)
16-27: 💤 Low valueproject_points 结构可简化为函数。
project_points结构只是为了在构造时调用impl并存储结果，这种模式可以直接用一个返回bool的静态函数替代，减少不必要的状态存储。♻️ 可选的简化方案
namespace details { - struct project_points { - bool success = false; - explicit project_points(std::span<const cv::Point3f> object_points, - const cv::Mat& intrinsic, const cv::Mat& distortion, - std::span<cv::Point2f> projected_points) - : success { impl(object_points, intrinsic, distortion, projected_points) } { }; - explicit operator bool() const { return success; } - - private: - static auto impl(std::span<const cv::Point3f> object_points, const cv::Mat& intrinsic, - const cv::Mat& distortion, std::span<cv::Point2f> projected_points) -> bool; - }; + auto project_points(std::span<const cv::Point3f> object_points, + const cv::Mat& intrinsic, const cv::Mat& distortion, + std::span<cv::Point2f> projected_points) -> bool; }并在 Line 51-53 处改为：
- if (!details::project_points { input.object_points, input.camera.intrinsic(), - input.camera.distortion(), result.projected_points }) + if (!details::project_points(input.object_points, input.camera.intrinsic(), + input.camera.distortion(), result.projected_points)) return false;🤖 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.hpp` around lines 16 - 27, The project_points struct is unnecessary boilerplate — replace it with a simple free/static function that directly calls impl and returns its bool result: remove the project_points struct, keep or convert impl (static auto impl(...) -> bool) into a single function signature like bool project_points(std::span<const cv::Point3f> object_points, const cv::Mat& intrinsic, const cv::Mat& distortion, std::span<cv::Point2f> projected_points) that forwards to the existing impl logic (or inline impl body), and update any call sites that constructed project_points(...) or used its bool conversion to call the new project_points(...) function and use its bool return value.
🤖 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/utility/math/reprojection.cpp`:
- Around line 1-5: Reorder the include directives so they follow the project
convention (local headers, then standard library, then third-party): keep
`#include` "reprojection.hpp" first, then move `#include` <vector> next, and finally
`#include` <opencv2/calib3d.hpp>; update the top of reprojection.cpp accordingly
(references: "reprojection.hpp", <vector>, and <opencv2/calib3d.hpp>).
---
Nitpick comments:
In `@src/kernel/pose_estimator.cpp`:
- Around line 211-212: The commented-out assignment result.armor.orientation =
outpost_in_camera.orientation is left ambiguous—either remove it if it was
temporary, or keep it but replace the bare comment with an explicit note; update
the surrounding code in the pose estimation routine (where result and
outpost_in_camera are used) to either delete that line entirely or change the
comment to a clear intent like "// TODO: consider optimizing rotation as well"
or "// Currently only optimizing distance; orientation intentionally unchanged"
so future readers know why orientation is not modified.
- Around line 238-269: The for-loop performing ros2opencv_position,
ReprojectionSolution setup/solve, and Lightbar2d construction is too
long—extract it into a private Impl helper named project_lightbar_to_2d that
takes (const Lightbar3d& bar, const cv::Scalar& color) and returns
std::optional<Lightbar2d>; inside the helper call ros2opencv_position for
upper/lower, populate projection.input.camera = camera_feature and
projection.input.object_points, call projection.solve() and return std::nullopt
on failure, otherwise construct and return the Lightbar2d; then replace the loop
body with: if (auto lb = project_lightbar_to_2d(bar, color))
addition.detected_2d.push_back(*lb).
- Around line 234-236: The color brightness arithmetic is unclear and uses a
redundant "* 1.0"; replace the inline multiplications with clearly named
variables or a short comment: compute a base cv::Scalar from color (e.g.,
baseColor), then derive full brightness and half brightness as fullColor
(baseColor) and halfColor (baseColor * 0.5) and use those instead of
cv::Scalar{...} * 1.0 / * 0.5; reference the existing color variable and the
cv::Scalar constructions in the pose estimation code to locate where to
introduce baseColor/fullColor/halfColor and add a one-line comment indicating
that fullColor is "full brightness" and halfColor is "half brightness".
- Around line 179-181: Create an Issue tracking the two FIXME items and add
runtime anomaly detection and logging around the coordinate transform and
yaw-disambiguation code: in the PoseEstimator::estimatePose (or the code block
performing the coordinate frame transform, e.g., transformCoordinateFrame) add a
check comparing current Pitch and Yaw to the previous values (or to a
smooth/expected trajectory) and if deltaPitch > configurable_threshold_deg or
deltaYaw > configurable_threshold_deg emit a warning log with the raw and
transformed vectors and the camera/observer angle; additionally, in the
outpost/front-station yaw handling (e.g., resolveYawAmbiguity or the branch that
picks between flipped yaw solutions) add detection for large-angle ambiguity
flips and log the candidate yaw pair and chosen solution, and optionally
reject/flag the frame for further inspection—make thresholds configurable and
ensure logs include enough context (frame id, timestamp, input angles) so the
Issue can be debugged.
In `@src/utility/math/outpost.cpp`:
- Around line 18-20: The expression computing vertical_axis redundantly calls
.normalized() on the rotated UnitZ vector; remove the unnecessary normalization
so vertical_axis is assigned directly from
(armor.orientation.make<Eigen::Quaterniond>() * Eigen::Vector3d::UnitZ()),
keeping the existing use of Eigen::Vector3d and preserving the vertical_axis
identifier and the armor.orientation.make<Eigen::Quaterniond>() *
Eigen::Vector3d::UnitZ() operation.
In `@src/utility/math/reprojection.cpp`:
- Around line 12-13: 说明当前调用 cv::projectPoints 时传入了 rvec=0 和 tvec=0，从而要求
object_points 已经位于相机坐标系；请在相关接口的文档注释中明确这一前置条件（例如在 reprojection.hpp
的注释区域/函数注释中），用类似 “Projects 3D points (already in camera frame) to 2D image
plane. `@note` object_points must be in camera coordinate system (rvec=0, tvec=0
is used).” 的说明标注 cv::projectPoints、object_points 和零旋转/零平移的假设，以避免调用方误用。
In `@src/utility/math/reprojection.hpp`:
- Around line 16-27: The project_points struct is unnecessary boilerplate —
replace it with a simple free/static function that directly calls impl and
returns its bool result: remove the project_points struct, keep or convert impl
(static auto impl(...) -> bool) into a single function signature like bool
project_points(std::span<const cv::Point3f> object_points, const cv::Mat&
intrinsic, const cv::Mat& distortion, std::span<cv::Point2f> projected_points)
that forwards to the existing impl logic (or inline impl body), and update any
call sites that constructed project_points(...) or used its bool conversion to
call the new project_points(...) function and use its bool return value.
🪄 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: 34061cf9-cff5-4757-9357-8c29f1909380
📒 Files selected for processing (9)
config/config.yamlsrc/kernel/pose_estimator.cppsrc/utility/math/outpost.cppsrc/utility/math/reprojection.cppsrc/utility/math/reprojection.hppsrc/utility/math/solve_pnp/pnp_solution.cppsrc/utility/math/solve_pnp/pnp_solution.hppsrc/utility/robot/constant.hpptest/CMakeLists.txt
✅ Files skipped from review due to trivial changes (2)
- test/CMakeLists.txt
- src/utility/robot/constant.hpp
🚧 Files skipped from review as they are similar to previous changes (2)
- src/utility/math/solve_pnp/pnp_solution.hpp
- src/utility/math/solve_pnp/pnp_solution.cpp
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.
No description provided.