wip: add diverged status getter for robot model and dev tracker

Author: creeper5820

src/kernel/auto_aim.cpp

@@ -173,7 +173,7 @@ struct AutoAim::Impl {
                     if (auto aim_2d = estimator.make_point2d(aimed->attack)) {
                         visual.draw_later(Canvas::Point {
                             .origin = aim_2d->make<cv::Point2i>(),
-                            .radius = 5,
+                            .radius = 3,
                             .color  = aimed->shoot ? kRed : kGreen,
                         });
                     }

src/kernel/tracker.cpp

@@ -41,7 +41,7 @@ struct TrackerV2::Impl {
 
     RobotModel::Config robot_config;
     std::unordered_map<ArmorGenre, Timestamp> robot_stamps;
-    std::unordered_map<ArmorGenre, RobotModel> robots;
+    std::unordered_map<ArmorGenre, RobotModel> robot_models;
 
     OutpostModel::Config outpost_config;
     Timestamp outpost_stamp;
@@ -128,7 +128,7 @@ struct TrackerV2::Impl {
 
             const auto dt = std::chrono::duration<double> { timestamp - stamp };
             if (dt.count() > config.timeout_seconds) {
-                robots.erase(id);
+                robot_models.erase(id);
                 logging.warn("Timeout model with {}", get_enum_name(id));
                 return true;
             }
@@ -182,17 +182,18 @@ struct TrackerV2::Impl {
             if (id == DeviceId::OUTPOST) {
                 continue;
             }
-            if (robots.contains(id) == false) {
-                robots.try_emplace(id, robot_config);
-                robots[id].configure_camera(
+            if (robot_models.contains(id) == false) {
+                robot_models.try_emplace(id, robot_config);
+                robot_models[id].update_camera(
                     std::bit_cast<std::array<double, 9>>(camera.camera_matrix),
                     camera.distort_coeff);
-                robots[id].update_transform({
+                robot_models[id].update_transform({
                     .translation = camera.translation,
                     .orientation = camera.orientation,
                 });
-                if (!robots[id].start_with(target.armor2ds)) {
-                    robots.erase(id);
+                if (!robot_models[id].init(target.armor2ds)) {
+                    robot_models.erase(id);
+                    robot_stamps.erase(id);
                     logging.warn("Init failed with {}", get_enum_name(id));
                 } else {
                     logging.info("Init OK with {}", get_enum_name(id));
@@ -202,13 +203,19 @@ struct TrackerV2::Impl {
                     timestamp - robot_stamps[id],
                 };
 
-                auto& model = robots[id];
+                auto& model = robot_models[id];
                 model.update_transform({
                     .translation = camera.translation,
                     .orientation = camera.orientation,
                 });
                 model.predict(dt.count());
                 model.correct(target.armor2ds, target.bars);
+
+                if (model.diverged()) {
+                    robot_models.erase(id);
+                    robot_stamps.erase(id);
+                    logging.warn("{} is diverged", get_enum_name(id));
+                }
             }
             robot_stamps[id] = timestamp;
         }
@@ -229,6 +236,8 @@ struct TrackerV2::Impl {
         auto result = Trackable::Unique { };
         auto better = std::numeric_limits<double>::max();
         {
+            /// @FIXME:
+            ///  1. 误识别的机器人，也会被认为是收敛的，因为只进行了一帧 init
             if (outpost && outpost->converge()) {
                 const auto state = outpost->state();
                 const auto score = calculate(DeviceId::OUTPOST, state.direction());
@@ -240,7 +249,7 @@ struct TrackerV2::Impl {
                 }
                 std::ranges::copy(outpost->full(), std::back_inserter(addition.tracked3d));
             }
-            for (const auto& [id, model] : robots) {
+            for (const auto& [id, model] : robot_models) {
                 if (model.converge()) {
                     const auto state = model.state();
                     const auto score = calculate(id, state.direction());

src/kernel/tracker.hpp

@@ -1,14 +1,13 @@
 #pragma once
 
-#include <optional>
-#include <span>
-#include <yaml-cpp/yaml.h>
-
 #include "module/tracker/trackable.hpp"
 #include "utility/clock.hpp"
 #include "utility/pimpl.hpp"
 #include "utility/robot/armor.hpp"
 
+#include <span>
+#include <yaml-cpp/yaml.h>
+
 namespace rmcs::kernel {
 
 class TrackerV2 {

src/module/tracker/model/robot.cpp

@@ -652,7 +652,7 @@ struct RobotModel::Impl {
     }
 
     auto converge() const -> bool {
-        constexpr auto kMinUpdate = std::size_t { 5 };
+        constexpr auto kMinUpdate = std::size_t { 10 };
         constexpr auto kMaxCovXY  = 1.0;
 
         if (update_count < kMinUpdate) return false;
@@ -663,21 +663,41 @@ struct RobotModel::Impl {
 
         return true;
     }
+    auto diverged() const -> bool {
+        const auto& state = context.posteriors_state;
+        const auto& cov   = context.posteriors_covariance;
+        return std::ranges::any_of(
+            std::array {
+                cov(kStateX, kStateX) > 150,
+                cov(kStateY, kStateY) > 150,
+
+                std::abs(state[kStateX]) > 15.0,
+                std::abs(state[kStateY]) > 15.0,
+                std::abs(state[kStateZ]) > 02.0,
+                std::abs(state[kStateW]) > 10.0 * std::numbers::pi,
+
+                std::abs(state[kStateVx]) > 5.,
+                std::abs(state[kStateVy]) > 5.,
+                std::abs(state[kStateVz]) > 1.,
+
+                state.hasNaN() == true,
+                state.allFinite() == false,
+            },
+            std::identity { });
+    }
 };
 
 RobotModel::RobotModel(const Config& cfg) noexcept
     : pimpl { std::make_unique<Impl>(cfg) } { }
 
 RobotModel::~RobotModel() noexcept = default;
 
-auto RobotModel::configure(const Config& cfg) noexcept -> void { pimpl->config = cfg; }
-
-auto RobotModel::configure_camera(
-    std::array<double, 9> matrix, std::array<double, 5> coeff) noexcept -> void {
+auto RobotModel::update_camera(std::array<double, 9> matrix, std::array<double, 5> coeff) noexcept
+    -> void {
     pimpl->configure_camera(matrix, coeff);
 }
 
-auto RobotModel::start_with(std::span<const Armor2d> armors) noexcept -> bool {
+auto RobotModel::init(std::span<const Armor2d> armors) noexcept -> bool {
     return pimpl->start_with(armors);
 }
 
@@ -697,5 +717,6 @@ auto RobotModel::state() const noexcept -> State { return pimpl->context.get_sta
 auto RobotModel::full() const -> std::array<Armor3d, 4> { return pimpl->full(); }
 
 auto RobotModel::converge() const -> bool { return pimpl->converge(); }
+auto RobotModel::diverged() const -> bool { return pimpl->diverged(); }
 
 auto RobotModel::addition() const -> const Addition& { return pimpl->addition; }

src/module/tracker/model/robot.hpp

@@ -86,22 +86,18 @@ class RobotModel {
 
     explicit RobotModel(const Config&) noexcept;
 
-    auto configure(const Config&) noexcept -> void;
-
-    auto configure_camera(std::array<double, 9>, std::array<double, 5>) noexcept -> void;
-
+    auto update_camera(std::array<double, 9>, std::array<double, 5>) noexcept -> void;
     auto update_transform(const Transform&) noexcept -> void;
 
-    auto start_with(std::span<const Armor2d>) noexcept -> bool;
+    auto init(std::span<const Armor2d>) noexcept -> bool;
     auto predict(double dt) noexcept -> void;
     auto correct(std::span<const Armor2d>, std::span<const Lightbar2d>) noexcept -> void;
 
-    auto state() const noexcept -> State;
-
-    auto full() const -> std::array<Armor3d, 4>;
-
     auto converge() const -> bool;
+    auto diverged() const -> bool;
 
+    auto state() const noexcept -> State;
+    auto full() const -> std::array<Armor3d, 4>;
     auto addition() const -> const Addition&;
 };