1 /*
2 * surview_fisheye_dewarp.cpp - dewarp fisheye image of surround view
3 *
4 * Copyright (c) 2016-2017 Intel Corporation
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 *
18 * Author: Junkai Wu <junkai.wu@intel.com>
19 */
20
21 #include "surview_fisheye_dewarp.h"
22 #include "xcam_utils.h"
23
24 namespace XCam {
25
SurViewFisheyeDewarp()26 SurViewFisheyeDewarp::SurViewFisheyeDewarp ()
27 {
28 }
~SurViewFisheyeDewarp()29 SurViewFisheyeDewarp::~SurViewFisheyeDewarp ()
30 {
31 }
32
PolyFisheyeDewarp()33 PolyFisheyeDewarp::PolyFisheyeDewarp()
34 : SurViewFisheyeDewarp()
35 {
36 }
37
38 void
set_intrinsic_param(const IntrinsicParameter & intrinsic_param)39 SurViewFisheyeDewarp::set_intrinsic_param(const IntrinsicParameter &intrinsic_param)
40 {
41 _intrinsic_param = intrinsic_param;
42 }
43
44 void
set_extrinsic_param(const ExtrinsicParameter & extrinsic_param)45 SurViewFisheyeDewarp::set_extrinsic_param(const ExtrinsicParameter &extrinsic_param)
46 {
47 _extrinsic_param = extrinsic_param;
48 }
49
50 IntrinsicParameter
get_intrinsic_param()51 SurViewFisheyeDewarp::get_intrinsic_param()
52 {
53 return _intrinsic_param;
54 }
55
56 ExtrinsicParameter
get_extrinsic_param()57 SurViewFisheyeDewarp::get_extrinsic_param()
58 {
59 return _extrinsic_param;
60 }
61
62 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)63 SurViewFisheyeDewarp::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)
64 {
65 PointFloat3 world_coord;
66 PointFloat3 cam_coord;
67 PointFloat3 cam_world_coord;
68 PointFloat2 image_coord;
69
70 XCAM_LOG_DEBUG ("fisheye-dewarp:\n table(%dx%d), out_size(%dx%d)"
71 "bowl(start:%.1f, end:%.1f, ground:%.2f, wall:%.2f, a:%.2f, b:%.2f, c:%.2f, center_z:%.2f )",
72 table_w, table_h, image_w, image_h,
73 bowl_config.angle_start, bowl_config.angle_end,
74 bowl_config.wall_height, bowl_config.ground_length,
75 bowl_config.a, bowl_config.b, bowl_config.c, bowl_config.center_z);
76
77 float scale_factor_w = (float)image_w / table_w;
78 float scale_factor_h = (float)image_h / table_h;
79
80 for(uint32_t row = 0; row < table_h; row++) {
81 for(uint32_t col = 0; col < table_w; col++) {
82 PointFloat2 out_pos (col * scale_factor_w, row * scale_factor_h);
83 world_coord = bowl_view_image_to_world (bowl_config, image_w, image_h, out_pos);
84 cal_cam_world_coord(world_coord, cam_world_coord);
85 world_coord2cam(cam_world_coord, cam_coord);
86 cal_image_coord(cam_coord, image_coord);
87
88 map_table[row * table_w + col] = image_coord;
89 }
90 }
91 }
92
93 void
cal_cam_world_coord(const PointFloat3 & world_coord,PointFloat3 & cam_world_coord)94 SurViewFisheyeDewarp::cal_cam_world_coord(const PointFloat3 &world_coord, PointFloat3 &cam_world_coord)
95 {
96 Mat4f rotation_mat = generate_rotation_matrix( degree2radian (_extrinsic_param.roll),
97 degree2radian (_extrinsic_param.pitch),
98 degree2radian (_extrinsic_param.yaw));
99 Mat4f rotation_tran_mat = rotation_mat;
100 rotation_tran_mat(0, 3) = _extrinsic_param.trans_x;
101 rotation_tran_mat(1, 3) = _extrinsic_param.trans_y;
102 rotation_tran_mat(2, 3) = _extrinsic_param.trans_z;
103
104 Mat4f world_coord_mat(Vec4f(1.0f, 0.0f, 0.0f, world_coord.x),
105 Vec4f(0.0f, 1.0f, 0.0f, world_coord.y),
106 Vec4f(0.0f, 0.0f, 1.0f, world_coord.z),
107 Vec4f(0.0f, 0.0f, 0.0f, 1.0f));
108
109 Mat4f cam_world_coord_mat = rotation_tran_mat.inverse() * world_coord_mat;
110
111 cam_world_coord.x = cam_world_coord_mat(0, 3);
112 cam_world_coord.y = cam_world_coord_mat(1, 3);
113 cam_world_coord.z = cam_world_coord_mat(2, 3);
114 }
115
116 Mat4f
generate_rotation_matrix(float roll,float pitch,float yaw)117 SurViewFisheyeDewarp::generate_rotation_matrix(float roll, float pitch, float yaw)
118 {
119 Mat4f matrix_x(Vec4f(1.0f, 0.0f, 0.0f, 0.0f),
120 Vec4f(0.0f, cos(roll), -sin(roll), 0.0f),
121 Vec4f(0.0f, sin(roll), cos(roll), 0.0f),
122 Vec4f(0.0f, 0.0f, 0.0f, 1.0f));
123
124 Mat4f matrix_y(Vec4f(cos(pitch), 0.0f, sin(pitch), 0.0f),
125 Vec4f(0.0f, 1.0f, 0.0f, 0.0f),
126 Vec4f(-sin(pitch), 0.0f, cos(pitch), 0.0f),
127 Vec4f(0.0f, 0.0f, 0.0f, 1.0f));
128
129 Mat4f matrix_z(Vec4f(cos(yaw), -sin(yaw), 0.0f, 0.0f),
130 Vec4f(sin(yaw), cos(yaw), 0.0f, 0.0f),
131 Vec4f(0.0f, 0.0f, 1.0f, 0.0f),
132 Vec4f(0.0f, 0.0f, 0.0f, 1.0f));
133
134 return matrix_z * matrix_y * matrix_x;
135 }
136
137 void
world_coord2cam(const PointFloat3 & cam_world_coord,PointFloat3 & cam_coord)138 SurViewFisheyeDewarp::world_coord2cam(const PointFloat3 &cam_world_coord, PointFloat3 &cam_coord)
139 {
140 cam_coord.x = -cam_world_coord.y;
141 cam_coord.y = -cam_world_coord.z;
142 cam_coord.z = -cam_world_coord.x;
143 }
144
145 void
cal_image_coord(const PointFloat3 & cam_coord,PointFloat2 & image_coord)146 SurViewFisheyeDewarp::cal_image_coord(const PointFloat3 &cam_coord, PointFloat2 &image_coord)
147 {
148 image_coord.x = cam_coord.x;
149 image_coord.y = cam_coord.y;
150 }
151
152 void
cal_image_coord(const PointFloat3 & cam_coord,PointFloat2 & image_coord)153 PolyFisheyeDewarp::cal_image_coord(const PointFloat3 &cam_coord, PointFloat2 &image_coord)
154 {
155 float dist2center = sqrt(cam_coord.x * cam_coord.x + cam_coord.y * cam_coord.y);
156 float angle = atan(cam_coord.z / dist2center);
157
158 float p = 1;
159 float poly_sum = 0;
160
161 IntrinsicParameter intrinsic_param = get_intrinsic_param();
162
163 if (dist2center != 0) {
164 for (uint32_t i = 0; i < intrinsic_param.poly_length; i++) {
165 poly_sum += intrinsic_param.poly_coeff[i] * p;
166 p = p * angle;
167 }
168
169 float image_x = cam_coord.x * poly_sum / dist2center;
170 float image_y = cam_coord.y * poly_sum / dist2center;
171
172 image_coord.x = image_x * intrinsic_param.c + image_y * intrinsic_param.d + intrinsic_param.xc;
173 image_coord.y = image_x * intrinsic_param.e + image_y + intrinsic_param.yc;
174 } else {
175 image_coord.x = intrinsic_param.xc;
176 image_coord.y = intrinsic_param.yc;
177 }
178 } // Adopt Scaramuzza's approach to calculate image coordinates from camera coordinates
179
180 }
181