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
24namespace XCam {
25
26SurViewFisheyeDewarp::SurViewFisheyeDewarp ()
27{
28}
29SurViewFisheyeDewarp::~SurViewFisheyeDewarp ()
30{
31}
32
33PolyFisheyeDewarp::PolyFisheyeDewarp()
34    : SurViewFisheyeDewarp()
35{
36}
37
38void
39SurViewFisheyeDewarp::set_intrinsic_param(const IntrinsicParameter &intrinsic_param)
40{
41    _intrinsic_param = intrinsic_param;
42}
43
44void
45SurViewFisheyeDewarp::set_extrinsic_param(const ExtrinsicParameter &extrinsic_param)
46{
47    _extrinsic_param = extrinsic_param;
48}
49
50IntrinsicParameter
51SurViewFisheyeDewarp::get_intrinsic_param()
52{
53    return _intrinsic_param;
54}
55
56ExtrinsicParameter
57SurViewFisheyeDewarp::get_extrinsic_param()
58{
59    return _extrinsic_param;
60}
61
62void
63SurViewFisheyeDewarp::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
93void
94SurViewFisheyeDewarp::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
116Mat4f
117SurViewFisheyeDewarp::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
137void
138SurViewFisheyeDewarp::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
145void
146SurViewFisheyeDewarp::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
152void
153PolyFisheyeDewarp::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