diff options
author | Junkai Wu <junkai.wu@intel.com> | 2017-11-16 16:22:30 +0800 |
---|---|---|
committer | windyuan <feng.yuan@intel.com> | 2017-11-16 17:47:57 +0800 |
commit | cc03b1cbc43f0918fd131aad9a0c6dd475aa58ab (patch) | |
tree | 0feee6a9090be9beb0795187444c2918a0be447a /modules | |
parent | 49736aeb7f37dfe139398aebeb4cee2659bb4c95 (diff) | |
download | libxcam-cc03b1cbc43f0918fd131aad9a0c6dd475aa58ab.tar.gz |
Add generation of top view and rectified view from stitch buffer.
Diffstat (limited to 'modules')
-rw-r--r-- | modules/interface/data_types.h | 5 | ||||
-rw-r--r-- | modules/ocl/cl_utils.cpp | 329 | ||||
-rw-r--r-- | modules/ocl/cl_utils.h | 34 |
3 files changed, 271 insertions, 97 deletions
diff --git a/modules/interface/data_types.h b/modules/interface/data_types.h index fee9d0a..ea246f7 100644 --- a/modules/interface/data_types.h +++ b/modules/interface/data_types.h @@ -74,10 +74,11 @@ struct BowlDataConfig { float ground_length; BowlDataConfig () - : a (5050.0f), b (3656.7f), c (3003.4f) + //: a (5050.0f), b (3656.7f), c (3003.4f) + : a (6060.0f), b (4388.0f), c (3003.4f) , angle_start (90.0f), angle_end (270.0f) , center_z (1500.0f), wall_height (3000.0f) - , ground_length (2168.0f) + , ground_length (2801.0f) // (2168.0f) {} }; diff --git a/modules/ocl/cl_utils.cpp b/modules/ocl/cl_utils.cpp index 9710f85..7fa78c3 100644 --- a/modules/ocl/cl_utils.cpp +++ b/modules/ocl/cl_utils.cpp @@ -192,53 +192,6 @@ convert_nv12_mem_to_video_buffer ( return XCAM_RETURN_NO_ERROR; } -/* - * P00 ------------- P01 - * | | | - * | -- Pinterpoint -- | - * | | | - * P10 ------------- P11 - */ -static void -weighting_interpolate ( - const NV12Pixel &p00, const NV12Pixel &p01, const NV12Pixel &p10, const NV12Pixel &p11, - NV12Pixel &inter_point) -{ - float weight00 = 0.0f, weight01 = 0.0f, weight10 = 0.0f, weight11 = 0.0f; - if (p00.x_pos != p01.x_pos && p00.y_pos != p10.y_pos) { - weight00 = (p11.x_pos - inter_point.x_pos) * (p11.y_pos - inter_point.y_pos); - weight01 = (inter_point.x_pos - p10.x_pos) * (p10.y_pos - inter_point.y_pos); - weight10 = (p01.x_pos - inter_point.x_pos) * (inter_point.y_pos - p00.y_pos); - weight11 = (inter_point.x_pos - p00.x_pos) * (inter_point.y_pos - p00.y_pos); - - inter_point.y = p00.y * weight00 + p01.y * weight01 + p10.y * weight10 + p11.y * weight11; - inter_point.u = p00.u * weight00 + p01.u * weight01 + p10.u * weight10 + p11.u * weight11; - inter_point.v = p00.v * weight00 + p01.v * weight01 + p10.v * weight10 + p11.v * weight11; - } else if (p00.x_pos == p01.x_pos && p00.y_pos != p10.y_pos) { - weight00 = p10.y_pos - inter_point.y_pos; - weight10 = inter_point.y_pos - p00.y_pos; - - inter_point.y = p00.y * weight00 + p10.y * weight10; - inter_point.u = p00.u * weight00 + p10.u * weight10; - inter_point.v = p00.v * weight00 + p10.v * weight10; - } else if (p00.y_pos == p10.y_pos && p00.x_pos != p01.x_pos ) { - weight00 = p01.x_pos - inter_point.x_pos; - weight01 = inter_point.x_pos - p00.x_pos; - - inter_point.y = p00.y * weight00 + p01.y * weight01; - inter_point.u = p00.u * weight00 + p01.u * weight01; - inter_point.v = p00.v * weight00 + p01.v * weight01; - } else { - inter_point.y = p00.y; - inter_point.u = p00.u; - inter_point.v = p00.v; - } - - clamp (inter_point.y, 0.0f, 255.0f); - clamp (inter_point.u, 0.0f, 255.0f); - clamp (inter_point.v, 0.0f, 255.0f); -} - static void transform_x_coordinate ( const VideoBufferInfo &info, @@ -246,8 +199,8 @@ transform_x_coordinate ( float &x_trans) { float step = info.width / (2.0f * PI); - float offset_radian = (x_pos < 0.0f) ? PI : ((y_pos >= 0.0f) ? 0.0f : 2.0f * PI); - float arctan_radian = (x_pos != 0.0f) ? atan (y_pos / x_pos) : ((y_pos >= 0.0f) ? PI / 2.0f : -PI / 2.0f); + float offset_radian = (x_pos < 0.0f) ? PI : ((y_pos >= 0.0f) ? 2.0f * PI : 0.0f); + float arctan_radian = (x_pos != 0.0f) ? atan (-y_pos / x_pos) : ((y_pos >= 0.0f) ? -PI / 2.0f : PI / 2.0f); x_trans = arctan_radian + offset_radian; x_trans *= step; @@ -268,14 +221,14 @@ transform_y_coordinate ( y_trans = (config.wall_height - z_pos) * wall_image_height / config.wall_height; clamp (y_trans, 0.0f, wall_image_height - 1.0f); } else { - float max_semimajor = config.a * + float max_semimajor = config.b * sqrt (1 - config.center_z * config.center_z / (config.c * config.c)); float min_semimajor = max_semimajor - config.ground_length; XCAM_ASSERT (max_semimajor > min_semimajor); float step = ground_image_height / (max_semimajor - min_semimajor); float axis_ratio = config.a / config.b; - float cur_semimajor = sqrt (x_pos * x_pos + y_pos * y_pos * axis_ratio * axis_ratio); + float cur_semimajor = sqrt (x_pos * x_pos + y_pos * y_pos * axis_ratio * axis_ratio) / axis_ratio; clamp (cur_semimajor, min_semimajor, max_semimajor); y_trans = (max_semimajor - cur_semimajor) * step + wall_image_height; @@ -283,69 +236,261 @@ transform_y_coordinate ( } } -static void -transform_coordinates ( +XCamReturn +bowl_view_coords_to_image ( const VideoBufferInfo &info, const BowlDataConfig &config, float x_pos, float y_pos, float z_pos, - NV12Pixel &p00, NV12Pixel &p01, NV12Pixel &p10, NV12Pixel &p11, NV12Pixel &inter_point) + float &x_trans, float &y_trans) { - float x_trans, y_trans; transform_x_coordinate (info, x_pos, y_pos, x_trans); transform_y_coordinate (info, config, x_pos, y_pos, z_pos, y_trans); - inter_point.x_pos = x_trans; - inter_point.y_pos = y_trans; + return XCAM_RETURN_NO_ERROR; +} + +XCamReturn +interpolate_pixel_value ( + uint8_t* stitch_mem, + float image_coord_x, float image_coord_y, + float &y, float &u, float &v, + const VideoBufferInfo& stitch_info) +{ + XCAM_ASSERT (image_coord_y < stitch_info.height && image_coord_x < stitch_info.width); + + uint8_t y00, y01, y10, y11; + uint8_t u00, u01, u10, u11; + uint8_t v00, v01, v10, v11; + + uint32_t x0 = (uint32_t) image_coord_x; + uint32_t x1 = (x0 < stitch_info.width - 1) ? (x0 + 1) : x0; + uint32_t y0 = (uint32_t) image_coord_y; + uint32_t y1 = (y0 < stitch_info.height - 1) ? (y0 + 1) : y0; - p00.x_pos = (uint32_t)inter_point.x_pos; - p00.y_pos = (uint32_t)inter_point.y_pos; - p01.x_pos = (p00.x_pos == (info.width - 1.0f)) ? p00.x_pos : (p00.x_pos + 1.0f); - p01.y_pos = p00.y_pos; + float rate00 = (x0 + 1 - image_coord_x) * (y0 + 1 - image_coord_y); + float rate01 = (x0 + 1 - image_coord_x) * (image_coord_y - y0); + float rate10 = (image_coord_x - x0) * (y0 + 1 - image_coord_y); + float rate11 = (image_coord_x - x0) * (image_coord_y - y0); - p10.x_pos = p00.x_pos; - p10.y_pos = (p00.y_pos == (info.height - 1.0f)) ? p00.y_pos : (p00.y_pos + 1.0f); - p11.x_pos = (p00.x_pos == (info.width - 1.0f)) ? p00.x_pos : (p00.x_pos + 1.0f); - p11.y_pos = (p00.y_pos == (info.height - 1.0f)) ? p00.y_pos : (p00.y_pos + 1.0f); + y00 = stitch_mem[y0 * stitch_info.strides[0] + x0]; + y01 = stitch_mem[y1 * stitch_info.strides[0] + x0]; + y10 = stitch_mem[y0 * stitch_info.strides[0] + x1]; + y11 = stitch_mem[y1 * stitch_info.strides[0] + x1]; + + u00 = stitch_mem[stitch_info.offsets[1] + y0 / 2 * stitch_info.strides[1] + XCAM_ALIGN_DOWN (x0, 2)]; + u01 = stitch_mem[stitch_info.offsets[1] + y1 / 2 * stitch_info.strides[1] + XCAM_ALIGN_DOWN (x0, 2)]; + u10 = stitch_mem[stitch_info.offsets[1] + y0 / 2 * stitch_info.strides[1] + XCAM_ALIGN_DOWN (x1, 2)]; + u11 = stitch_mem[stitch_info.offsets[1] + y1 / 2 * stitch_info.strides[1] + XCAM_ALIGN_DOWN (x1, 2)]; + + v00 = stitch_mem[stitch_info.offsets[1] + y0 / 2 * stitch_info.strides[1] + XCAM_ALIGN_DOWN (x0, 2) + 1]; + v01 = stitch_mem[stitch_info.offsets[1] + y1 / 2 * stitch_info.strides[1] + XCAM_ALIGN_DOWN (x0, 2) + 1]; + v10 = stitch_mem[stitch_info.offsets[1] + y0 / 2 * stitch_info.strides[1] + XCAM_ALIGN_DOWN (x1, 2) + 1]; + v11 = stitch_mem[stitch_info.offsets[1] + y1 / 2 * stitch_info.strides[1] + XCAM_ALIGN_DOWN (x1, 2) + 1]; + + y = y00 * rate00 + y01 * rate01 + y10 * rate10 + y11 * rate11; + u = u00 * rate00 + u01 * rate01 + u10 * rate10 + u11 * rate11; + v = v00 * rate00 + v01 * rate01 + v10 * rate10 + v11 * rate11; + + return XCAM_RETURN_NO_ERROR; } -static void -fill_pixel_value (uint8_t *mem, const VideoBufferInfo &info, NV12Pixel &p) +XCamReturn +map_to_specific_view ( + uint8_t *specific_view_mem, uint8_t* stitch_mem, + uint32_t row, uint32_t col, + float image_coord_x, float image_coord_y, + const VideoBufferInfo& specific_view_info, const VideoBufferInfo& stitch_info) { - uint32_t y_pos = (uint32_t)p.y_pos; - uint32_t x_pos = (uint32_t)p.x_pos; + XCAM_ASSERT (row < specific_view_info.height && col < specific_view_info.width); + + float y, u, v; - uint32_t pos = y_pos * info.strides[0] + x_pos; - p.y = mem[pos]; + interpolate_pixel_value (stitch_mem, image_coord_x, image_coord_y, y, u, v, stitch_info); - pos = info.offsets[1] + y_pos / 2 * info.strides[1] + XCAM_ALIGN_DOWN (x_pos, 2); - p.u = mem[pos]; - p.v = mem[pos + 1]; + uint32_t y_index = row * specific_view_info.strides[0] + col; + uint32_t u_index = specific_view_info.offsets[1] + row / 2 * specific_view_info.strides[1] + XCAM_ALIGN_DOWN (col, 2); + + specific_view_mem[y_index] = (uint8_t)y; + specific_view_mem[u_index] = (uint8_t)u; + specific_view_mem[u_index + 1] = (uint8_t)v; + + return XCAM_RETURN_NO_ERROR; } XCamReturn -get_bowl_view_data ( - SmartPtr<VideoBuffer> &buf, const BowlDataConfig &config, - float x_pos, float y_pos, float z_pos, - float &y, float &u, float &v) +generate_topview_map_table ( + const VideoBufferInfo &stitch_info, + const BowlDataConfig &config, + std::vector<float> &map_table, + int width, int height) { - const VideoBufferInfo info = buf->get_video_info (); + int center_x = width / 2; + int center_y = height / 2; - NV12Pixel p00, p01, p10, p11, inter_point; - transform_coordinates (info, config, x_pos, y_pos, z_pos, p00, p01, p10, p11, inter_point); + float show_width_mm = 5000.0f; + float length_per_pixel = show_width_mm / height; - uint8_t *mem = buf->map (); - XCAM_FAIL_RETURN (ERROR, mem, XCAM_RETURN_ERROR_MEM, "map buffer failed"); + for(int row = 0; row < height; row++) { + for(int col = 0; col < width; col++) { + float world_x = (col - center_x) * length_per_pixel; + float world_y = (center_y - row) * length_per_pixel; + float world_z = 0.0f; - fill_pixel_value (mem, info, p00); - fill_pixel_value (mem, info, p01); - fill_pixel_value (mem, info, p10); - fill_pixel_value (mem, info, p11); - buf->unmap (); + float image_coord_x, image_coord_y; + bowl_view_coords_to_image (stitch_info, config, world_x, world_y, world_z, image_coord_x, image_coord_y); + + map_table[row * width * 2 + col * 2] = image_coord_x; + map_table[row * width * 2 + col * 2 + 1] = image_coord_y; + } + } + + return XCAM_RETURN_NO_ERROR; +} + +XCamReturn +generate_rectifiedview_map_table ( + const VideoBufferInfo &stitch_info, + const BowlDataConfig &config, + std::vector<float> &map_table, + float angle_start, float angle_end, + int width, int height) +{ + float center_x = width / 2; + + float focal_plane_dist = 6000.0f; + + float angle_center = (angle_start + angle_end) / 2.0f; + float theta = degree2radian((angle_end - angle_start)) / 2.0f; + float length_per_pixel_x = 2 * focal_plane_dist * tan (theta) / width; + + float fov_up = degree2radian (20.0f); + float fov_down = degree2radian (35.0f); + + float length_per_pixel_y = (focal_plane_dist * tan (fov_up) + focal_plane_dist * tan (fov_down)) / height; + + float center_y = tan (fov_up) / (tan (fov_up) + tan (fov_down)) * height; + + float world_x, world_y, world_z; + float plane_center_coords[3]; + + plane_center_coords[0] = focal_plane_dist * cos (degree2radian (angle_center)); + plane_center_coords[1] = -focal_plane_dist * sin (degree2radian (angle_center)); + plane_center_coords[2] = 0.0f; + + for (int row = 0; row < height; row++) { + for (int col = 0; col < width; col++) { + float plane_point_coords[3]; + plane_point_coords[0] = (center_x - col) * length_per_pixel_x * cos (PI / 2 - degree2radian (angle_center)) + plane_center_coords[0]; + plane_point_coords[1] = (center_x - col) * length_per_pixel_x * sin (PI / 2 - degree2radian (angle_center)) + plane_center_coords[1]; + plane_point_coords[2] = (center_y - row) * length_per_pixel_y + plane_center_coords[2]; + + float rate_xz, rate_yz; + if (XCAM_DOUBLE_EQUAL_AROUND (plane_point_coords[2], 0.0f) && XCAM_DOUBLE_EQUAL_AROUND (plane_point_coords[1], 0.0f)) { + world_x = config.a; + world_y = 0; + world_z = 0; + } else if (XCAM_DOUBLE_EQUAL_AROUND (plane_point_coords[2], 0.0f)) { + world_z = 0.0f; + + float rate_xy = plane_point_coords[0] / plane_point_coords[1]; + float square_y = 1 / (rate_xy * rate_xy / (config.a * config.a) + 1 / (config.b * config.b)); + world_y = (plane_point_coords[1] > 0) ? sqrt (square_y) : -sqrt (square_y); + world_x = rate_xy * world_y; + } else { + rate_xz = plane_point_coords[0] / plane_point_coords[2]; + rate_yz = plane_point_coords[1] / plane_point_coords[2]; + + float square_z = 1 / (rate_xz * rate_xz / (config.a * config.a) + rate_yz * rate_yz / (config.b * config.b) + 1 / (config.c * config.c)); + world_z = (plane_point_coords[2] > 0) ? sqrt (square_z) : -sqrt (square_z); + world_z = (world_z <= -config.center_z) ? -config.center_z : world_z; + world_x = rate_xz * world_z; + world_y = rate_yz * world_z; + } + + world_z += config.center_z; + + float image_coord_x, image_coord_y; + bowl_view_coords_to_image (stitch_info, config, world_x, world_y, world_z, image_coord_x, image_coord_y); + + map_table[row * width * 2 + col * 2] = image_coord_x; + map_table[row * width * 2 + col * 2 + 1] = image_coord_y; + } + } + + return XCAM_RETURN_NO_ERROR; +} + +XCamReturn +sample_generate_top_view ( + SmartPtr<VideoBuffer> &stitch_buf, + SmartPtr<VideoBuffer> top_view_buf, + const BowlDataConfig &config, + std::vector<float> &map_table, int frame_id) +{ + const VideoBufferInfo top_view_info = top_view_buf->get_video_info (); + const VideoBufferInfo stitch_info = stitch_buf->get_video_info (); + + int top_view_resolution_w = top_view_buf->get_video_info ().width; + int top_view_resolution_h = top_view_buf->get_video_info ().height; + + if(frame_id == 0) { + generate_topview_map_table (stitch_info, config, map_table, top_view_resolution_w, top_view_resolution_h); + } + + uint8_t *top_view_mem = NULL; + uint8_t *stitch_mem = NULL; + top_view_mem = top_view_buf->map (); + stitch_mem = stitch_buf->map (); + + for(int row = 0; row < top_view_resolution_h; row++) { + for(int col = 0; col < top_view_resolution_w; col++) { + float image_coord_x = map_table[row * top_view_resolution_w * 2 + col * 2]; + float image_coord_y = map_table[row * top_view_resolution_w * 2 + col * 2 + 1]; + + map_to_specific_view (top_view_mem, stitch_mem, row, col, image_coord_x, image_coord_y, top_view_info, stitch_info); + } + } + + top_view_buf->unmap(); + stitch_buf->unmap(); + + return XCAM_RETURN_NO_ERROR; +} + +XCamReturn +sample_generate_rectified_view ( + SmartPtr<VideoBuffer> &stitch_buf, + SmartPtr<VideoBuffer> rectified_view_buf, + const BowlDataConfig &config, + float angle_start, float angle_end, + std::vector<float> &map_table, int frame_id) +{ + const VideoBufferInfo rectified_view_info = rectified_view_buf->get_video_info (); + const VideoBufferInfo stitch_info = stitch_buf->get_video_info (); + + int rectified_view_resolution_w = rectified_view_buf->get_video_info ().width; + int rectified_view_resolution_h = rectified_view_buf->get_video_info ().height; + + if(frame_id == 0) { + generate_rectifiedview_map_table (stitch_info, config, map_table, angle_start, angle_end, rectified_view_resolution_w, rectified_view_resolution_h); + } + + uint8_t *rectified_view_mem = NULL; + uint8_t *stitch_mem = NULL; + rectified_view_mem = rectified_view_buf->map (); + stitch_mem = stitch_buf->map (); + + for(int row = 0; row < rectified_view_resolution_h; row++) { + for(int col = 0; col < rectified_view_resolution_w; col++) { + float image_coord_x = map_table[row * rectified_view_resolution_w * 2 + col * 2]; + float image_coord_y = map_table[row * rectified_view_resolution_w * 2 + col * 2 + 1]; + + map_to_specific_view (rectified_view_mem, stitch_mem, row, col, image_coord_x, image_coord_y, rectified_view_info, stitch_info); + } + } - weighting_interpolate (p00, p01, p10, p11, inter_point); - y = inter_point.y; - u = inter_point.u; - v = inter_point.v; + rectified_view_buf->unmap(); + stitch_buf->unmap(); return XCAM_RETURN_NO_ERROR; } diff --git a/modules/ocl/cl_utils.h b/modules/ocl/cl_utils.h index 753c321..70d17ff 100644 --- a/modules/ocl/cl_utils.h +++ b/modules/ocl/cl_utils.h @@ -49,11 +49,39 @@ XCamReturn convert_nv12_mem_to_video_buffer ( void *nv12_mem, uint32_t width, uint32_t height, uint32_t row_pitch, uint32_t offset_uv, SmartPtr<VideoBuffer> &buf); -XCamReturn get_bowl_view_data ( - SmartPtr<VideoBuffer> &buf, const BowlDataConfig &config, +XCamReturn bowl_view_coords_to_image ( + const VideoBufferInfo& info, + const BowlDataConfig &config, float x_pos, float y_pos, float z_pos, - float &y, float &u, float &v); + float &x_trans, float &y_trans); +XCamReturn +generate_topview_map_table ( + const VideoBufferInfo &stitch_info, + const BowlDataConfig &config, + std::vector<float> &map_table, + int width, int height); + +XCamReturn +generate_rectifiedview_map_table ( + const VideoBufferInfo &stitch_info, + const BowlDataConfig &config, + std::vector<float> &map_table, + float angle_start, float angle_end, + int width, int height); + +XCamReturn sample_generate_top_view ( + SmartPtr<VideoBuffer> &stitch_buf, + SmartPtr<VideoBuffer> top_view_buf, + const BowlDataConfig &config, + std::vector<float> &map_table, int frame_id); + +XCamReturn sample_generate_rectified_view ( + SmartPtr<VideoBuffer> &stitch_buf, + SmartPtr<VideoBuffer> rectified_view_buf, + const BowlDataConfig &config, + float angle_start, float angle_end, + std::vector<float> &map_table, int frame_id); } #endif //XCAM_CL_UTILS_H |