使用getRotationMatrix2D创建变换矩阵通过SIMD指令加速warpAffine带参数WARP_INVERSE_MAP效果例程C++

下面是一个使用SIMD指令加速warpAffine函数的例程，其中使用了getRotationMatrix2D函数创建变换矩阵，并使用WARP_INVERSE_MAP参数来反转变换结果：

#include <opencv2/opencv.hpp>
#include <immintrin.h>

using namespace cv;

void warpAffine_SIMD(const Mat& src, Mat& dst, const Mat& M) {
    int rows = src.rows;
    int cols = src.cols;

    int num_channels = src.channels();
    int num_blocks = (cols / 8) * (rows / 2);

    __m256i offsets = _mm256_set_epi32(0, 1, 2, 3, 4, 5, 6, 7);

    __m256i row_indices = _mm256_set_epi32(0, 0, 1, 1, 2, 2, 3, 3);

    __m256i col_indices = _mm256_set_epi32(7, 6, 5, 4, 3, 2, 1, 0);

    __m256i zero = _mm256_setzero_si256();

    __m256i ones = _mm256_set1_epi32(1);

    int dst_cols = dst.cols;
    int dst_rows = dst.rows;

    for (int i = 0; i < dst_rows; ++i) {
        for (int j = 0; j < dst_cols; j += 8) {
            __m256i x_indices = _mm256_set_epi32(j + 7, j + 6, j + 5, j + 4, j + 3, j + 2, j + 1, j);
            __m256i y_indices = _mm256_set1_epi32(i);

            __m256i x_coords = _mm256_add_epi32(_mm256_mullo_epi32(x_indices, ones), col_indices);
            __m256i y_coords = _mm256_add_epi32(_mm256_mullo_epi32(y_indices, ones), row_indices);

            __m256i x_coords_f = _mm256_cvtepi32_ps(x_coords);
            __m256i y_coords_f = _mm256_cvtepi32_ps(y_coords);

            __m256i xy_coords[2];
            xy_coords[0] = x_coords;
            xy_coords[1] = y_coords;

            __m256i xy_coords_f[2];
            xy_coords_f[0] = _mm256_cvtps_epi32(x_coords_f);
            xy_coords_f[1] = _mm256_cvtps_epi32(y_coords_f);

            __m256i mask[2];
            mask[0] = _mm256_cmpgt_epi32(x_coords_f, _mm256_set1_ps(cols - 1));
            mask[1] = _mm256_cmpgt_epi32(y_coords_f, _mm256_set1_ps(rows - 1));

            __m256i zero_mask[2];
            zero_mask[0] = _mm256_and_si256(mask[0], zero);
            zero_mask[1] = _mm256_and_si256(mask[1], zero);

            __m256i max_indices[2];
            max_indices[0] = _mm256_set1_epi32(cols - 1);
            max_indices[1] = _mm256_set1_epi32(rows - 1);

            __m256i result_indices[2];
            result_indices[0] = _mm256_min_epi32(xy_coords_f[0], max_indices[0]);
            result_indices[1] = _mm256_min_epi32(xy_coords_f[1], max_indices[1]);

            __m256i result_indices_offset[2];
            result_indices_offset[0] = _mm256_add_epi32(result_indices[0], offsets);
            result_indices_offset[1] = _mm256_mullo_epi32(result_indices[1], _mm256_set1_epi32(src.step / num_channels));

            __m256i src_indices[2];
            src_indices[0] = _mm256_add_epi32(result_indices_offset[0], result_indices_offset[1]);
            src_indices[1] = _mm256_add_epi32(src_indices[0], _mm256_set1_epi32(j*num_channels));

            __m256i dst_indices[2];
            dst_indices[0] = _mm256_set1_epi32(j*num_channels);
            dst_indices[1] = _mm256_add_epi32(dst_indices[0], _mm256_mullo_epi32(_mm256_set1_epi32(i), _mm256_set1_epi32(dst.step)));

            __m256i src_pixels[8];
            for (int k = 0; k < 8; ++k) {
                src_pixels[k] = _mm256_set_epi8(
                    src.data[src_indices[0].m256i_i32[k] + 7],
                    src.data[src_indices[0].m256i_i32[k] + 6],
                    src.data[src_indices[0].m256i_i32[k] + 5],
                    src.data[src_indices[0].m256i_i32[k] + 4],
                    src.data[src_indices[0].m256i_i32[k] + 3],
                    src.data[src_indices[0].m256i_i32[k] + 2],
                    src.data[src_indices[0].m256i_i32[k] + 1],
                    src.data[src_indices[0].m256i_i32[k]],
                    src.data[src_indices[1].m256i_i32[k] + 7],
                    src.data[src_indices[1].m256i_i32[k] + 6],
                    src.data[src_indices[1].m256i_i32[k] + 5],
                    src.data[src_indices[1].m256i_i32[k] + 4],
                    src.data[src_indices[1].m256i_i32[k] + 3],
                    src.data[src_indices[1].m256i_i32[k] + 2],
                    src.data[src_indices[1].m256i_i32[k] + 1],
                    src.data[src_indices[1].m256i_i32[k]]
                );
            }

            __m256i src_pixels1[4], src_pixels2[4];
            for (int k = 0; k < 4; ++k) {
                src_pixels1[k] = _mm256_unpacklo_epi8(src_pixels[k * 2], zero);
                src_pixels2[k] = _mm256_unpackhi_epi8(src_pixels[k * 2], zero);
            }

            __m256i src_pixels_f1[4], src_pixels_f2[4];
            for (int k = 0; k < 4; ++k) {
                src_pixels_f1[k] = _mm256_cvtps_epi32(_mm256_mul_ps(_mm256_cvtepi32_ps(src_pixels1[k]), _mm256_set1_ps(256.f)));
                src_pixels_f2[k] = _mm256_cvtps_epi32(_mm256_mul_ps(_mm256_cvtepi32_ps(src_pixels2[k]), _mm256_set1_ps(256.f)));
            }

            __m256i src_pixels_f[2];
            for (int k = 0; k < 2; ++k) {
                src_pixels_f[k] = _mm256_or_si256(_mm256_slli_epi32(src_pixels_f1[k * 2], 16), src_pixels_f1[k * 2 + 1]);
                src_pixels_f[k] = _mm256_or_si256(_mm256_slli_epi32(src_pixels_f[k], 8), _mm256_or_si256(_mm256_slli_epi32(src_pixels_f2[k * 2], 16), src_pixels_f2[k * 2 + 1]));
            }

            __m256i dst_pixels = _mm256_i32gather_epi32(dst.data, dst_indices[0], 1);

            __m256i dst_pixels1 = _mm256_unpacklo_epi8(dst_pixels, zero);
            __m256i dst_pixels2 = _mm256_unpackhi_epi8(dst_pixels, zero);

            __m256i dst_pixels_f1 = _mm256_cvtps_epi32(_mm256_mul_ps(_mm256_cvtepi32_ps(dst_pixels1), _mm256_set1_ps(256.f)));
            __m256i dst_pixels_f2 = _mm256_cvtps_epi32(_mm256_mul_ps(_mm256_cvtepi32_ps(dst_pixels2), _mm256_set1_ps(256.f)));

            __m256i dst_pixels_f = _mm256_or_si256(_mm256_slli_epi32(dst_pixels_f1, 16), dst_pixels_f2);
            dst_pixels_f = _mm256_or_si256(_mm256_slli_epi32(dst_pixels_f, 8), _mm256_srli_epi32(dst_pixels_f, 24));

            __m256i mask_f[2];
            mask_f[0] = _mm256_and_si256(_mm256_cmpgt_epi32(result_indices[0], zero), _mm256_cmpgt_epi32(result_indices[1], zero));
            mask_f[1] = _mm256_and_si256(_mm256_cmpgt_epi32(max_indices[0], result_indices[0]), _mm256_cmpgt_epi32(max_indices[1], result_indices[1]));
            mask_f[1] = _mm256_and_si256(mask_f[1], mask_f[0]);

            __m256i transformed_pixels_f = _mm256_i32gather_epi32((int*)M.data, src_indices[0], 4);
            transformed_pixels_f = _mm256_mullo_epi32(transformed_pixels_f, _mm256_set1_epi32(1024));
            transformed_pixels_f = _mm256_add_epi32(transformed_pixels_f, _mm256_set1_epi32(1 << 19));
            transformed_pixels_f = _mm256_srli_epi32(transformed_pixels_f, 20);

            __m256i transformed_pixels[8];
            for (int k = 0; k < 8; ++k) {
                transformed_pixels[k] = _mm256_set_epi8(
                    0, 0, 0, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0
                );
            }

            for (int k = 0; k < 8; ++k) {
                int index = dst_indices[0].m256i_i32[k] / num_channels;
                if (index < dst_cols && mask_f[1].m256i_i32[k]) {
                    int x = transformed_pixels_f.m256i_i32[k * 2];
                    int y = transformed_pixels_f.m256i_i32[k * 2 + 1];
                    int src_index = y * (src.step / num_channels) + x;
                    if (src_index >= 0 && src_index < num_blocks) {
                        transformed_pixels[k] = _mm256_set_epi8(
                            src.data[src_indices[0].m256i_i32[index] + 7],
                            src.data[src_indices[0].m256i_i32[index] + 6],
                            src.data[src_indices[0].m256i_i32[index] + 5],
                            src.data[src_indices[0].m256i_i32[index] + 4],
                            src.data[src_indices[0].m256i_i32[index] + 3],
                            src.data[src_indices[0].m256i_i32[index] + 2],
                            src.data[src_indices[0].m256i_i32[index] + 1],
                            src.data[src_indices[0].m256i_i32[index]],
                            src.data[src_indices[1].m256i_i32[index] + 7],
                            src.data[src_indices[1].m256i_i32[index] + 6],
                            src.data[src_indices[1].m256i_i32[index] + 5],
                            src.data[src_indices[1].m256i_i32[index] + 4],
                            src.data[src_indices[1].m256i_i32[index] + 3],
                            src.data[src_indices[1].m256i_i32[index] + 2],
                            src.data[src_indices[1].m256i_i32[index] + 1],
                            src.data[src_indices[1].m256i_i32[index]]
                        );
                    }
                }
            }

            __m256i transformed_pixels1[4], transformed_pixels2[4];
            for (int k = 0; k < 4; ++k) {
                transformed_pixels1[k] = _mm256_unpacklo_epi8(transformed_pixels[k * 2], zero);
                transformed_pixels2[k] = _mm256_unpackhi_epi8(transformed_pixels[k * 2], zero);
            }

            __m256i transformed_pixels_f1[4], transformed_pixels_f2[4];
            for (int k = 0; k < 4; ++k) {
                transformed_pixels_f1[k] = _mm256_cvtps_epi32(_mm256_mul_ps(_mm256_cvtepi32_ps(transformed_pixels1[k]), _mm256_set1_ps(256.f)));
                transformed_pixels_f2[k] = _mm256_cvtps_epi32(_mm256_mul_ps(_mm256_cvtepi32_ps(transformed_pixels2[k]), _mm256_set1_ps(256.f)));
            }

            __m256i transformed_pixels_f[2];
            for (int k = 0; k < 2; ++k) {
                transformed_pixels_f[k] = _mm256_or_si256(_mm256_slli_epi32(transformed_pixels_f1[k * 2], 16), transformed_pixels_f1[k * 2 + 1]);
                transformed_pixels_f[k] = _mm256_or_si256(_mm256_slli_epi32(transformed_pixels_f[k], 8), _mm256_or_si256(_mm256_slli_epi32(transformed_pixels_f2[k * 2], 16), transformed_pixels_f2[k * 2 + 1]));
            }

            __m256i result_pixels_f = _mm256_mullo_epi32(transformed_pixels_f[0], _mm256_set1_epi32(256));
            result_pixels_f = _mm256_add_epi32(result_pixels_f, transformed_pixels_f[1]);
            result_pixels_f = _mm256_srli_epi32(result_pixels_f, 8);

            __m256i result_pixels = _mm256_packus_epi32(result_pixels_f, zero);
            result_pixels = _mm256_permute4x64_epi64(result_pixels, _MM_SHUFFLE(3, 1, 2, 0));
            result_pixels = _mm256_permute4x64_epi64(result_pixels, _MM_SHUFFLE(3, 1, 2, 0));

            _mm256_maskstore_epi32((int*)dst.data + dst_indices[0].m256i_i32[0], mask_f[1], result_pixels);
        }
    }
}

int main() {
    Mat src = imread("test.jpg");
    int cols = src.cols;
    int rows = src.rows;

    float angle = 45.f;
    float scale = 1.f;

    Mat M = getRotationMatrix2D(Point2f(cols / 2.f, rows / 2.f), angle, scale);
    Mat M_inv;
    invertAffineTransform(M, M_inv);

    Mat dst(rows, cols, src.type());

    warpAffine_SIMD(src, dst, M_inv, WARP_INVERSE_MAP);

    imshow("src", src);
    imshow("dst", dst);
    waitKey(0);

    return 0;
}

需要注意的是，这个例程只适用于8通道的图像，并且使用了AVX2指令集，需要在支持AVX2指令集的CPU上运行。