1/*
2 * kernel_fisheye_2_gps
3 * input_y,      input image, CL_R + CL_UNORM_INT8 //sampler
4 * input_uv, CL_RG + CL_UNORM_INT8  //sampler
5 * output_y,  CL_RGBA + CL_UNSIGNED_INT8, // 4-pixel
6 * output_uv,  CL_RGBA + CL_UNSIGNED_INT8, // 4-pixel
7 *
8 * all angles are in radian
9 */
10
11#define PI 3.1415926f
12#define PIXEL_PER_WI 4
13
14typedef struct {
15    float    center_x;
16    float    center_y;
17    float    wide_angle;
18    float    radius;
19    float    rotate_angle;
20} FisheyeInfo;
21
22__inline float2 calculate_fisheye_pos (float2 gps_pos, const FisheyeInfo *info)
23{
24    float z = cos (gps_pos.y);
25    float x = sin (gps_pos.y) * cos (gps_pos.x);
26    float y = sin (gps_pos.y) * sin (gps_pos.x);
27    float r_angle = acos (y);
28    float r = r_angle * (info->radius * 2.0f) / info->wide_angle;
29    float xz_size = sqrt(x * x + z * z);
30
31    float2 dst;
32    dst.x = -r * x / xz_size;
33    dst.y = -r * z / xz_size;
34
35    float2 ret;
36    ret.x = cos(info->rotate_angle) * dst.x - sin(info->rotate_angle) * dst.y;
37    ret.y = sin(info->rotate_angle) * dst.x + cos (info->rotate_angle) * dst.y;
38
39    return ret + (float2)(info->center_x, info->center_y);
40}
41
42__kernel void
43kernel_fisheye_table (
44    const FisheyeInfo info, const float2 fisheye_image_size,
45    __write_only image2d_t table, const float2 radian_per_pixel, const float2 table_center)
46{
47    int2 out_pos = (int2)(get_global_id (0), get_global_id (1));
48    float2 gps_pos = (convert_float2 (out_pos) - table_center) * radian_per_pixel + PI / 2.0f;
49    float2 pos = calculate_fisheye_pos (gps_pos, &info);
50    float2 min_pos = (float2)(info.center_x - info.radius, info.center_y - info.radius);
51    float2 max_pos = (float2)(info.center_x + info.radius, info.center_y + info.radius);
52    pos = clamp (pos, min_pos, max_pos);
53    pos /= fisheye_image_size;
54    write_imagef (table, out_pos, (float4)(pos, 0.0f, 0.0f));
55}
56
57__kernel void
58kernel_lsc_table (
59    __read_only image2d_t geo_table, __write_only image2d_t lsc_table,
60    __global float *lsc_array, int array_size, const FisheyeInfo info, const float2 fisheye_image_size)
61{
62    sampler_t sampler = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP_TO_EDGE | CLK_FILTER_NEAREST;
63    int2 pos = (int2) (get_global_id (0), get_global_id (1));
64
65    float2 geo_data = read_imagef (geo_table, sampler, pos).xy * fisheye_image_size;
66    float2 dist = geo_data - (float2)(info.center_x, info.center_y);
67    float r = sqrt (dist.x * dist.x + dist.y * dist.y);
68    r /= (1.0f * info.radius / array_size);
69
70    int min_idx = r;
71    int max_idx = r + 1.0f;
72    float lsc_data = max_idx > (array_size - 1) ? lsc_array[array_size - 1] :
73                     (r - min_idx) * (lsc_array[max_idx] - lsc_array[min_idx]) + lsc_array[min_idx];
74
75    write_imagef (lsc_table, pos, (float4)(lsc_data, 0.0f, 0.0f, 1.0f));
76}
77
78__kernel void
79kernel_fisheye_2_gps (
80    __read_only image2d_t input_y, __read_only image2d_t input_uv,
81    const float2 input_y_size, const FisheyeInfo info,
82    __write_only image2d_t output_y, __write_only image2d_t output_uv,
83    const float2 dst_center, const float2 radian_per_pixel)
84{
85    const int g_x = get_global_id (0);
86    const int g_y_uv = get_global_id (1);
87    const int g_y = get_global_id (1) * 2;
88    float2 src_pos[4];
89    float4 src_data;
90    float *src_ptr = (float*)(&src_data);
91    sampler_t sampler = CLK_NORMALIZED_COORDS_TRUE | CLK_ADDRESS_CLAMP_TO_EDGE | CLK_FILTER_LINEAR;
92
93    float2 gps_start_pos =
94        (convert_float2((int2)(g_x * PIXEL_PER_WI, g_y)) - dst_center) * radian_per_pixel + PI / 2.0f;
95    float2 gps_pos = gps_start_pos;
96
97#pragma unroll
98    for (int i = 0; i < PIXEL_PER_WI; ++i) {
99        float2 pos = calculate_fisheye_pos (gps_pos, &info);
100        src_pos[i] = pos / input_y_size;
101        src_ptr[i] = read_imagef (input_y, sampler, src_pos[i]).x;
102        gps_pos.x += radian_per_pixel.x;
103    }
104    write_imageui (output_y, (int2)(g_x, g_y), convert_uint4(convert_uchar4(src_data * 255.0f)));
105
106    src_data.s01 = read_imagef (input_uv, sampler, src_pos[0]).xy;
107    src_data.s23 = read_imagef (input_uv, sampler, src_pos[2]).xy;
108    write_imageui (output_uv, (int2)(g_x, g_y_uv), convert_uint4(convert_uchar4(src_data * 255.0f)));
109
110    gps_pos = gps_start_pos;
111    gps_pos.y += radian_per_pixel.y;
112#pragma unroll
113    for (int i = 0; i < PIXEL_PER_WI; ++i) {
114        float2 pos = calculate_fisheye_pos (gps_pos, &info);
115        pos /= input_y_size;
116        src_ptr[i] = read_imagef (input_y, sampler, pos).x;
117        gps_pos.x += radian_per_pixel.x;
118    }
119    write_imageui (output_y, (int2)(g_x, g_y + 1), convert_uint4(convert_uchar4(src_data * 255.0f)));
120
121}
122