VisionExecutor.cpp

Go to the documentation of this file.

#include "VisionExecutor.hpp"
 
#include "Contours.hpp"
#include "Gradient.hpp"
#include "Harris.hpp"
#include "ImageFilter.hpp"
#include "Morphology.hpp"
#include "OpticalFlow.hpp"
#include "PixelOps.hpp"
 
#include <execution>
 
namespace MayaFlux::Kinesis::Vision {
 
namespace {
 
    template <typename T>
    const T& get_params(const VisionParams& p, VisionOp op)
    {
        const T* ptr = std::get_if<T>(&p);
        if (!ptr) {
            error<std::logic_error>(
                Journal::Component::Kinesis, Journal::Context::Runtime,
                std::source_location::current(),
                "VisionExecutor: parameter type mismatch for op {}",
                Reflect::enum_to_string(op));
        }
        return *ptr;
    }
 
    void render_labels(std::vector<float>& pixels, std::span<const uint32_t> labels)
    {
        pixels.resize(labels.size() * 4);
        std::for_each(std::execution::par_unseq,
            std::views::iota(size_t { 0 }, labels.size()).begin(),
            std::views::iota(size_t { 0 }, labels.size()).end(),
            [&](size_t i) {
                const uint32_t color = labels[i] * 2654435761U;
                const size_t pixel = i * 4;
                pixels[pixel] = static_cast<float>((color >> 16) & 0xFFU) / 255.0F;
                pixels[pixel + 1] = static_cast<float>((color >> 8) & 0xFFU) / 255.0F;
                pixels[pixel + 2] = static_cast<float>(color & 0xFFU) / 255.0F;
                pixels[pixel + 3] = 1.0F;
            });
    }
 
    void render_contours(
        std::vector<float>& pixels, std::span<const Contour> contours,
        uint32_t w, uint32_t h)
    {
        pixels.assign(static_cast<size_t>(w) * h * 4, 0.0F);
        for (size_t i = 3; i < pixels.size(); i += 4)
            pixels[i] = 1.0F;
 
        const glm::vec2 extent(static_cast<float>(w), static_cast<float>(h));
        for (const auto& contour : contours) {
            for (size_t i = 0; i < contour.points.size(); ++i) {
                const glm::vec2 a = contour.points[i] * extent;
                const glm::vec2 b = contour.points[(i + 1) % contour.points.size()] * extent;
                int32_t x = static_cast<int32_t>(std::round(a.x));
                int32_t y = static_cast<int32_t>(std::round(a.y));
                const auto end_x = static_cast<int32_t>(std::round(b.x));
                const auto end_y = static_cast<int32_t>(std::round(b.y));
                const int32_t dx = std::abs(end_x - x);
                const int32_t dy = -std::abs(end_y - y);
                const int32_t sx = x < end_x ? 1 : -1;
                const int32_t sy = y < end_y ? 1 : -1;
                int32_t error = dx + dy;
 
                while (true) {
                    if (x >= 0 && y >= 0 && static_cast<uint32_t>(x) < w && static_cast<uint32_t>(y) < h) {
                        const size_t pixel = (static_cast<size_t>(y) * w + static_cast<uint32_t>(x)) * 4;
                        pixels[pixel] = pixels[pixel + 1] = pixels[pixel + 2] = 1.0F;
                    }
                    if (x == end_x && y == end_y)
                        break;
                    const int32_t twice_error = 2 * error;
                    if (twice_error >= dy) {
                        error += dy;
                        x += sx;
                    }
                    if (twice_error <= dx) {
                        error += dx;
                        y += sy;
                    }
                }
            }
        }
    }
 
} // namespace
 
// =============================================================================
// Slot management
// =============================================================================
 
void VisionExecutor::ensure_slots(uint32_t w, uint32_t h)
{
    if (m_slot_w == w && m_slot_h == h)
        return;
 
    reset();
    const size_t n = static_cast<size_t>(w) * h;
 
    for (size_t i = 0; i < k_slot_count; ++i)
        m_slots[i] = std::vector<float>(n, 0.0F);
 
    m_slot_w = w;
    m_slot_h = h;
}
 
// =============================================================================
// Kernel cache
// =============================================================================
 
const std::vector<float>& VisionExecutor::gaussian_kernel(float sigma)
{
    const auto key = std::bit_cast<uint32_t>(sigma);
    auto it = m_kernel_cache.find(key);
    if (it != m_kernel_cache.end())
        return it->second;
 
    const auto radius = static_cast<int32_t>(std::ceil(3.0F * sigma));
    const int32_t size = 2 * radius + 1;
    const float inv_2s2 = 1.0F / (2.0F * sigma * sigma);
 
    std::vector<float> k(static_cast<size_t>(size));
    float sum = 0.0F;
    for (int32_t i = -radius; i <= radius; ++i) {
        const float v = std::exp(-static_cast<float>(i * i) * inv_2s2);
        k[static_cast<size_t>(i + radius)] = v;
        sum += v;
    }
 
    auto kmap = Eigen::Map<Eigen::ArrayXf>(k.data(), static_cast<Eigen::Index>(k.size()));
    kmap /= sum;
 
    return m_kernel_cache.emplace(key, std::move(k)).first->second;
}
 
// =============================================================================
// reset
// =============================================================================
 
void VisionExecutor::reset()
{
    std::get<std::vector<float>>(m_prev_gray).clear();
    m_curr_gray_cache.clear();
    m_prev_keypoints.clear();
}
 
// =============================================================================
// run
// =============================================================================
 
VisionResult VisionExecutor::run(
    const VisionSequence& sequence,
    std::span<const float> frame,
    uint32_t w, uint32_t h)
{
    ensure_slots(w, h);
    const size_t slot_min = static_cast<size_t>(m_slot_w) * m_slot_h * 4;
    if (slot_vec(k_slot_nxt).size() < slot_min)
        slot_vec(k_slot_nxt).resize(slot_min);
    if (slot_vec(k_slot_cur).capacity() < slot_min)
        slot_vec(k_slot_cur).reserve(slot_min);
 
    auto& cur_vec = slot_vec(k_slot_cur);
    cur_vec.assign(frame.begin(), frame.end());
 
    m_pass.begin(sequence, w, h);
    m_pass.current = k_slot_cur;
 
    size_t nxt = k_slot_nxt;
    auto en = static_cast<Eigen::Index>(m_pass.plane_size());
    const bool wants_tracking = tracks_keypoints(sequence);
    std::vector<Keypoint> current_keypoints;
 
    for (m_pass.index = 0; m_pass.index < sequence.steps.size(); ++m_pass.index) {
        const auto& step = m_pass.step();
 
        switch (step.op) {
        case VisionOp::Downsample2x: {
            uint32_t new_w = 0, new_h = 0;
            downsample_2x(slot_vec(m_pass.current), slot_vec(nxt), w, h, m_pass.channels, new_w, new_h);
            w = new_w;
            h = new_h;
            m_pass.set_geometry(w, h);
            en = static_cast<Eigen::Index>(m_pass.plane_size());
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::RgbaToGray: {
            rgba_to_gray(slot_vec(m_pass.current), slot_vec(nxt), w, h);
            m_pass.channels = 1;
            std::swap(m_pass.current, nxt);
 
            if (wants_tracking) {
                m_curr_gray_cache.assign(slot_vec(m_pass.current).begin(),
                    slot_vec(m_pass.current).begin() + m_pass.plane_size());
            }
 
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::RgbaToHsv: {
            rgba_to_hsv(slot_vec(m_pass.current), slot_vec(nxt), w, h);
            m_pass.channels = 3;
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::GrayToRgba: {
            gray_to_rgba(slot_vec(m_pass.current), slot_vec(nxt), w, h);
            m_pass.channels = 4;
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::Threshold: {
            const auto& p = get_params<ThresholdParams>(step.params, step.op);
            slot_map_mut(m_pass.current, en) = (slot_map(m_pass.current, en) >= p.value).cast<float>();
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::ThresholdAdaptive: {
            const auto& p = get_params<ThresholdAdaptiveParams>(step.params, step.op);
            threshold_adaptive(slot_vec(m_pass.current), slot_vec(nxt), w, h, p.block_size, p.offset);
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::ThresholdOtsu: {
            const size_t n = m_pass.plane_size();
            threshold_otsu(
                std::span<const float>(slot_vec(m_pass.current)).first(n),
                std::span<float>(slot_vec(nxt)).first(n));
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::NormalizeInplace: {
            auto m = slot_map_mut(m_pass.current, en);
            const float mn = m.minCoeff();
            const float mx = m.maxCoeff();
            if (mx > mn)
                m = (m - mn) / (mx - mn);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::NormalizeRange: {
            const auto& p = get_params<NormalizeRangeParams>(step.params, step.op);
            if (p.hi > p.lo) {
                auto m = slot_map_mut(m_pass.current, en);
                m = ((m - p.lo) / (p.hi - p.lo)).max(0.0F).min(1.0F);
            }
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::GaussianBlur: {
            const auto& p = get_params<GaussianBlurParams>(step.params, step.op);
            const auto& kern = gaussian_kernel(p.sigma);
            filter_separable(slot_vec(m_pass.current), slot_vec(k_slot_tmp), slot_vec(nxt), w, h, kern, kern);
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::FilterSeparable: {
            const auto& p = get_params<FilterSeparableParams>(step.params, step.op);
            filter_separable(slot_vec(m_pass.current), slot_vec(k_slot_tmp), slot_vec(nxt), w, h, p.kernel_x, p.kernel_y);
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::Sobel: {
            GradientResult grad;
            sobel(slot_vec(m_pass.current), slot_vec(k_slot_dx), slot_vec(k_slot_dy),
                slot_vec(k_slot_tmp), w, h);
 
            auto dx = slot_map(k_slot_dx, en);
            auto dy = slot_map(k_slot_dy, en);
            slot_map_mut(nxt, en) = (dx.square() + dy.square()).sqrt();
            const float peak = slot_map(nxt, en).maxCoeff();
 
            if (peak > 0.0F)
                slot_map_mut(nxt, en) /= peak;
 
            grad.dx.assign(slot_vec(k_slot_dx).begin(), slot_vec(k_slot_dx).begin() + en);
            grad.dy.assign(slot_vec(k_slot_dy).begin(), slot_vec(k_slot_dy).begin() + en);
            grad.magnitude.assign(slot_vec(nxt).begin(), slot_vec(nxt).begin() + en);
            grad.angle.resize(static_cast<size_t>(en));
 
            std::transform(std::execution::par_unseq,
                slot_vec(k_slot_dx).begin(), slot_vec(k_slot_dx).begin() + en,
                slot_vec(k_slot_dy).begin(), grad.angle.begin(),
                [](float gx, float gy) { return std::atan2(gy, gx); });
 
            m_pass.result.structured = std::move(grad);
            std::swap(m_pass.current, nxt);
            break;
        }
 
        case VisionOp::Scharr: {
            GradientResult grad;
            scharr(slot_vec(m_pass.current), slot_vec(k_slot_dx), slot_vec(k_slot_dy),
                slot_vec(k_slot_tmp), w, h);
 
            auto dx = slot_map(k_slot_dx, en);
            auto dy = slot_map(k_slot_dy, en);
            slot_map_mut(nxt, en) = (dx.square() + dy.square()).sqrt();
 
            const float peak = slot_map(nxt, en).maxCoeff();
            if (peak > 0.0F)
                slot_map_mut(nxt, en) /= peak;
 
            grad.dx.assign(slot_vec(k_slot_dx).begin(), slot_vec(k_slot_dx).begin() + en);
            grad.dy.assign(slot_vec(k_slot_dy).begin(), slot_vec(k_slot_dy).begin() + en);
            grad.magnitude.assign(slot_vec(nxt).begin(), slot_vec(nxt).begin() + en);
            grad.angle.resize(static_cast<size_t>(en));
 
            std::transform(std::execution::par_unseq,
                slot_vec(k_slot_dx).begin(), slot_vec(k_slot_dx).begin() + en,
                slot_vec(k_slot_dy).begin(), grad.angle.begin(),
                [](float gx, float gy) { return std::atan2(gy, gx); });
 
            m_pass.result.structured = std::move(grad);
            std::swap(m_pass.current, nxt);
            break;
        }
 
        case VisionOp::Canny: {
            const auto& p = get_params<CannyParams>(step.params, step.op);
            canny(slot_vec(m_pass.current), slot_vec(nxt), w, h, p.sigma, p.low_threshold, p.high_threshold);
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::Erode: {
            const auto& p = get_params<MorphParams>(step.params, step.op);
            erode(slot_vec(m_pass.current), slot_vec(nxt), w, h, p.radius);
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::Dilate: {
            const auto& p = get_params<MorphParams>(step.params, step.op);
            dilate(slot_vec(m_pass.current), slot_vec(nxt), w, h, p.radius);
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::Open: {
            const auto& p = get_params<MorphParams>(step.params, step.op);
            open(slot_vec(m_pass.current), slot_vec(k_slot_tmp), slot_vec(nxt), w, h, p.radius);
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::Close: {
            const auto& p = get_params<MorphParams>(step.params, step.op);
            close(slot_vec(m_pass.current), slot_vec(k_slot_tmp), slot_vec(nxt), w, h, p.radius);
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::MorphGradient: {
            const auto& p = get_params<MorphParams>(step.params, step.op);
            morph_gradient(slot_vec(m_pass.current), slot_vec(k_slot_tmp), slot_vec(nxt), w, h, p.radius);
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::ConnectedComponents: {
            const auto& p = get_params<ConnectedComponentsParams>(step.params, step.op);
            const size_t n = m_pass.plane_size();
            auto components = connected_components(
                std::span<const float>(slot_vec(m_pass.current)).subspan(0, n),
                w, h);
 
            const auto* next = m_pass.ahead();
            const bool contours_follow = next && next->op == VisionOp::FindContours;
            if (p.with_colors) {
                render_labels(slot_vec(m_pass.current), components.label_map);
                m_pass.channels = 4;
                m_pass.set_geometry(w, h);
            } else if (!contours_follow) {
                slot_vec(m_pass.current).clear();
                m_pass.result.w = 0;
                m_pass.result.h = 0;
            }
 
            if (!p.export_labels && !contours_follow)
                components.label_map.clear();
            m_pass.result.structured = std::move(components);
            break;
        }
 
        case VisionOp::FindContours: {
            const auto* prev = m_pass.behind();
            if (!prev || prev->op != VisionOp::ConnectedComponents) {
                MF_ERROR(Journal::Component::Kinesis, Journal::Context::Runtime,
                    "VisionExecutor: FindContours requires ConnectedComponents as the immediately preceding step");
                return {};
            }
 
            const auto* components = std::get_if<ComponentResult>(&m_pass.result.structured);
            if (!components) {
                MF_ERROR(Journal::Component::Kinesis, Journal::Context::Runtime,
                    "VisionExecutor: FindContours requires a connected-component result");
                return {};
            }
 
            const auto& p = get_params<FindContoursParams>(step.params, step.op);
            auto contours = find_contours(*components, w, h,
                p.min_area, p.max_contours, p.max_points_per_contour);
            if (p.as_image) {
                render_contours(slot_vec(m_pass.current), contours, w, h);
                m_pass.channels = 4;
                m_pass.set_geometry(w, h);
                m_pass.result.structured = std::monostate {};
            } else {
                m_pass.result.structured = std::move(contours);
                slot_vec(m_pass.current).clear();
                m_pass.result.w = 0;
                m_pass.result.h = 0;
            }
            break;
        }
 
        case VisionOp::HarrisResponse: {
            const auto& p = get_params<HarrisParams>(step.params, step.op);
            harris_response(
                slot_vec(m_pass.current),
                slot_vec(k_slot_dx), slot_vec(k_slot_dy), slot_vec(k_slot_tmp),
                slot_vec(k_slot_ixx), slot_vec(k_slot_iyy), slot_vec(k_slot_ixy),
                slot_vec(k_slot_sxx), slot_vec(k_slot_syy), slot_vec(k_slot_sxy),
                slot_vec(nxt),
                w, h, p.k, gaussian_kernel(p.sigma));
            std::swap(m_pass.current, nxt);
            m_pass.result.structured = std::monostate {};
            break;
        }
 
        case VisionOp::ExtractPeaks: {
            const auto& p = get_params<ExtractPeaksParams>(step.params, step.op);
            auto kpts = extract_peaks(slot_vec(m_pass.current), w, h, p.threshold, p.nms_radius);
            const auto* next = m_pass.ahead();
            const bool peaks_feed_track = next && next->op == VisionOp::TrackKeypoints;
            if (peaks_feed_track)
                current_keypoints = kpts;
            else
                m_prev_keypoints = kpts;
 
            if (wants_tracking && !peaks_feed_track)
                std::swap(std::get<std::vector<float>>(m_prev_gray), m_curr_gray_cache);
 
            m_pass.result.structured = std::move(kpts);
            if (!peaks_feed_track) {
                slot_vec(m_pass.current).clear();
                m_pass.result.w = 0;
                m_pass.result.h = 0;
            }
            break;
        }
 
        case VisionOp::TrackKeypoints: {
            const auto& p = get_params<TrackKeypointsParams>(step.params, step.op);
            const auto* prev = m_pass.behind();
            const bool peaks_feed_track = prev && prev->op == VisionOp::ExtractPeaks;
            const auto curr_gray = peaks_feed_track
                ? std::span<const float>(m_curr_gray_cache)
                : std::span<const float>(slot_vec(m_pass.current)).first(m_pass.plane_size());
            auto& prev_vec = std::get<std::vector<float>>(m_prev_gray);
            std::vector<TrackResult> tracked;
            if (!prev_vec.empty() && !m_prev_keypoints.empty()) {
                std::vector<glm::vec2> prev_pos;
                prev_pos.reserve(m_prev_keypoints.size());
                for (const auto& kp : m_prev_keypoints)
                    prev_pos.push_back(kp.position);
 
                tracked = track_keypoints(
                    prev_vec, curr_gray,
                    w, h, prev_pos,
                    p.window_radius, p.max_iterations,
                    p.eigen_threshold, p.error_threshold,
                    p.forward_backward_threshold);
            }
 
            if (peaks_feed_track) {
                std::swap(prev_vec, m_curr_gray_cache);
                m_prev_keypoints = std::move(current_keypoints);
            } else {
                std::swap(prev_vec, slot_vec(m_pass.current));
            }
            m_pass.result.structured = std::move(tracked);
            slot_vec(m_pass.current).clear();
            m_pass.result.w = 0;
            m_pass.result.h = 0;
            break;
        }
 
        case VisionOp::Snapshot: {
            if (slot_vec(m_pass.current).empty())
                break;
            const size_t n = m_pass.plane_size() * m_pass.channels;
            SnapshotEntry entry;
            entry.pixels.assign(slot_vec(m_pass.current).begin(), slot_vec(m_pass.current).begin() + n);
            entry.w = w;
            entry.h = h;
            entry.channels = m_pass.channels;
            m_pass.result.snapshots.push_back(std::move(entry));
            break;
        }
 
        case VisionOp::OpticalFlowDense:
            MF_ERROR(Journal::Component::Kinesis, Journal::Context::Runtime,
                "VisionExecutor: OpticalFlowDense has no CPU implementation");
            break;
        }
    }
 
    if (!slot_vec(m_pass.current).empty())
        slot_vec(m_pass.current).resize(m_pass.plane_size() * m_pass.channels);
 
    m_pass.result.pixel_image = std::move(m_slots[m_pass.current]);
    slot_vec(m_pass.current).reserve(static_cast<size_t>(m_slot_w) * m_slot_h * 4);
 
    VisionResult out = std::move(m_pass.result);
    m_pass.result = VisionResult {};
    return out;
}
 
} // namespace MayaFlux::Kinesis::Vision