move tracker: ignore sporadic invalid results

The tracker may fail for a couple of frames occasionally.
Just report the last known position instead.
This commit is contained in:
Megamouse 2024-12-12 02:42:57 +01:00
parent f96cfe08f4
commit 55ead61ea8
2 changed files with 32 additions and 3 deletions

View File

@ -40,6 +40,26 @@ ps_move_tracker<DiagnosticsEnabled>::~ps_move_tracker()
}
}
template <bool DiagnosticsEnabled>
void ps_move_tracker<DiagnosticsEnabled>::set_valid(ps_move_info& info, u32 index, bool valid)
{
u32& fail_count = ::at32(m_fail_count, index);
if (info.valid && !valid)
{
// Ignore a couple of untracked frames. This reduces noise.
if (++fail_count >= 3)
{
info.valid = valid;
}
return;
}
info.valid = valid;
fail_count = 0; // Reset fail count
};
template <bool DiagnosticsEnabled>
void ps_move_tracker<DiagnosticsEnabled>::set_image_data(const void* buf, u64 size, u32 width, u32 height, s32 format)
{
@ -136,7 +156,7 @@ void ps_move_tracker<DiagnosticsEnabled>::init_workers()
// Find contours
ps_move_info& info = m_info[index];
ps_move_info new_info{};
ps_move_info new_info = info;
process_contours(new_info, index);
if (new_info.valid)
@ -179,6 +199,7 @@ void ps_move_tracker<DiagnosticsEnabled>::process_image()
{
ps_move_info& info = m_info[index];
info.valid = false;
m_fail_count[index] = 0;
}
}
@ -303,7 +324,6 @@ void ps_move_tracker<DiagnosticsEnabled>::process_contours(ps_move_info& info, u
const u32 height = m_height;
const bool wrapped_hue = config.min_hue > config.max_hue; // e.g. min=355, max=5 (red)
info.valid = false;
info.x_max = width;
info.y_max = height;
@ -362,7 +382,10 @@ void ps_move_tracker<DiagnosticsEnabled>::process_contours(ps_move_info& info, u
cv::findContours(binary, all_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
if (all_contours.empty())
{
set_valid(info, index, false);
return;
}
std::vector<std::vector<cv::Point>> contours;
contours.reserve(all_contours.size());
@ -406,7 +429,10 @@ void ps_move_tracker<DiagnosticsEnabled>::process_contours(ps_move_info& info, u
}
if (best_index == umax)
{
set_valid(info, index, false);
return;
}
// Calculate distance from sphere to camera
const f32 sphere_radius_pixels = radii[best_index];
@ -417,7 +443,7 @@ void ps_move_tracker<DiagnosticsEnabled>::process_contours(ps_move_info& info, u
const f32 distance_mm = (focal_length_pixels * CELL_GEM_SPHERE_RADIUS_MM) / sphere_radius_pixels;
// Set results
info.valid = true;
set_valid(info, index, true);
info.distance_mm = distance_mm;
info.radius = sphere_radius_pixels;
info.x_pos = std::clamp(static_cast<u32>(centers[best_index].x), 0u, width);

View File

@ -71,6 +71,8 @@ private:
void calculate_values();
};
void set_valid(ps_move_info& info, u32 index, bool valid);
u32 m_width = 0;
u32 m_height = 0;
s32 m_format = 0;
@ -90,6 +92,7 @@ private:
std::array<std::vector<u8>, CELL_GEM_MAX_NUM> m_image_hue_filtered{};
std::array<std::vector<u8>, CELL_GEM_MAX_NUM> m_image_binary{};
std::array<u32, CELL_GEM_MAX_NUM> m_fail_count{};
std::array<u32, 360> m_hues{};
std::array<ps_move_info, CELL_GEM_MAX_NUM> m_info{};