rs_brief

/*
Copyright (C) 2018-2024 Geoffrey Daniels. https://gpdaniels.com/

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, version 3 of the License only.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <https://www.gnu.org/licenses/>.
*/

#pragma once
#ifndef GTL_VISION_FEATURE_DESCRIPTOR_RS_BRIEF_HPP
#define GTL_VISION_FEATURE_DESCRIPTOR_RS_BRIEF_HPP

// Summary: Implemetation of feature description from the paper "eSLAM: An Energy-Efficient Accelerator for Real-Time ORB-SLAM on FPGA Platform". Design Automation Conference (2019). [wip]

#include <vision/feature/binary_descriptor>

namespace gtl {
    void rs_brief(
        const unsigned char* __restrict const data,
        const int stride,
        const float angle_degrees,
        gtl::binary_descriptor<32>& descriptor
    );

    void rs_brief(
        const unsigned char* __restrict const data,
        const int stride,
        const float angle_degrees,
        gtl::binary_descriptor<32>& descriptor
    ) {
        constexpr static const int pattern_size = 256;
        constexpr static const auto round = [](float value) -> int {
            return (value > 0.0f) ? static_cast<int>(value + 0.5f) : static_cast<int>(value - 0.5f);
        };

        struct pattern_type final {
            int data[pattern_size][2][2];
        };

        constexpr static const pattern_type pattern = []() {
            pattern_type pattern_builder = {};
            // This is the first 8 values of the standard ORB pattern.
            constexpr const int pattern_sample[8][2][2] = {
                { { 8, -3 }, { 9, 5 } },
                { { 4, 2 }, { 7, -12 } },
                { { -11, 9 }, { -8, 2 } },
                { { 7, -12 }, { 12, -13 } },
                { { 2, -13 }, { 2, 12 } },
                { { 1, -7 }, { 1, 6 } },
                { { -2, -10 }, { -2, -4 } },
                { { -13, -13 }, { -11, -8 } }
            };
            // Values of sin and cos at (360.0f / 32.0f) = 11.25 degree intervals.
            constexpr const float angle_table[32][2] = {
                { 0.0000f, 1.0000f },
                { 0.1951f, 0.9808f },
                { 0.3827f, 0.9239f },
                { 0.5556f, 0.8315f },
                { 0.7071f, 0.7071f },
                { 0.8315f, 0.5556f },
                { 0.9239f, 0.3827f },
                { 0.9808f, 0.1951f },
                { 1.0000f, -0.0000f },
                { 0.9808f, -0.1951f },
                { 0.9239f, -0.3827f },
                { 0.8315f, -0.5556f },
                { 0.7071f, -0.7071f },
                { 0.5556f, -0.8315f },
                { 0.3827f, -0.9239f },
                { 0.1951f, -0.9808f },
                { -0.0000f, -1.0000f },
                { -0.1951f, -0.9808f },
                { -0.3827f, -0.9239f },
                { -0.5556f, -0.8315f },
                { -0.7071f, -0.7071f },
                { -0.8315f, -0.5556f },
                { -0.9239f, -0.3827f },
                { -0.9808f, -0.1951f },
                { -1.0000f, 0.0000f },
                { -0.9808f, 0.1951f },
                { -0.9239f, 0.3827f },
                { -0.8315f, 0.5556f },
                { -0.7071f, 0.7071f },
                { -0.5556f, 0.8315f },
                { -0.3827f, 0.9239f },
                { -0.1951f, 0.9808f }
            };
            // The computed full pattern is found by rotating the initial pattern.
            for (int i = 0; i < pattern_size; i += 8) {
                const float angle_sin = angle_table[i / 8][0];
                const float angle_cos = angle_table[i / 8][1];
                for (int j = 0; j < 8; ++j) {
                    // Start x and y.
                    pattern_builder.data[i + j][0][0] = round(static_cast<float>(pattern_sample[j][0][0]) * angle_cos - static_cast<float>(pattern_sample[j][0][1]) * angle_sin);
                    pattern_builder.data[i + j][0][1] = round(static_cast<float>(pattern_sample[j][0][0]) * angle_sin + static_cast<float>(pattern_sample[j][0][1]) * angle_cos);
                    // End x and y.
                    pattern_builder.data[i + j][1][0] = round(static_cast<float>(pattern_sample[j][1][0]) * angle_cos - static_cast<float>(pattern_sample[j][1][1]) * angle_sin);
                    pattern_builder.data[i + j][1][1] = round(static_cast<float>(pattern_sample[j][1][0]) * angle_sin + static_cast<float>(pattern_sample[j][1][1]) * angle_cos);
                }
            }
            return pattern_builder;
        }();

        // The descriptor is comprised of 32 sets of 8 comparisons on the input image data.
        gtl::binary_descriptor<32> descriptor_unrotated;
        for (int index = 0; index < pattern_size; index += 8) {
            const unsigned char pixels_lhs[8] = {
                data[pattern.data[index + 0][0][1] * stride + pattern.data[index + 0][0][0]],
                data[pattern.data[index + 1][0][1] * stride + pattern.data[index + 1][0][0]],
                data[pattern.data[index + 2][0][1] * stride + pattern.data[index + 2][0][0]],
                data[pattern.data[index + 3][0][1] * stride + pattern.data[index + 3][0][0]],
                data[pattern.data[index + 4][0][1] * stride + pattern.data[index + 4][0][0]],
                data[pattern.data[index + 5][0][1] * stride + pattern.data[index + 5][0][0]],
                data[pattern.data[index + 6][0][1] * stride + pattern.data[index + 6][0][0]],
                data[pattern.data[index + 7][0][1] * stride + pattern.data[index + 7][0][0]],
            };

            const unsigned char pixels_rhs[8] = {
                data[pattern.data[index + 0][1][1] * stride + pattern.data[index + 0][1][0]],
                data[pattern.data[index + 1][1][1] * stride + pattern.data[index + 1][1][0]],
                data[pattern.data[index + 2][1][1] * stride + pattern.data[index + 2][1][0]],
                data[pattern.data[index + 3][1][1] * stride + pattern.data[index + 3][1][0]],
                data[pattern.data[index + 4][1][1] * stride + pattern.data[index + 4][1][0]],
                data[pattern.data[index + 5][1][1] * stride + pattern.data[index + 5][1][0]],
                data[pattern.data[index + 6][1][1] * stride + pattern.data[index + 6][1][0]],
                data[pattern.data[index + 7][1][1] * stride + pattern.data[index + 7][1][0]],
            };

            descriptor_unrotated[index / 8] = static_cast<unsigned char>(
                ((pixels_lhs[0] < pixels_rhs[0]) << 0) |
                ((pixels_lhs[1] < pixels_rhs[1]) << 1) |
                ((pixels_lhs[2] < pixels_rhs[2]) << 2) |
                ((pixels_lhs[3] < pixels_rhs[3]) << 3) |
                ((pixels_lhs[4] < pixels_rhs[4]) << 4) |
                ((pixels_lhs[5] < pixels_rhs[5]) << 5) |
                ((pixels_lhs[6] < pixels_rhs[6]) << 6) |
                ((pixels_lhs[7] < pixels_rhs[7]) << 7)
            );
        }

        // Rotate the descriptor by rotating the binary data according to the feature's orientation.
        const int rotation = (round(angle_degrees / (360.0f / 32.0f)) % 32) + 32;
        for (int index = 0; index < 32; ++index) {
            descriptor[index] = descriptor_unrotated[(index + rotation) % 32];
        }
    }
}

#endif // GTL_VISION_FEATURE_DESCRIPTOR_RS_BRIEF_HPP