mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-09-15 03:53:41 +08:00
Move camerad to system/ (#24836)
* mv camerad * add hardware symlink * fix unit tests old-commit-hash: 6123ab3d1c901ed3763e1a7cb8e1aac3f6b8fda3
This commit is contained in:
Adeeb Shihadeh
@@ -0,0 +1,36 @@
|
||||
#include "system/camerad/transforms/rgb_to_yuv.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
|
||||
Rgb2Yuv::Rgb2Yuv(cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride) {
|
||||
assert(width % 2 == 0 && height % 2 == 0);
|
||||
char args[1024];
|
||||
snprintf(args, sizeof(args),
|
||||
"-cl-fast-relaxed-math -cl-denorms-are-zero "
|
||||
#ifdef CL_DEBUG
|
||||
"-DCL_DEBUG "
|
||||
#endif
|
||||
"-DWIDTH=%d -DHEIGHT=%d -DUV_WIDTH=%d -DUV_HEIGHT=%d -DRGB_STRIDE=%d -DRGB_SIZE=%d",
|
||||
width, height, width / 2, height / 2, rgb_stride, width * height);
|
||||
|
||||
cl_program prg = cl_program_from_file(ctx, device_id, "transforms/rgb_to_yuv.cl", args);
|
||||
krnl = CL_CHECK_ERR(clCreateKernel(prg, "rgb_to_yuv", &err));
|
||||
CL_CHECK(clReleaseProgram(prg));
|
||||
|
||||
work_size[0] = (width + (width % 4 == 0 ? 0 : (4 - width % 4))) / 4;
|
||||
work_size[1] = (height + (height % 4 == 0 ? 0 : (4 - height % 4))) / 4;
|
||||
}
|
||||
|
||||
Rgb2Yuv::~Rgb2Yuv() {
|
||||
CL_CHECK(clReleaseKernel(krnl));
|
||||
}
|
||||
|
||||
void Rgb2Yuv::queue(cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl) {
|
||||
CL_CHECK(clSetKernelArg(krnl, 0, sizeof(cl_mem), &rgb_cl));
|
||||
CL_CHECK(clSetKernelArg(krnl, 1, sizeof(cl_mem), &yuv_cl));
|
||||
cl_event event;
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, krnl, 2, NULL, &work_size[0], NULL, 0, 0, &event));
|
||||
CL_CHECK(clWaitForEvents(1, &event));
|
||||
CL_CHECK(clReleaseEvent(event));
|
||||
}
|
||||
@@ -0,0 +1,127 @@
|
||||
#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16)
|
||||
#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8)
|
||||
#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8)
|
||||
#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1)
|
||||
|
||||
inline void convert_2_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1) {
|
||||
uchar2 yy = (uchar2)(
|
||||
RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0),
|
||||
RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3)
|
||||
);
|
||||
#ifdef CL_DEBUG
|
||||
if(yi >= RGB_SIZE)
|
||||
printf("Y vector2 overflow, %d > %d\n", yi, RGB_SIZE);
|
||||
#endif
|
||||
vstore2(yy, 0, out_yuv + yi);
|
||||
}
|
||||
|
||||
inline void convert_4_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1, const uchar8 rgbs3) {
|
||||
const uchar4 yy = (uchar4)(
|
||||
RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0),
|
||||
RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3),
|
||||
RGB_TO_Y(rgbs3.s0, rgbs1.s7, rgbs1.s6),
|
||||
RGB_TO_Y(rgbs3.s3, rgbs3.s2, rgbs3.s1)
|
||||
);
|
||||
#ifdef CL_DEBUG
|
||||
if(yi > RGB_SIZE - 4)
|
||||
printf("Y vector4 overflow, %d > %d\n", yi, RGB_SIZE - 4);
|
||||
#endif
|
||||
vstore4(yy, 0, out_yuv + yi);
|
||||
}
|
||||
|
||||
inline void convert_uv(__global uchar * out_yuv, int ui, int vi,
|
||||
const uchar8 rgbs1, const uchar8 rgbs2) {
|
||||
// U & V: average of 2x2 pixels square
|
||||
const short ab = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3);
|
||||
const short ag = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4);
|
||||
const short ar = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5);
|
||||
#ifdef CL_DEBUG
|
||||
if(ui >= RGB_SIZE + RGB_SIZE / 4)
|
||||
printf("U overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4);
|
||||
if(vi >= RGB_SIZE + RGB_SIZE / 2)
|
||||
printf("V overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2);
|
||||
#endif
|
||||
out_yuv[ui] = RGB_TO_U(ar, ag, ab);
|
||||
out_yuv[vi] = RGB_TO_V(ar, ag, ab);
|
||||
}
|
||||
|
||||
inline void convert_2_uvs(__global uchar * out_yuv, int ui, int vi,
|
||||
const uchar8 rgbs1, const uchar8 rgbs2, const uchar8 rgbs3, const uchar8 rgbs4) {
|
||||
// U & V: average of 2x2 pixels square
|
||||
const short ab1 = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3);
|
||||
const short ag1 = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4);
|
||||
const short ar1 = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5);
|
||||
const short ab2 = AVERAGE(rgbs1.s6, rgbs3.s1, rgbs2.s6, rgbs4.s1);
|
||||
const short ag2 = AVERAGE(rgbs1.s7, rgbs3.s2, rgbs2.s7, rgbs4.s2);
|
||||
const short ar2 = AVERAGE(rgbs3.s0, rgbs3.s3, rgbs4.s0, rgbs4.s3);
|
||||
uchar2 u2 = (uchar2)(
|
||||
RGB_TO_U(ar1, ag1, ab1),
|
||||
RGB_TO_U(ar2, ag2, ab2)
|
||||
);
|
||||
uchar2 v2 = (uchar2)(
|
||||
RGB_TO_V(ar1, ag1, ab1),
|
||||
RGB_TO_V(ar2, ag2, ab2)
|
||||
);
|
||||
#ifdef CL_DEBUG1
|
||||
if(ui > RGB_SIZE + RGB_SIZE / 4 - 2)
|
||||
printf("U 2 overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4 - 2);
|
||||
if(vi > RGB_SIZE + RGB_SIZE / 2 - 2)
|
||||
printf("V 2 overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2 - 2);
|
||||
#endif
|
||||
vstore2(u2, 0, out_yuv + ui);
|
||||
vstore2(v2, 0, out_yuv + vi);
|
||||
}
|
||||
|
||||
__kernel void rgb_to_yuv(__global uchar const * const rgb,
|
||||
__global uchar * out_yuv)
|
||||
{
|
||||
const int dx = get_global_id(0);
|
||||
const int dy = get_global_id(1);
|
||||
const int col = mul24(dx, 4); // Current column in rgb image
|
||||
const int row = mul24(dy, 4); // Current row in rgb image
|
||||
const int bgri_start = mad24(row, RGB_STRIDE, mul24(col, 3)); // Start offset of rgb data being converted
|
||||
const int yi_start = mad24(row, WIDTH, col); // Start offset in the target yuv buffer
|
||||
int ui = mad24(row / 2, UV_WIDTH, RGB_SIZE + col / 2);
|
||||
int vi = mad24(row / 2 , UV_WIDTH, RGB_SIZE + UV_WIDTH * UV_HEIGHT + col / 2);
|
||||
int num_col = min(WIDTH - col, 4);
|
||||
int num_row = min(HEIGHT - row, 4);
|
||||
if(num_row == 4) {
|
||||
const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start);
|
||||
const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8);
|
||||
const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE);
|
||||
const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8);
|
||||
const uchar8 rgbs2_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2);
|
||||
const uchar8 rgbs2_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2 + 8);
|
||||
const uchar8 rgbs3_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3);
|
||||
const uchar8 rgbs3_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3 + 8);
|
||||
if(num_col == 4) {
|
||||
convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1);
|
||||
convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1);
|
||||
convert_4_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0, rgbs2_1);
|
||||
convert_4_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0, rgbs3_1);
|
||||
convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1);
|
||||
convert_2_uvs(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0, rgbs2_1, rgbs3_1);
|
||||
} else if(num_col == 2) {
|
||||
convert_2_ys(out_yuv, yi_start, rgbs0_0);
|
||||
convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0);
|
||||
convert_2_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0);
|
||||
convert_2_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0);
|
||||
convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0);
|
||||
convert_uv(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0);
|
||||
}
|
||||
} else {
|
||||
const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start);
|
||||
const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8);
|
||||
const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE);
|
||||
const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8);
|
||||
if(num_col == 4) {
|
||||
convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1);
|
||||
convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1);
|
||||
convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1);
|
||||
} else if(num_col == 2) {
|
||||
convert_2_ys(out_yuv, yi_start, rgbs0_0);
|
||||
convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0);
|
||||
convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
#pragma once
|
||||
|
||||
#include "common/clutil.h"
|
||||
|
||||
class Rgb2Yuv {
|
||||
public:
|
||||
Rgb2Yuv(cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride);
|
||||
~Rgb2Yuv();
|
||||
void queue(cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl);
|
||||
private:
|
||||
size_t work_size[2];
|
||||
cl_kernel krnl;
|
||||
};
|
||||
|
||||
@@ -0,0 +1,191 @@
|
||||
#include <fcntl.h>
|
||||
#include <getopt.h>
|
||||
#include <memory.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <csignal>
|
||||
#include <cstdint>
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#ifdef ANDROID
|
||||
|
||||
#define MAXE 0
|
||||
#include <unistd.h>
|
||||
|
||||
#else
|
||||
// The libyuv implementation on ARM is slightly different than on x86
|
||||
// Our implementation matches the ARM version, so accept errors of 1
|
||||
#define MAXE 1
|
||||
|
||||
#endif
|
||||
|
||||
#include <CL/cl.h>
|
||||
|
||||
#include "libyuv.h"
|
||||
#include "system/camerad/transforms/rgb_to_yuv.h"
|
||||
#include "common/clutil.h"
|
||||
|
||||
static inline double millis_since_boot() {
|
||||
struct timespec t;
|
||||
clock_gettime(CLOCK_BOOTTIME, &t);
|
||||
return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6;
|
||||
}
|
||||
|
||||
void cl_init(cl_device_id &device_id, cl_context &context) {
|
||||
device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
|
||||
context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
|
||||
}
|
||||
|
||||
|
||||
bool compare_results(uint8_t *a, uint8_t *b, int len, int stride, int width, int height, uint8_t *rgb) {
|
||||
int min_diff = 0., max_diff = 0., max_e = 0.;
|
||||
int e1 = 0, e0 = 0;
|
||||
int e0y = 0, e0u = 0, e0v = 0, e1y = 0, e1u = 0, e1v = 0;
|
||||
int max_e_i = 0;
|
||||
for (int i = 0;i < len;i++) {
|
||||
int e = ((int)a[i]) - ((int)b[i]);
|
||||
if(e < min_diff) {
|
||||
min_diff = e;
|
||||
}
|
||||
if(e > max_diff) {
|
||||
max_diff = e;
|
||||
}
|
||||
int e_abs = std::abs(e);
|
||||
if(e_abs > max_e) {
|
||||
max_e = e_abs;
|
||||
max_e_i = i;
|
||||
}
|
||||
if(e_abs < 1) {
|
||||
e0++;
|
||||
if(i < stride * height)
|
||||
e0y++;
|
||||
else if(i < stride * height + stride * height / 4)
|
||||
e0u++;
|
||||
else
|
||||
e0v++;
|
||||
} else {
|
||||
e1++;
|
||||
if(i < stride * height)
|
||||
e1y++;
|
||||
else if(i < stride * height + stride * height / 4)
|
||||
e1u++;
|
||||
else
|
||||
e1v++;
|
||||
}
|
||||
}
|
||||
//printf("max diff : %d, min diff : %d, e < 1: %d, e >= 1: %d\n", max_diff, min_diff, e0, e1);
|
||||
//printf("Y: e < 1: %d, e >= 1: %d, U: e < 1: %d, e >= 1: %d, V: e < 1: %d, e >= 1: %d\n", e0y, e1y, e0u, e1u, e0v, e1v);
|
||||
if(max_e <= MAXE) {
|
||||
return true;
|
||||
}
|
||||
int row = max_e_i / stride;
|
||||
if(row < height) {
|
||||
printf("max error is Y: %d = (libyuv: %u - cl: %u), row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], row, max_e_i % stride);
|
||||
} else if(row >= height && row < (height + height / 4)) {
|
||||
printf("max error is U: %d = %u - %u, row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], (row - height) / 2, max_e_i % stride / 2);
|
||||
} else {
|
||||
printf("max error is V: %d = %u - %u, row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], (row - height - height / 4) / 2, max_e_i % stride / 2);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
srand(1337);
|
||||
|
||||
cl_device_id device_id;
|
||||
cl_context context;
|
||||
cl_init(device_id, context) ;
|
||||
|
||||
int err;
|
||||
const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0};
|
||||
cl_command_queue q = clCreateCommandQueueWithProperties(context, device_id, props, &err);
|
||||
if(err != 0) {
|
||||
std::cout << "clCreateCommandQueueWithProperties error: " << err << std::endl;
|
||||
}
|
||||
|
||||
int width = 1164;
|
||||
int height = 874;
|
||||
|
||||
int opt = 0;
|
||||
while ((opt = getopt(argc, argv, "f")) != -1)
|
||||
{
|
||||
switch (opt)
|
||||
{
|
||||
case 'f':
|
||||
std::cout << "Using front camera dimensions" << std::endl;
|
||||
int width = 1152;
|
||||
int height = 846;
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << "Width: " << width << " Height: " << height << std::endl;
|
||||
uint8_t *rgb_frame = new uint8_t[width * height * 3];
|
||||
|
||||
|
||||
RGBToYUVState rgb_to_yuv_state;
|
||||
rgb_to_yuv_init(&rgb_to_yuv_state, context, device_id, width, height, width * 3);
|
||||
|
||||
int frame_yuv_buf_size = width * height * 3 / 2;
|
||||
cl_mem yuv_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, frame_yuv_buf_size, (void*)NULL, &err));
|
||||
uint8_t *frame_yuv_buf = new uint8_t[frame_yuv_buf_size];
|
||||
uint8_t *frame_yuv_ptr_y = frame_yuv_buf;
|
||||
uint8_t *frame_yuv_ptr_u = frame_yuv_buf + (width * height);
|
||||
uint8_t *frame_yuv_ptr_v = frame_yuv_ptr_u + ((width/2) * (height/2));
|
||||
|
||||
cl_mem rgb_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, width * height * 3, (void*)NULL, &err));
|
||||
int mismatched = 0;
|
||||
int counter = 0;
|
||||
srand (time(NULL));
|
||||
|
||||
for (int i = 0; i < 100; i++) {
|
||||
for (int i = 0; i < width * height * 3; i++) {
|
||||
rgb_frame[i] = (uint8_t)rand();
|
||||
}
|
||||
|
||||
double t1 = millis_since_boot();
|
||||
libyuv::RGB24ToI420((uint8_t*)rgb_frame, width * 3,
|
||||
frame_yuv_ptr_y, width,
|
||||
frame_yuv_ptr_u, width/2,
|
||||
frame_yuv_ptr_v, width/2,
|
||||
width, height);
|
||||
double t2 = millis_since_boot();
|
||||
//printf("Libyuv: rgb to yuv: %.2fms\n", t2-t1);
|
||||
|
||||
clEnqueueWriteBuffer(q, rgb_cl, CL_TRUE, 0, width * height * 3, (void *)rgb_frame, 0, NULL, NULL);
|
||||
t1 = millis_since_boot();
|
||||
rgb_to_yuv_queue(&rgb_to_yuv_state, q, rgb_cl, yuv_cl);
|
||||
t2 = millis_since_boot();
|
||||
|
||||
//printf("OpenCL: rgb to yuv: %.2fms\n", t2-t1);
|
||||
uint8_t *yyy = (uint8_t *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE,
|
||||
CL_MAP_READ, 0, frame_yuv_buf_size,
|
||||
0, NULL, NULL, &err);
|
||||
if(!compare_results(frame_yuv_ptr_y, yyy, frame_yuv_buf_size, width, width, height, (uint8_t*)rgb_frame))
|
||||
mismatched++;
|
||||
clEnqueueUnmapMemObject(q, yuv_cl, yyy, 0, NULL, NULL);
|
||||
|
||||
// std::this_thread::sleep_for(std::chrono::milliseconds(20));
|
||||
if(counter++ % 100 == 0)
|
||||
printf("Matched: %d, Mismatched: %d\n", counter - mismatched, mismatched);
|
||||
|
||||
}
|
||||
printf("Matched: %d, Mismatched: %d\n", counter - mismatched, mismatched);
|
||||
|
||||
delete[] frame_yuv_buf;
|
||||
rgb_to_yuv_destroy(&rgb_to_yuv_state);
|
||||
clReleaseContext(context);
|
||||
delete[] rgb_frame;
|
||||
|
||||
if (mismatched == 0)
|
||||
return 0;
|
||||
else
|
||||
return -1;
|
||||
}
|
||||
Reference in New Issue
Block a user