From e06c745d4a88fe4a59e7fa1bc7a0c7c024f79fca Mon Sep 17 00:00:00 2001 From: Junkai Wu Date: Sat, 30 Sep 2017 14:22:46 +0800 Subject: Add module for dewarping fisheye cameras for surround view. --- modules/ocl/Makefile.am | 2 + modules/ocl/cv_surview_fisheye_dewarp.cpp | 243 ++++++++++++++++++++++++++++++ modules/ocl/cv_surview_fisheye_dewarp.h | 100 ++++++++++++ 3 files changed, 345 insertions(+) create mode 100644 modules/ocl/cv_surview_fisheye_dewarp.cpp create mode 100644 modules/ocl/cv_surview_fisheye_dewarp.h (limited to 'modules/ocl') diff --git a/modules/ocl/Makefile.am b/modules/ocl/Makefile.am index 395a481..8964825 100644 --- a/modules/ocl/Makefile.am +++ b/modules/ocl/Makefile.am @@ -81,6 +81,7 @@ xcam_ocl_sources += cv_wiener_filter.cpp xcam_ocl_sources += cv_feature_match.cpp xcam_ocl_sources += cv_image_deblurring.cpp xcam_ocl_sources += cv_calibration_parser.cpp +xcam_ocl_sources += cv_surview_fisheye_dewarp.cpp endif if HAVE_LIBDRM @@ -161,6 +162,7 @@ nobase_libxcam_oclinclude_HEADERS += cv_wiener_filter.h nobase_libxcam_oclinclude_HEADERS += cv_feature_match.h nobase_libxcam_oclinclude_HEADERS += cv_image_deblurring.h nobase_libxcam_oclinclude_HEADERS += cv_calibration_parser.h +nobase_libxcam_oclinclude_HEADERS += cv_surview_fisheye_dewarp.h endif if HAVE_LIBDRM diff --git a/modules/ocl/cv_surview_fisheye_dewarp.cpp b/modules/ocl/cv_surview_fisheye_dewarp.cpp new file mode 100644 index 0000000..be1c5fc --- /dev/null +++ b/modules/ocl/cv_surview_fisheye_dewarp.cpp @@ -0,0 +1,243 @@ +/* + * cv_surview_fisheye_dewarp.cpp - dewarp fisheye image of surround view + * + * Copyright (c) 2016-2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Author: Junkai Wu + */ + +#include "cv_surview_fisheye_dewarp.h" + +namespace XCam { + +BowlDataConfig::BowlDataConfig() +{ + image_height = 480; + image_heightZ0 = 320; + angle_start = PI / 2; + angle_end = 3 * PI / 2; + + a = 5050.0f; + b = 3656.7f; + c = 3003.4f; +} + +CVSurViewFisheyeDewarp::CVSurViewFisheyeDewarp () + : CVBaseClass() +{ +} + +CVPolyFisheyeDewarp::CVPolyFisheyeDewarp() + : CVSurViewFisheyeDewarp() +{ +} + +void +CVSurViewFisheyeDewarp::set_intrinsic_param(const IntrinsicParameter &intrinsic_param) +{ + _intrinsic_param = intrinsic_param; +} + +void +CVSurViewFisheyeDewarp::set_extrinsic_param(const ExtrinsicParameter &extrinsic_param) +{ + _extrinsic_param = extrinsic_param; +} + +IntrinsicParameter +CVSurViewFisheyeDewarp::get_intrinsic_param() +{ + return _intrinsic_param; +} + +ExtrinsicParameter +CVSurViewFisheyeDewarp::get_extrinsic_param() +{ + return _extrinsic_param; +} + +void +CVSurViewFisheyeDewarp::fisheye_dewarp(MapTable &map_table, uint32_t table_w, uint32_t table_h, uint32_t image_w, uint32_t image_h, const BowlDataConfig &bowl_config) +{ + MapTable world_coord(3); + MapTable cam_coord(3); + MapTable cam_world_coord(3); + MapTable image_coord(2); + + uint32_t scale_factor_w = image_w / table_w; + uint32_t scale_factor_h = image_h / table_h; + + for(uint32_t row = 0; row < table_h; row++) { + for(uint32_t col = 0; col < table_w; col++) { + uint32_t x = col * scale_factor_w; + uint32_t y = row * scale_factor_h; + + cal_world_coord(x, y, world_coord, image_w, bowl_config); + cal_cam_world_coord(world_coord, cam_world_coord); + world_coord2cam(cam_world_coord, cam_coord); + cal_image_coord(cam_coord, image_coord); + + map_table[row * table_w * 2 + col * 2] = image_coord[0]; + map_table[row * table_w * 2 + col * 2 + 1] = image_coord[1]; + } + } +} + +void +CVSurViewFisheyeDewarp::cal_world_coord(uint32_t x, uint32_t y, MapTable &world_coord, uint32_t image_w, const BowlDataConfig &bowl_config) +{ + float world_x, world_y, world_z; + float angle; + + float a = bowl_config.a; + float b = bowl_config.b; + float c = bowl_config.c; + + float z_step = 3000.0f / bowl_config.image_height; + float angle_step = fabs(bowl_config.angle_end - bowl_config.angle_start) / image_w; + + if(y < bowl_config.image_height) { + world_z = 1500.0f - y * z_step; + angle = bowl_config.angle_start - x * angle_step; + float r2 = 1 - world_z * world_z / (c * c); + + if(angle == PI / 2) { + world_x = 0.0f; + world_y = sqrt(r2 * b * b); + } else if (angle == PI * 3 / 2) { + world_x = 0.0f; + world_y = -sqrt(r2 * b * b); + } else if((angle < PI / 2) || (angle > PI * 3 / 2)) { + world_x = sqrt(r2 * a * a * b * b / (b * b + a * a * tan(angle) * tan(angle))); + world_y = world_x * tan(angle); + } else { + world_x = -sqrt(r2 * a * a * b * b / (b * b + a * a * tan(angle) * tan(angle))); + world_y = world_x * tan(angle); + } + } else { + world_z = -1500.0f; + a = a * sqrt(1 - world_z * world_z / (c * c)); + b = b * sqrt(1 - world_z * world_z / (c * c)); + + float step_a = (a - 920.0f) / bowl_config.image_heightZ0; + float step_b = (b - 920.0f) / bowl_config.image_heightZ0; + + a = a - (y - bowl_config.image_height) * step_a; + b = b - (y - bowl_config.image_height) * step_b; + + angle = bowl_config.angle_start - x * angle_step; + + if(angle == PI / 2) { + world_x = 0.0f; + world_y = b; + } else if (angle == PI * 3 / 2) { + world_x = 0.0f; + world_y = -b; + } else if((angle < PI / 2) || (angle > PI * 3 / 2)) { + world_x = a * b / sqrt(b * b + a * a * tan(angle) * tan(angle)); + world_y = world_x * tan(angle); + } else { + world_x = -a * b / sqrt(b * b + a * a * tan(angle) * tan(angle)); + world_y = world_x * tan(angle); + } + } + + world_coord[0] = world_x - 2000.0f; + world_coord[1] = world_y; + world_coord[2] = world_z + 1500.0f; +} + +void +CVSurViewFisheyeDewarp::cal_cam_world_coord(MapTable world_coord, MapTable &cam_world_coord) +{ + cv::Matx44f rotation_mat = generate_rotation_matrix(_extrinsic_param.roll * PI / 180, + _extrinsic_param.pitch * PI / 180, + _extrinsic_param.yaw * PI / 180); + cv::Matx44f rotation_tran_mat(rotation_mat); + rotation_tran_mat(0, 3) = _extrinsic_param.trans_x; + rotation_tran_mat(1, 3) = _extrinsic_param.trans_y; + rotation_tran_mat(2, 3) = _extrinsic_param.trans_z; + + cv::Matx44f world_coord_mat(1.0, 0.0, 0.0, world_coord[0], 0.0, 1.0, 0.0, world_coord[1], 0.0, 0.0, 1.0, world_coord[2], 0.0, 0.0, 0.0, 1.0); + cv::Matx44f cam_world_coord_mat = rotation_tran_mat.inv() * world_coord_mat; + + cam_world_coord[0] = cam_world_coord_mat(0, 3); + cam_world_coord[1] = cam_world_coord_mat(1, 3); + cam_world_coord[2] = cam_world_coord_mat(2, 3); +} + +cv::Matx44f +CVSurViewFisheyeDewarp::generate_rotation_matrix(float roll, float pitch, float yaw) +{ + cv::Matx44f matrix_x(1.0, 0.0, 0.0, 0.0, + 0.0, cos(roll), -sin(roll), 0.0, + 0.0, sin(roll), cos(roll), 0.0, + 0.0, 0.0, 0.0, 1.0); + + cv::Matx44f matrix_y(cos(pitch), 0.0, sin(pitch), 0.0, + 0.0, 1.0, 0.0, 0.0, + -sin(pitch), 0.0, cos(pitch), 0.0, + 0.0, 0.0, 0.0, 1.0); + + cv::Matx44f matrix_z(cos(yaw), -sin(yaw), 0.0, 0.0, + sin(yaw), cos(yaw), 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0); + + return matrix_z * matrix_y * matrix_x; +} + +void +CVSurViewFisheyeDewarp::world_coord2cam(MapTable cam_world_coord, MapTable &cam_coord) +{ + cam_coord[0] = -cam_world_coord[1]; + cam_coord[1] = -cam_world_coord[2]; + cam_coord[2] = -cam_world_coord[0]; +} + +void +CVSurViewFisheyeDewarp::cal_image_coord(MapTable cam_coord, MapTable &image_coord) +{ +} + +void +CVPolyFisheyeDewarp::cal_image_coord(MapTable cam_coord, MapTable &image_coord) +{ + float dist2center = sqrt(cam_coord[0] * cam_coord[0] + cam_coord[1] * cam_coord[1]); + float angle = atan(cam_coord[2] / dist2center); + + float p = 1; + float poly_sum = 0; + + IntrinsicParameter intrinsic_param = get_intrinsic_param(); + + if (dist2center != 0) { + for (uint32_t i = 0; i < intrinsic_param.poly_length; i++) { + poly_sum += intrinsic_param.poly_coeff[i] * p; + p = p * angle; + } + + float image_x = cam_coord[0] * poly_sum / dist2center; + float image_y = cam_coord[1] * poly_sum / dist2center; + + image_coord[0] = image_x * intrinsic_param.c + image_y * intrinsic_param.d + intrinsic_param.xc; + image_coord[1] = image_x * intrinsic_param.e + image_y + intrinsic_param.yc; + } else { + image_coord[0] = intrinsic_param.xc; + image_coord[1] = intrinsic_param.yc; + } +} // Adopt Scaramuzza's approach to calculate image coordinates from camera coordinates + +} diff --git a/modules/ocl/cv_surview_fisheye_dewarp.h b/modules/ocl/cv_surview_fisheye_dewarp.h new file mode 100644 index 0000000..3c098a8 --- /dev/null +++ b/modules/ocl/cv_surview_fisheye_dewarp.h @@ -0,0 +1,100 @@ +/* + * cv_surview_fisheye_dewarp.h - dewarp fisheye image for surround view + * + * Copyright (c) 2016-2017 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Author: Junkai Wu + */ + +#ifndef XCAM_CV_SURVIEW_FISHEYE_DEWARP_H +#define XCAM_CV_SURVIEW_FISHEYE_DEWARP_H + +#include +#include +#include +#include +#include "xcam_obj_debug.h" +#include "image_file_handle.h" +#include "cv_base_class.h" +#include "cv_calibration_parser.h" + +#include +#include +#include + +#include +#include + +namespace XCam { + +class BowlDataConfig +{ + +public: + explicit BowlDataConfig(); + + uint32_t image_height; + uint32_t image_heightZ0; + + float a, b, c; + float angle_start, angle_end; + +}; + +class CVSurViewFisheyeDewarp : public CVBaseClass +{ + +public: + typedef std::vector MapTable; + + explicit CVSurViewFisheyeDewarp (); + + void fisheye_dewarp(MapTable &map_table, uint32_t table_w, uint32_t table_h, uint32_t image_w, uint32_t image_h, const BowlDataConfig &bowl_config); + + void set_intrinsic_param(const IntrinsicParameter &intrinsic_param); + void set_extrinsic_param(const ExtrinsicParameter &extrinsic_param); + + IntrinsicParameter get_intrinsic_param(); + ExtrinsicParameter get_extrinsic_param(); + +private: + XCAM_DEAD_COPY (CVSurViewFisheyeDewarp); + + virtual void cal_image_coord(MapTable cam_coord, MapTable &image_coord); + + void cal_world_coord(uint32_t x, uint32_t y, MapTable &world_coord, uint32_t image_w, const BowlDataConfig &bowl_config); + void cal_cam_world_coord(MapTable world_coord, MapTable &cam_world_coord); + void world_coord2cam(MapTable cam_world_coord, MapTable &cam_coord); + + cv::Matx44f generate_rotation_matrix(float roll, float pitch, float yaw); + +private: + IntrinsicParameter _intrinsic_param; + ExtrinsicParameter _extrinsic_param; +}; + +class CVPolyFisheyeDewarp : public CVSurViewFisheyeDewarp +{ + +public: + explicit CVPolyFisheyeDewarp (); + + void cal_image_coord(MapTable cam_coord, MapTable &image_coord); + +}; + +} // Adopt Scaramuzza's approach to calculate image coordinates from camera coordinates + +#endif // XCAM_CV_SURVIEW_FISHEYE_DEWARP_H -- cgit v1.2.3