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 (at) intel.com> 19 */ 20 21 #include "surview_fisheye_dewarp.h" 22 #include "xcam_utils.h" 23 24 namespace XCam { 25 26 SurViewFisheyeDewarp::SurViewFisheyeDewarp () 27 { 28 } 29 SurViewFisheyeDewarp::~SurViewFisheyeDewarp () 30 { 31 } 32 33 PolyFisheyeDewarp::PolyFisheyeDewarp() 34 : SurViewFisheyeDewarp() 35 { 36 } 37 38 void 39 SurViewFisheyeDewarp::set_intrinsic_param(const IntrinsicParameter &intrinsic_param) 40 { 41 _intrinsic_param = intrinsic_param; 42 } 43 44 void 45 SurViewFisheyeDewarp::set_extrinsic_param(const ExtrinsicParameter &extrinsic_param) 46 { 47 _extrinsic_param = extrinsic_param; 48 } 49 50 IntrinsicParameter 51 SurViewFisheyeDewarp::get_intrinsic_param() 52 { 53 return _intrinsic_param; 54 } 55 56 ExtrinsicParameter 57 SurViewFisheyeDewarp::get_extrinsic_param() 58 { 59 return _extrinsic_param; 60 } 61 62 void 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 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 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 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 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 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
