Loongson_2k0300_SmartCar_Framework/vision/element.cpp

#include "element.hpp"
#include <algorithm>
#include <cmath>
#include <cstring>

ElementMachine g_elm;

namespace {
    static constexpr uint8 k_cross_confirm_frames = 3;
    static constexpr uint8 k_cross_exit_frames    = 5;
    static constexpr uint8 k_lock_min_frames      = 5;
    static constexpr float k_width_widen_ratio    = 1.6f;
    static constexpr float k_width_normal_ratio   = 1.3f;

    static int bottom_width(const SearchResult& sr)
    {
        int y = IMAGE_HEIGHT - 1;
        if (sr.lines.left[y] > 0 && sr.lines.right[y] > 0)
            return sr.lines.right[y] - sr.lines.left[y];
        return -1;
    }

    static uint8 line_lost_state(const SearchResult& sr)
    {
        uint8 st = 0;
        if (!sr.lines.left_valid)  st |= 1;
        if (!sr.lines.right_valid) st |= 2;
        return st;
    }

    static void calibrate_width(const SearchResult& sr)
    {
        int w = bottom_width(sr);
        if (w > 10 && w < IMAGE_WIDTH - 10)
        {
            g_elm.normal_width = (g_elm.normal_width * g_elm.calib_cnt + w) / (g_elm.calib_cnt + 1);
            if (g_elm.calib_cnt < 50) ++g_elm.calib_cnt;
        }
    }

    static bool is_cross_entry(const SearchResult& sr)
    {
        if (g_elm.calib_cnt < 20) return false;

        int w = bottom_width(sr);
        if (w < 0) return false;

        bool too_wide = (w > g_elm.normal_width * k_width_widen_ratio);
        uint8 lost = line_lost_state(sr);
        bool both_lost_upper = (lost == 3);

        if (too_wide || both_lost_upper) g_elm.confirm_cnt++;
        else g_elm.confirm_cnt = 0;

        return g_elm.confirm_cnt >= k_cross_confirm_frames;
    }

    static bool is_cross_exit(const SearchResult& sr)
    {
        int w = bottom_width(sr);
        if (w < 0) { g_elm.exit_cnt = 0; return false; }

        bool width_normal = (w < g_elm.normal_width * k_width_normal_ratio);
        bool bounds_ok = (sr.lines.left_valid && sr.lines.right_valid);

        if (width_normal && bounds_ok) g_elm.exit_cnt++;
        else g_elm.exit_cnt = 0;

        return g_elm.exit_cnt >= k_cross_exit_frames;
    }

    static void cross_patch_midline(SearchResult& sr)
    {
        int bottom_y = IMAGE_HEIGHT - 1;
        int mid_x = sr.lines.mid[bottom_y];
        if (mid_x <= 0) return;

        int lx = mid_x - static_cast<int>(g_elm.normal_width / 2);
        int rx = mid_x + static_cast<int>(g_elm.normal_width / 2);
        lx = std::max(0, lx);
        rx = std::min(IMAGE_WIDTH - 1, rx);

        for (int y = 0; y < IMAGE_HEIGHT; ++y)
        {
            if (sr.lines.left[y] == 0 && sr.lines.right[y] == 0)
            {
                sr.lines.left[y]  = static_cast<uint8>(lx);
                sr.lines.right[y] = static_cast<uint8>(rx);
                sr.lines.mid[y]   = static_cast<uint8>(mid_x);
            }
        }
        sr.lines.left_valid  = 1;
        sr.lines.right_valid = 1;
    }
}

void element_init()
{
    g_elm = ElementMachine{};
    g_elm.state      = TRACK_STRAIGHT;
    g_elm.last_state = TRACK_STRAIGHT;
    g_elm.normal_width = 50.0f;
}

bool element_is_cross()
{
    return g_elm.state == TRACK_CROSS;
}

void element_recognize(SearchResult& sr, TrackInfo& info)
{
    calibrate_width(sr);

    g_elm.last_state = g_elm.state;
    ++g_elm.in_state_frames;

    if (g_elm.lock_cnt > 0)
    {
        --g_elm.lock_cnt;
        info.scene = g_elm.state;
        return;
    }

    switch (g_elm.state)
    {
    case TRACK_STRAIGHT:
    case TRACK_GENTLE_CURVE:
        if (is_cross_entry(sr))
        {
            int bottom_y = IMAGE_HEIGHT - 1;
            g_elm.entry_deviation = (sr.lines.mid[bottom_y] > 0)
                ? (sr.lines.mid[bottom_y] - IMAGE_WIDTH / 2) / static_cast<float>(IMAGE_WIDTH / 2)
                : 0.0f;

            g_elm.state           = TRACK_CROSS;
            g_elm.lock_cnt        = k_lock_min_frames;
            g_elm.in_state_frames = 0;
            g_elm.confirm_cnt     = 0;
            g_elm.exit_cnt        = 0;
            info.scene = TRACK_CROSS;
        }
        else
        {
            info.scene = sr.lines.left_valid || sr.lines.right_valid
                         ? TRACK_STRAIGHT : TRACK_LOST_LINE;
        }
        info.line_valid = (info.scene != TRACK_LOST_LINE);
        break;

    case TRACK_CROSS:
        cross_patch_midline(sr);

        if (is_cross_exit(sr))
        {
            g_elm.state           = TRACK_STRAIGHT;
            g_elm.lock_cnt        = 0;
            g_elm.in_state_frames = 0;
            g_elm.confirm_cnt     = 0;
            g_elm.exit_cnt        = 0;
            info.scene = TRACK_STRAIGHT;
        }
        else
        {
            info.scene = TRACK_CROSS;
        }
        info.line_valid = true;
        break;

    case TRACK_LOST_LINE:
        g_elm.confirm_cnt = 0;
        if (sr.lines.left_valid && sr.lines.right_valid)
        {
            g_elm.state = TRACK_STRAIGHT;
            info.scene  = TRACK_STRAIGHT;
        }
        else
        {
            info.scene = TRACK_LOST_LINE;
        }
        info.line_valid = (info.scene != TRACK_LOST_LINE);
        break;

    default:
        info.scene      = TRACK_STRAIGHT;
        info.line_valid = true;
        break;
    }
}