/*
|
* 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.wall_height, bowl_config.ground_length,
|
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
|
|
}
|
