1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
|
/*
* 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 "surview_fisheye_dewarp.h"
#include "xcam_utils.h"
namespace XCam {
SurViewFisheyeDewarp::SurViewFisheyeDewarp ()
{
}
SurViewFisheyeDewarp::~SurViewFisheyeDewarp ()
{
}
PolyFisheyeDewarp::PolyFisheyeDewarp()
: SurViewFisheyeDewarp()
{
}
void
SurViewFisheyeDewarp::set_intrinsic_param(const IntrinsicParameter &intrinsic_param)
{
_intrinsic_param = intrinsic_param;
}
void
SurViewFisheyeDewarp::set_extrinsic_param(const ExtrinsicParameter &extrinsic_param)
{
_extrinsic_param = extrinsic_param;
}
IntrinsicParameter
SurViewFisheyeDewarp::get_intrinsic_param()
{
return _intrinsic_param;
}
ExtrinsicParameter
SurViewFisheyeDewarp::get_extrinsic_param()
{
return _extrinsic_param;
}
void
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)
{
PointFloat3 world_coord;
PointFloat3 cam_coord;
PointFloat3 cam_world_coord;
PointFloat2 image_coord;
XCAM_LOG_DEBUG ("fisheye-dewarp:\n table(%dx%d) out_size(%dx%d) "
"bowl(start:%.1f, end:%.1f, ground:%.2f, wall:%.2f, a:%.2f, b:%.2f, c:%.2f, center_z:%.2f)",
table_w, table_h, image_w, image_h,
bowl_config.angle_start, bowl_config.angle_end,
bowl_config.ground_length, bowl_config.wall_height,
bowl_config.a, bowl_config.b, bowl_config.c, bowl_config.center_z);
float scale_factor_w = (float)image_w / table_w;
float scale_factor_h = (float)image_h / table_h;
for(uint32_t row = 0; row < table_h; row++) {
for(uint32_t col = 0; col < table_w; col++) {
PointFloat2 out_pos (col * scale_factor_w, row * scale_factor_h);
world_coord = bowl_view_image_to_world (bowl_config, image_w, image_h, out_pos);
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 + col] = image_coord;
}
}
}
void
SurViewFisheyeDewarp::cal_cam_world_coord(const PointFloat3 &world_coord, PointFloat3 &cam_world_coord)
{
Mat4f rotation_mat = generate_rotation_matrix( degree2radian (_extrinsic_param.roll),
degree2radian (_extrinsic_param.pitch),
degree2radian (_extrinsic_param.yaw));
Mat4f 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;
Mat4f world_coord_mat(Vec4f(1.0f, 0.0f, 0.0f, world_coord.x),
Vec4f(0.0f, 1.0f, 0.0f, world_coord.y),
Vec4f(0.0f, 0.0f, 1.0f, world_coord.z),
Vec4f(0.0f, 0.0f, 0.0f, 1.0f));
Mat4f cam_world_coord_mat = rotation_tran_mat.inverse() * world_coord_mat;
cam_world_coord.x = cam_world_coord_mat(0, 3);
cam_world_coord.y = cam_world_coord_mat(1, 3);
cam_world_coord.z = cam_world_coord_mat(2, 3);
}
Mat4f
SurViewFisheyeDewarp::generate_rotation_matrix(float roll, float pitch, float yaw)
{
Mat4f matrix_x(Vec4f(1.0f, 0.0f, 0.0f, 0.0f),
Vec4f(0.0f, cos(roll), -sin(roll), 0.0f),
Vec4f(0.0f, sin(roll), cos(roll), 0.0f),
Vec4f(0.0f, 0.0f, 0.0f, 1.0f));
Mat4f matrix_y(Vec4f(cos(pitch), 0.0f, sin(pitch), 0.0f),
Vec4f(0.0f, 1.0f, 0.0f, 0.0f),
Vec4f(-sin(pitch), 0.0f, cos(pitch), 0.0f),
Vec4f(0.0f, 0.0f, 0.0f, 1.0f));
Mat4f matrix_z(Vec4f(cos(yaw), -sin(yaw), 0.0f, 0.0f),
Vec4f(sin(yaw), cos(yaw), 0.0f, 0.0f),
Vec4f(0.0f, 0.0f, 1.0f, 0.0f),
Vec4f(0.0f, 0.0f, 0.0f, 1.0f));
return matrix_z * matrix_y * matrix_x;
}
void
SurViewFisheyeDewarp::world_coord2cam(const PointFloat3 &cam_world_coord, PointFloat3 &cam_coord)
{
cam_coord.x = -cam_world_coord.y;
cam_coord.y = -cam_world_coord.z;
cam_coord.z = -cam_world_coord.x;
}
void
SurViewFisheyeDewarp::cal_image_coord(const PointFloat3 &cam_coord, PointFloat2 &image_coord)
{
image_coord.x = cam_coord.x;
image_coord.y = cam_coord.y;
}
void
PolyFisheyeDewarp::cal_image_coord(const PointFloat3 &cam_coord, PointFloat2 &image_coord)
{
float dist2center = sqrt(cam_coord.x * cam_coord.x + cam_coord.y * cam_coord.y);
float angle = atan(cam_coord.z / 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.x * poly_sum / dist2center;
float image_y = cam_coord.y * poly_sum / dist2center;
image_coord.x = image_x * intrinsic_param.c + image_y * intrinsic_param.d + intrinsic_param.xc;
image_coord.y = image_x * intrinsic_param.e + image_y + intrinsic_param.yc;
} else {
image_coord.x = intrinsic_param.xc;
image_coord.y = intrinsic_param.yc;
}
} // Adopt Scaramuzza's approach to calculate image coordinates from camera coordinates
}
|
