Add module for dewarping fisheye cameras for surround view.

3 files changed, 345 insertions, 0 deletions

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 <junkai.wu@intel.com>

+ */

+

+#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 <junkai.wu@intel.com>

+ */

+

+#ifndef XCAM_CV_SURVIEW_FISHEYE_DEWARP_H

+#define XCAM_CV_SURVIEW_FISHEYE_DEWARP_H

+

+#include <base/xcam_common.h>

+#include <base/xcam_buffer.h>

+#include <dma_video_buffer.h>

+#include <smartptr.h>

+#include "xcam_obj_debug.h"

+#include "image_file_handle.h"

+#include "cv_base_class.h"

+#include "cv_calibration_parser.h"

+

+#include <ocl/cl_context.h>

+#include <ocl/cl_device.h>

+#include <ocl/cl_memory.h>

+

+#include <opencv2/opencv.hpp>

+#include <opencv2/core/ocl.hpp>

+

+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<float> 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