1 /* 2 * xcam_utils.h - xcam utilities 3 * 4 * Copyright (c) 2014-2015 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: Wind Yuan <feng.yuan (at) intel.com> 19 * Author: Zong Wei <wei.zong (at) intel.com> 20 * Author: Junkai Wu <junkai.wu (at) intel.com> 21 * Author: Yinhang Liu <yinhangx.liu (at) intel.com> 22 */ 23 24 #include "xcam_utils.h" 25 #include "video_buffer.h" 26 #include "image_file_handle.h" 27 28 namespace XCam { 29 30 static float 31 transform_bowl_coord_to_image_x ( 32 const float bowl_x, const float bowl_y, 33 const uint32_t img_width) 34 { 35 float offset_radian = (bowl_x < 0.0f) ? PI : ((bowl_y >= 0.0f) ? 2.0f * PI : 0.0f); 36 float arctan_radian = (bowl_x != 0.0f) ? atan (-bowl_y / bowl_x) : ((bowl_y >= 0.0f) ? -PI / 2.0f : PI / 2.0f); 37 38 float img_x = arctan_radian + offset_radian; 39 img_x *= img_width / (2.0f * PI); 40 return XCAM_CLAMP (img_x, 0.0f, img_width - 1.0f); 41 } 42 43 static float 44 transform_bowl_coord_to_image_y ( 45 const BowlDataConfig &config, 46 const float bowl_x, const float bowl_y, const float bowl_z, 47 const uint32_t img_height) 48 { 49 float wall_image_height = config.wall_height / (config.wall_height + config.ground_length) * img_height; 50 float ground_image_height = img_height - wall_image_height; 51 float img_y = 0.0f; 52 53 if (bowl_z > 0.0f) { 54 img_y = (config.wall_height - bowl_z) * wall_image_height / config.wall_height; 55 img_y = XCAM_CLAMP (img_y, 0.0f, wall_image_height - 1.0f); 56 } else { 57 float max_semimajor = config.b * 58 sqrt (1 - config.center_z * config.center_z / (config.c * config.c)); 59 float min_semimajor = max_semimajor - config.ground_length; 60 XCAM_ASSERT (min_semimajor >= 0); 61 XCAM_ASSERT (max_semimajor > min_semimajor); 62 float step = ground_image_height / (max_semimajor - min_semimajor); 63 64 float axis_ratio = config.a / config.b; 65 float cur_semimajor = sqrt (bowl_x * bowl_x + bowl_y * bowl_y * axis_ratio * axis_ratio) / axis_ratio; 66 cur_semimajor = XCAM_CLAMP (cur_semimajor, min_semimajor, max_semimajor); 67 68 img_y = (max_semimajor - cur_semimajor) * step + wall_image_height; 69 img_y = XCAM_CLAMP (img_y, wall_image_height, img_height - 1.0f); 70 } 71 return img_y; 72 } 73 74 PointFloat2 bowl_view_coords_to_image ( 75 const BowlDataConfig &config, 76 const PointFloat3 &bowl_pos, 77 const uint32_t img_width, const uint32_t img_height) 78 { 79 PointFloat2 img_pos; 80 img_pos.x = transform_bowl_coord_to_image_x (bowl_pos.x, bowl_pos.y, img_width); 81 img_pos.y = transform_bowl_coord_to_image_y (config, bowl_pos.x, bowl_pos.y, bowl_pos.z, img_height); 82 83 return img_pos; 84 } 85 86 PointFloat3 bowl_view_image_to_world ( 87 const BowlDataConfig &config, 88 const uint32_t img_width, const uint32_t img_height, 89 const PointFloat2 &img_pos) 90 { 91 PointFloat3 world; 92 float angle; 93 94 float a = config.a; 95 float b = config.b; 96 float c = config.c; 97 98 float wall_image_height = config.wall_height / (float)(config.wall_height + config.ground_length) * (float)img_height; 99 float ground_image_height = (float)img_height - wall_image_height; 100 101 float z_step = (float)config.wall_height / wall_image_height; 102 float angle_step = fabs(config.angle_end - config.angle_start) / img_width; 103 104 if(img_pos.y < wall_image_height) { 105 world.z = config.wall_height - img_pos.y * z_step; // TODO world.z 106 angle = degree2radian (config.angle_start + img_pos.x * angle_step); 107 float r2 = 1 - (world.z - config.center_z) * (world.z - config.center_z) / (c * c); 108 109 if(XCAM_DOUBLE_EQUAL_AROUND (angle, PI / 2)) { 110 world.x = 0.0f; 111 world.y = -sqrt(r2 * b * b); 112 } else if (XCAM_DOUBLE_EQUAL_AROUND (angle, PI * 3 / 2)) { 113 world.x = 0.0f; 114 world.y = sqrt(r2 * b * b); 115 } else if((angle < PI / 2) || (angle > PI * 3 / 2)) { 116 world.x = sqrt(r2 * a * a * b * b / (b * b + a * a * tan(angle) * tan(angle))); 117 world.y = -world.x * tan(angle); 118 } else { 119 world.x = -sqrt(r2 * a * a * b * b / (b * b + a * a * tan(angle) * tan(angle))); 120 world.y = -world.x * tan(angle); 121 } 122 } else { 123 a = a * sqrt(1 - config.center_z * config.center_z / (c * c)); 124 b = b * sqrt(1 - config.center_z * config.center_z / (c * c)); 125 126 float ratio_ab = b / a; 127 128 float step_b = config.ground_length / ground_image_height; 129 130 b = b - (img_pos.y - wall_image_height) * step_b; 131 a = b / ratio_ab; 132 133 angle = degree2radian (config.angle_start + img_pos.x * angle_step); 134 135 if(XCAM_DOUBLE_EQUAL_AROUND (angle, PI / 2)) { 136 world.x = 0.0f; 137 world.y = -b; 138 } else if (XCAM_DOUBLE_EQUAL_AROUND (angle, PI * 3 / 2)) { 139 world.x = 0.0f; 140 world.y = b; 141 } else if((angle < PI / 2) || (angle > PI * 3 / 2)) { 142 world.x = a * b / sqrt(b * b + a * a * tan(angle) * tan(angle)); 143 world.y = -world.x * tan(angle); 144 } else { 145 world.x = -a * b / sqrt(b * b + a * a * tan(angle) * tan(angle)); 146 world.y = -world.x * tan(angle); 147 } 148 world.z = 0.0f; 149 } 150 151 return world; 152 } 153 154 void centralize_bowl_coord_from_cameras ( 155 ExtrinsicParameter &front_cam, ExtrinsicParameter &right_cam, 156 ExtrinsicParameter &rear_cam, ExtrinsicParameter &left_cam, 157 PointFloat3 &bowl_coord_offset) 158 { 159 bowl_coord_offset.x = (front_cam.trans_x + rear_cam.trans_x) / 2.0f; 160 bowl_coord_offset.y = (right_cam.trans_y + left_cam.trans_y) / 2.0f; 161 bowl_coord_offset.z = 0.0f; 162 163 front_cam.trans_x -= bowl_coord_offset.x; 164 front_cam.trans_y -= bowl_coord_offset.y; 165 166 right_cam.trans_x -= bowl_coord_offset.x; 167 right_cam.trans_y -= bowl_coord_offset.y; 168 169 rear_cam.trans_x -= bowl_coord_offset.x; 170 rear_cam.trans_y -= bowl_coord_offset.y; 171 172 left_cam.trans_x -= bowl_coord_offset.x; 173 left_cam.trans_y -= bowl_coord_offset.y; 174 } 175 176 double 177 linear_interpolate_p2 ( 178 double value_start, double value_end, 179 double ref_start, double ref_end, 180 double ref_curr) 181 { 182 double weight_start = 0; 183 double weight_end = 0; 184 double dist_start = 0; 185 double dist_end = 0; 186 double dist_sum = 0; 187 double value = 0; 188 189 dist_start = abs(ref_curr - ref_start); 190 dist_end = abs(ref_end - ref_curr); 191 dist_sum = dist_start + dist_end; 192 193 if (dist_start == 0) { 194 weight_start = 10000000.0; 195 } else { 196 weight_start = ((double)dist_sum / dist_start); 197 } 198 199 if (dist_end == 0) { 200 weight_end = 10000000.0; 201 } else { 202 weight_end = ((double)dist_sum / dist_end); 203 } 204 205 value = (value_start * weight_start + value_end * weight_end) / (weight_start + weight_end); 206 return value; 207 } 208 209 double 210 linear_interpolate_p4( 211 double value_lt, double value_rt, 212 double value_lb, double value_rb, 213 double ref_lt_x, double ref_rt_x, 214 double ref_lb_x, double ref_rb_x, 215 double ref_lt_y, double ref_rt_y, 216 double ref_lb_y, double ref_rb_y, 217 double ref_curr_x, double ref_curr_y) 218 { 219 double weight_lt = 0; 220 double weight_rt = 0; 221 double weight_lb = 0; 222 double weight_rb = 0; 223 double dist_lt = 0; 224 double dist_rt = 0; 225 double dist_lb = 0; 226 double dist_rb = 0; 227 double dist_sum = 0; 228 double value = 0; 229 230 dist_lt = (double)abs(ref_curr_x - ref_lt_x) + (double)abs(ref_curr_y - ref_lt_y); 231 dist_rt = (double)abs(ref_curr_x - ref_rt_x) + (double)abs(ref_curr_y - ref_rt_y); 232 dist_lb = (double)abs(ref_curr_x - ref_lb_x) + (double)abs(ref_curr_y - ref_lb_y); 233 dist_rb = (double)abs(ref_curr_x - ref_rb_x) + (double)abs(ref_curr_y - ref_rb_y); 234 dist_sum = dist_lt + dist_rt + dist_lb + dist_rb; 235 236 if (dist_lt == 0) { 237 weight_lt = 10000000.0; 238 } else { 239 weight_lt = ((float)dist_sum / dist_lt); 240 } 241 if (dist_rt == 0) { 242 weight_rt = 10000000.0; 243 } else { 244 weight_rt = ((float)dist_sum / dist_rt); 245 } 246 if (dist_lb == 0) { 247 weight_lb = 10000000.0; 248 } else { 249 weight_lb = ((float)dist_sum / dist_lb); 250 } 251 if (dist_rb == 0) { 252 weight_rb = 10000000.0; 253 } else { 254 weight_rb = ((float)dist_sum / dist_rt); 255 } 256 257 value = (double)floor ( (value_lt * weight_lt + value_rt * weight_rt + 258 value_lb * weight_lb + value_rb * weight_rb) / 259 (weight_lt + weight_rt + weight_lb + weight_rb) + 0.5 ); 260 return value; 261 } 262 263 void 264 get_gauss_table (uint32_t radius, float sigma, std::vector<float> &table, bool normalize) 265 { 266 uint32_t i; 267 uint32_t scale = radius * 2 + 1; 268 float dis = 0.0f, sum = 1.0f; 269 270 //XCAM_ASSERT (scale < 512); 271 table.resize (scale); 272 table[radius] = 1.0f; 273 274 for (i = 0; i < radius; i++) { 275 dis = ((float)i - radius) * ((float)i - radius); 276 table[i] = table[scale - i - 1] = exp(-dis / (2.0f * sigma * sigma)); 277 sum += table[i] * 2.0f; 278 } 279 280 if (!normalize) 281 return; 282 283 for(i = 0; i < scale; i++) 284 table[i] /= sum; 285 } 286 287 void 288 dump_buf_perfix_path (const SmartPtr<VideoBuffer> buf, const char *prefix_name) 289 { 290 char file_name[256]; 291 XCAM_ASSERT (prefix_name); 292 XCAM_ASSERT (buf.ptr ()); 293 294 const VideoBufferInfo &info = buf->get_video_info (); 295 snprintf ( 296 file_name, 256, "%s-%dx%d.%s", 297 prefix_name, info.width, info.height, xcam_fourcc_to_string (info.format)); 298 299 dump_video_buf (buf, file_name); 300 } 301 302 bool 303 dump_video_buf (const SmartPtr<VideoBuffer> buf, const char *file_name) 304 { 305 ImageFileHandle file; 306 XCAM_ASSERT (file_name); 307 308 XCamReturn ret = file.open (file_name, "wb"); 309 XCAM_FAIL_RETURN ( 310 ERROR, xcam_ret_is_ok (ret), false, 311 "dump buffer failed when open file: %s", file_name); 312 313 ret = file.write_buf (buf); 314 XCAM_FAIL_RETURN ( 315 ERROR, xcam_ret_is_ok (ret), false, 316 "dump buffer to file: %s failed", file_name); 317 318 return true; 319 } 320 321 } 322
