Haoxiang Li | fd727e2 | 2020-04-03 23:58:46 +0000 | [diff] [blame^] | 1 | #ifndef WIRELESS_ANDROID_AUTOMOTIVE_CAML_SURROUND_VIEW_CORE_LIB_H_ |
| 2 | #define WIRELESS_ANDROID_AUTOMOTIVE_CAML_SURROUND_VIEW_CORE_LIB_H_ |
| 3 | |
| 4 | #include <cstdint> |
| 5 | #include <vector> |
| 6 | |
| 7 | namespace android_auto { |
| 8 | namespace surround_view { |
| 9 | |
| 10 | // bounding box (bb) |
| 11 | // It is used to describe the car model bounding box in 3D. |
| 12 | // It assumes z = 0 and only x, y are used in the struct. |
| 13 | // Of course, it is compatible to the 2d version bounding box and may be used |
| 14 | // for other bounding box purpose (e.g., 2d bounding box in image). |
| 15 | struct BoundingBox { |
| 16 | // (x,y) is bounding box's top left corner coordinate. |
| 17 | float x; |
| 18 | float y; |
| 19 | |
| 20 | // (width, height) is the size of the bounding box. |
| 21 | float width; |
| 22 | float height; |
| 23 | |
| 24 | BoundingBox() : x(0.0f), y(0.0f), width(0.0f), height(0.0f) {} |
| 25 | |
| 26 | BoundingBox(float x_, float y_, float width_, float height_) |
| 27 | : x(x_), y(y_), width(width_), height(height_) {} |
| 28 | |
| 29 | BoundingBox(const BoundingBox& bb_) |
| 30 | : x(bb_.x), y(bb_.y), width(bb_.width), height(bb_.height) {} |
| 31 | |
| 32 | // Checks if data is valid. |
| 33 | bool IsValid() const { return width >= 0 && height >= 0; } |
| 34 | |
| 35 | bool operator==(const BoundingBox& rhs) const { |
| 36 | return x == rhs.x && y == rhs.y && width == rhs.width && |
| 37 | height == rhs.height; |
| 38 | } |
| 39 | |
| 40 | BoundingBox& operator=(const BoundingBox& rhs) { |
| 41 | x = rhs.x; |
| 42 | y = rhs.y; |
| 43 | width = rhs.width; |
| 44 | height = rhs.height; |
| 45 | return *this; |
| 46 | } |
| 47 | }; |
| 48 | |
| 49 | template <typename T> |
| 50 | struct Coordinate2dBase { |
| 51 | // x coordinate. |
| 52 | T x; |
| 53 | |
| 54 | // y coordinate. |
| 55 | T y; |
| 56 | |
| 57 | Coordinate2dBase() : x(0), y(0) {} |
| 58 | |
| 59 | Coordinate2dBase(T x_, T y_) : x(x_), y(y_) {} |
| 60 | |
| 61 | bool operator==(const Coordinate2dBase& rhs) const { |
| 62 | return x == rhs.x && y == rhs.y; |
| 63 | } |
| 64 | |
| 65 | Coordinate2dBase& operator=(const Coordinate2dBase& rhs) { |
| 66 | x = rhs.x; |
| 67 | y = rhs.y; |
| 68 | return *this; |
| 69 | } |
| 70 | }; |
| 71 | |
| 72 | // integer type size. |
| 73 | typedef Coordinate2dBase<int> Coordinate2dInteger; |
| 74 | |
| 75 | // float type size. |
| 76 | typedef Coordinate2dBase<float> Coordinate2dFloat; |
| 77 | |
| 78 | struct Coordinate3dFloat { |
| 79 | // x coordinate. |
| 80 | float x; |
| 81 | |
| 82 | // y coordinate. |
| 83 | float y; |
| 84 | |
| 85 | // z coordinate. |
| 86 | float z; |
| 87 | |
| 88 | Coordinate3dFloat() : x(0), y(0), z(0) {} |
| 89 | |
| 90 | Coordinate3dFloat(float x_, float y_, float z_) : x(x_), y(y_), z(z_) {} |
| 91 | |
| 92 | bool operator==(const Coordinate3dFloat& rhs) const { |
| 93 | return x == rhs.x && y == rhs.y; |
| 94 | } |
| 95 | |
| 96 | Coordinate3dFloat& operator=(const Coordinate3dFloat& rhs) { |
| 97 | x = rhs.x; |
| 98 | y = rhs.y; |
| 99 | return *this; |
| 100 | } |
| 101 | }; |
| 102 | |
| 103 | // pixel weight used for illumination assessment |
| 104 | struct PixelWeight { |
| 105 | // x and y are the coordinates (absolute value) in image space. |
| 106 | // pixel coordinate x in horizontal direction. |
| 107 | float x; |
| 108 | |
| 109 | // pixel coordinate y in vertical direction. |
| 110 | float y; |
| 111 | |
| 112 | // pixel weight, range in [0, 1]. |
| 113 | float weight; |
| 114 | |
| 115 | PixelWeight() : x(-1), y(-1), weight(0) {} |
| 116 | |
| 117 | PixelWeight(int x_, int y_, int weight_) : x(x_), y(y_), weight(weight_) {} |
| 118 | |
| 119 | bool operator==(const PixelWeight& rhs) const { |
| 120 | return x == rhs.x && y == rhs.y && weight == rhs.weight; |
| 121 | } |
| 122 | |
| 123 | PixelWeight& operator=(const PixelWeight& rhs) { |
| 124 | x = rhs.x; |
| 125 | y = rhs.y; |
| 126 | weight = rhs.weight; |
| 127 | return *this; |
| 128 | } |
| 129 | }; |
| 130 | |
| 131 | // base size 2d type template. |
| 132 | template <typename T> |
| 133 | struct Size2dBase { |
| 134 | // width of size. |
| 135 | T width; |
| 136 | |
| 137 | // height of size. |
| 138 | T height; |
| 139 | |
| 140 | Size2dBase() : width(0), height(0) {} |
| 141 | |
| 142 | Size2dBase(T width_, T height_) : width(width_), height(height_) {} |
| 143 | |
| 144 | bool IsValid() const { return width > 0 && height > 0; } |
| 145 | |
| 146 | bool operator==(const Size2dBase& rhs) const { |
| 147 | return width == rhs.width && height == rhs.height; |
| 148 | } |
| 149 | |
| 150 | Size2dBase& operator=(const Size2dBase& rhs) { |
| 151 | width = rhs.width; |
| 152 | height = rhs.height; |
| 153 | return *this; |
| 154 | } |
| 155 | }; |
| 156 | |
| 157 | // integer type size. |
| 158 | typedef Size2dBase<int> Size2dInteger; |
| 159 | |
| 160 | // float type size. |
| 161 | typedef Size2dBase<float> Size2dFloat; |
| 162 | |
| 163 | // surround view 2d parameters |
| 164 | struct SurroundView2dParams { |
| 165 | // surround view 2d image resolution (width, height). |
| 166 | Size2dInteger resolution; |
| 167 | |
| 168 | // the physical size of surround view 2d area in surround view coordinate. |
| 169 | // (surround view coordinate is defined as X rightward, Y forward and |
| 170 | // the origin lies on the center of the (symmetric) bowl (ground). |
| 171 | // When bowl is not used, surround view coordinate origin lies on the |
| 172 | // center of car model bounding box.) |
| 173 | // The unit should be consistent with camera extrinsics (translation). |
| 174 | Size2dFloat physical_size; |
| 175 | |
| 176 | // the center of surround view 2d area in surround view coordinate |
| 177 | // (consistent with extrinsics coordinate). |
| 178 | Coordinate2dFloat physical_center; |
| 179 | |
| 180 | SurroundView2dParams() |
| 181 | : resolution{0, 0}, |
| 182 | physical_size{0.0f, 0.0f}, |
| 183 | physical_center{0.0f, 0.0f} {} |
| 184 | |
| 185 | SurroundView2dParams(Size2dInteger resolution_, Size2dFloat physical_size_, |
| 186 | Coordinate2dFloat physical_center_) |
| 187 | : resolution(resolution_), |
| 188 | physical_size(physical_size_), |
| 189 | physical_center(physical_center_) {} |
| 190 | |
| 191 | // Checks if data is valid. |
| 192 | bool IsValid() const { |
| 193 | return resolution.IsValid() && physical_size.IsValid(); |
| 194 | } |
| 195 | |
| 196 | bool operator==(const SurroundView2dParams& rhs) const { |
| 197 | return resolution == rhs.resolution && physical_size == rhs.physical_size && |
| 198 | physical_center == rhs.physical_center; |
| 199 | } |
| 200 | |
| 201 | SurroundView2dParams& operator=(const SurroundView2dParams& rhs) { |
| 202 | resolution = rhs.resolution; |
| 203 | physical_size = rhs.physical_size; |
| 204 | physical_center = rhs.physical_center; |
| 205 | return *this; |
| 206 | } |
| 207 | }; |
| 208 | |
| 209 | // surround view 3d parameters |
| 210 | struct SurroundView3dParams { |
| 211 | // Bowl center is the origin of the surround view coordinate. If surround view |
| 212 | // coordinate is different from the global one, a coordinate system |
| 213 | // transformation function is required. |
| 214 | |
| 215 | // planar area radius. |
| 216 | // Range in (0, +Inf). |
| 217 | float plane_radius; |
| 218 | |
| 219 | // the number of divisions on the plane area of bowl, in the direction |
| 220 | // of the radius. |
| 221 | // Range in [1, +Inf). |
| 222 | int plane_divisions; |
| 223 | |
| 224 | // bowl curve curve height. |
| 225 | // Range in (0, +Inf). |
| 226 | float curve_height; |
| 227 | |
| 228 | // the number of points on bowl curve curve along radius direction. |
| 229 | // Range in [1, +Inf). |
| 230 | int curve_divisions; |
| 231 | |
| 232 | // the number of points along circle (360 degrees) |
| 233 | // Range in [1, +Inf). |
| 234 | int angular_divisions; |
| 235 | |
| 236 | // the parabola coefficient of bowl curve curve. |
| 237 | // The curve formula is z = a * (x^2 + y^2) for sqrt(x^2 + y^2) > |
| 238 | // plane_radius; a is curve_coefficient. |
| 239 | // Range in (0, +Inf). |
| 240 | float curve_coefficient; |
| 241 | |
| 242 | // render output image size. |
| 243 | Size2dInteger resolution; |
| 244 | |
| 245 | SurroundView3dParams() |
| 246 | : plane_radius(0.0f), |
| 247 | plane_divisions(0), |
| 248 | curve_height(0.0f), |
| 249 | curve_divisions(0), |
| 250 | angular_divisions(0), |
| 251 | curve_coefficient(0.0f), |
| 252 | resolution(0, 0) {} |
| 253 | |
| 254 | SurroundView3dParams(float plane_radius_, int plane_divisions_, |
| 255 | float curve_height_, int curve_divisions_, |
| 256 | int angular_divisions_, float curve_coefficient_, |
| 257 | Size2dInteger resolution_) |
| 258 | : plane_radius(plane_radius_), |
| 259 | plane_divisions(plane_divisions_), |
| 260 | curve_height(curve_height_), |
| 261 | curve_divisions(curve_divisions_), |
| 262 | angular_divisions(angular_divisions_), |
| 263 | curve_coefficient(curve_coefficient_), |
| 264 | resolution(resolution_) {} |
| 265 | |
| 266 | // Checks if data is valid. |
| 267 | bool IsValid() const { |
| 268 | return plane_radius > 0 && plane_divisions > 0 && curve_height > 0 && |
| 269 | angular_divisions > 0 && curve_coefficient > 0 && |
| 270 | curve_divisions > 0 && resolution.IsValid(); |
| 271 | } |
| 272 | |
| 273 | bool operator==(const SurroundView3dParams& rhs) const { |
| 274 | return plane_radius == rhs.plane_radius && |
| 275 | plane_divisions == rhs.plane_divisions && |
| 276 | curve_height == rhs.curve_height && |
| 277 | curve_divisions == rhs.curve_divisions && |
| 278 | angular_divisions == rhs.angular_divisions && |
| 279 | curve_coefficient == rhs.curve_coefficient && |
| 280 | resolution == rhs.resolution; |
| 281 | } |
| 282 | |
| 283 | SurroundView3dParams& operator=(const SurroundView3dParams& rhs) { |
| 284 | plane_radius = rhs.plane_radius; |
| 285 | plane_divisions = rhs.plane_divisions; |
| 286 | curve_height = rhs.curve_height; |
| 287 | curve_divisions = rhs.curve_divisions; |
| 288 | angular_divisions = rhs.angular_divisions; |
| 289 | curve_coefficient = rhs.curve_coefficient; |
| 290 | resolution = rhs.resolution; |
| 291 | return *this; |
| 292 | } |
| 293 | }; |
| 294 | |
| 295 | // surround view camera parameters with native types only. |
| 296 | struct SurroundViewCameraParams { |
| 297 | // All calibration data |intrinsics|, |rvec| and |tvec| |
| 298 | // follow OpenCV format excepting using native arrays, refer: |
| 299 | // https://docs.opencv.org/3.4.0/db/d58/group__calib3d__fisheye.html |
| 300 | // camera intrinsics. It is the 1d array of camera matrix(3X3) with row first. |
| 301 | float intrinsics[9]; |
| 302 | |
| 303 | // lens distortion parameters. |
| 304 | float distorion[4]; |
| 305 | |
| 306 | // rotation vector. |
| 307 | float rvec[3]; |
| 308 | |
| 309 | // translation vector. |
| 310 | float tvec[3]; |
| 311 | |
| 312 | // camera image size (width, height). |
| 313 | Size2dInteger size; |
| 314 | |
| 315 | // fisheye circular fov. |
| 316 | float circular_fov; |
| 317 | |
| 318 | bool operator==(const SurroundViewCameraParams& rhs) const { |
| 319 | return (0 == std::memcmp(intrinsics, rhs.intrinsics, 9 * sizeof(float))) && |
| 320 | (0 == std::memcmp(distorion, rhs.distorion, 4 * sizeof(float))) && |
| 321 | (0 == std::memcmp(rvec, rhs.rvec, 3 * sizeof(float))) && |
| 322 | (0 == std::memcmp(tvec, rhs.tvec, 3 * sizeof(float))) && |
| 323 | size == rhs.size && circular_fov == rhs.circular_fov; |
| 324 | } |
| 325 | |
| 326 | SurroundViewCameraParams& operator=(const SurroundViewCameraParams& rhs) { |
| 327 | std::memcpy(intrinsics, rhs.intrinsics, 9 * sizeof(float)); |
| 328 | std::memcpy(distorion, rhs.distorion, 4 * sizeof(float)); |
| 329 | std::memcpy(rvec, rhs.rvec, 3 * sizeof(float)); |
| 330 | std::memcpy(tvec, rhs.tvec, 3 * sizeof(float)); |
| 331 | size = rhs.size; |
| 332 | circular_fov = rhs.circular_fov; |
| 333 | return *this; |
| 334 | } |
| 335 | }; |
| 336 | |
| 337 | // 3D vertex of an overlay object. |
| 338 | struct OverlayVertex { |
| 339 | // Position in 3d coordinates in world space in order X,Y,Z. |
| 340 | float pos[3]; |
| 341 | // RGBA values, A is used for transparency. |
| 342 | uint8_t rgba[4]; |
| 343 | |
| 344 | // normalized texture coordinates, in width and height direction. Range [0, |
| 345 | // 1]. |
| 346 | float tex[2]; |
| 347 | |
| 348 | // normalized vertex normal. |
| 349 | float nor[3]; |
| 350 | |
| 351 | bool operator==(const OverlayVertex& rhs) const { |
| 352 | return (0 == std::memcmp(pos, rhs.pos, 3 * sizeof(float))) && |
| 353 | (0 == std::memcmp(rgba, rhs.rgba, 4 * sizeof(uint8_t))) && |
| 354 | (0 == std::memcmp(tex, rhs.tex, 2 * sizeof(float))) && |
| 355 | (0 == std::memcmp(nor, rhs.nor, 3 * sizeof(float))); |
| 356 | } |
| 357 | |
| 358 | OverlayVertex& operator=(const OverlayVertex& rhs) { |
| 359 | std::memcpy(pos, rhs.pos, 3 * sizeof(float)); |
| 360 | std::memcpy(rgba, rhs.rgba, 4 * sizeof(uint8_t)); |
| 361 | std::memcpy(tex, rhs.tex, 2 * sizeof(float)); |
| 362 | std::memcpy(nor, rhs.nor, 3 * sizeof(float)); |
| 363 | return *this; |
| 364 | } |
| 365 | }; |
| 366 | |
| 367 | // Overlay is a list of vertices (may be a single or multiple objects in scene) |
| 368 | // coming from a single source or type of sensor. |
| 369 | struct Overlay { |
| 370 | // Uniqiue Id identifying each overlay. |
| 371 | uint16_t id; |
| 372 | |
| 373 | // List of overlay vertices. 3 consecutive vertices form a triangle. |
| 374 | std::vector<OverlayVertex> vertices; |
| 375 | |
| 376 | // Constructor initializing all member. |
| 377 | Overlay(uint16_t id_, const std::vector<OverlayVertex>& vertices_) { |
| 378 | id = id_; |
| 379 | vertices = vertices_; |
| 380 | } |
| 381 | |
| 382 | // Default constructor. |
| 383 | Overlay() { |
| 384 | id = 0; |
| 385 | vertices = std::vector<OverlayVertex>(); |
| 386 | } |
| 387 | }; |
| 388 | |
| 389 | enum Format { |
| 390 | GRAY = 0, |
| 391 | RGB = 1, |
| 392 | RGBA = 2, |
| 393 | }; |
| 394 | |
| 395 | struct SurroundViewInputBufferPointers { |
| 396 | void* gpu_data_pointer; |
| 397 | void* cpu_data_pointer; |
| 398 | Format format; |
| 399 | int width; |
| 400 | int height; |
| 401 | SurroundViewInputBufferPointers() |
| 402 | : gpu_data_pointer(nullptr), |
| 403 | cpu_data_pointer(nullptr), |
| 404 | width(0), |
| 405 | height(0) {} |
| 406 | SurroundViewInputBufferPointers(void* gpu_data_pointer_, |
| 407 | void* cpu_data_pointer_, Format format_, |
| 408 | int width_, int height_) |
| 409 | : gpu_data_pointer(gpu_data_pointer_), |
| 410 | cpu_data_pointer(cpu_data_pointer_), |
| 411 | format(format_), |
| 412 | width(width_), |
| 413 | height(height_) {} |
| 414 | }; |
| 415 | |
| 416 | struct SurroundViewResultPointer { |
| 417 | void* data_pointer; |
| 418 | Format format; |
| 419 | int width; |
| 420 | int height; |
| 421 | SurroundViewResultPointer() : data_pointer(nullptr), width(0), height(0) {} |
| 422 | SurroundViewResultPointer(Format format_, int width_, int height_) |
| 423 | : format(format_), width(width_), height(height_) { |
| 424 | // default formate is gray. |
| 425 | const int byte_per_pixel = format_ == RGB ? 3 : format_ == RGBA ? 4 : 1; |
| 426 | data_pointer = |
| 427 | static_cast<void*>(new char[width * height * byte_per_pixel]); |
| 428 | } |
| 429 | ~SurroundViewResultPointer() { |
| 430 | if (data_pointer) { |
| 431 | // delete[] static_cast<char*>(data_pointer); |
| 432 | data_pointer = nullptr; |
| 433 | } |
| 434 | } |
| 435 | }; |
| 436 | |
| 437 | class SurroundView { |
| 438 | public: |
| 439 | virtual ~SurroundView() = default; |
| 440 | |
| 441 | // Sets SurroundView static data. |
| 442 | // For each input, please refer to the definition. |
| 443 | virtual bool SetStaticData( |
| 444 | const std::vector<SurroundViewCameraParams>& cameras_params, |
| 445 | const SurroundView2dParams& surround_view_2d_params, |
| 446 | const SurroundView3dParams& surround_view_3d_params, |
| 447 | const std::vector<float>& undistortion_focal_length_scales, |
| 448 | const BoundingBox& car_model_bb) = 0; |
| 449 | |
| 450 | // Starts 2d pipeline. Returns false if error occurs. |
| 451 | virtual bool Start2dPipeline() = 0; |
| 452 | |
| 453 | // Starts 3d pipeline. Returns false if error occurs. |
| 454 | virtual bool Start3dPipeline() = 0; |
| 455 | |
| 456 | // Stops 2d pipleline. It releases resource owned by the pipeline. |
| 457 | // Returns false if error occurs. |
| 458 | virtual void Stop2dPipeline() = 0; |
| 459 | |
| 460 | // Stops 3d pipeline. It releases resource owned by the pipeline. |
| 461 | virtual void Stop3dPipeline() = 0; |
| 462 | |
| 463 | // Updates 2d output resolution on-the-fly. Starts2dPipeline() must be called |
| 464 | // before this can be called. For quality assurance, the resolution should not |
| 465 | // be larger than the original one. This call is not thread safe and there is |
| 466 | // no sync between Get2dSurroundView() and this call. |
| 467 | virtual bool Update2dOutputResolution(const Size2dInteger& resolution) = 0; |
| 468 | |
| 469 | // Updates 3d output resolution on-the-fly. Starts3dPipeline() must be called |
| 470 | // before this can be called. For quality assurance, the resolution should not |
| 471 | // be larger than the original one. This call is not thread safe and there is |
| 472 | // no sync between Get3dSurroundView() and this call. |
| 473 | virtual bool Update3dOutputResolution(const Size2dInteger& resolution) = 0; |
| 474 | |
| 475 | // Projects camera's pixel location to surround view 2d image location. |
| 476 | // camera_point is the pixel location in raw camera's space. |
| 477 | // camera_index is the camera's index. |
| 478 | // surround_view_2d_point is the surround view 2d image pixel location. |
| 479 | virtual bool GetProjectionPointFromRawCameraToSurroundView2d( |
| 480 | const Coordinate2dInteger& camera_point, int camera_index, |
| 481 | Coordinate2dFloat* surround_view_2d_point) = 0; |
| 482 | |
| 483 | // Projects camera's pixel location to surround view 3d bowl coordinate. |
| 484 | // camera_point is the pixel location in raw camera's space. |
| 485 | // camera_index is the camera's index. |
| 486 | // surround_view_3d_point is the surround view 3d vertex. |
| 487 | virtual bool GetProjectionPointFromRawCameraToSurroundView3d( |
| 488 | const Coordinate2dInteger& camera_point, int camera_index, |
| 489 | Coordinate3dFloat* surround_view_3d_point) = 0; |
| 490 | |
| 491 | // Gets 2d surround view image. |
| 492 | // It takes input_pointers as input, and output is result_pointer. |
| 493 | // Please refer to the definition of SurroundViewInputBufferPointers and |
| 494 | // SurroundViewResultPointer. |
| 495 | virtual bool Get2dSurroundView( |
| 496 | const std::vector<SurroundViewInputBufferPointers>& input_pointers, |
| 497 | SurroundViewResultPointer* result_pointer) = 0; |
| 498 | |
| 499 | // Gets 3d surround view image. |
| 500 | // It takes input_pointers and view_matrix as input, and output is |
| 501 | // result_pointer. view_matrix is 4 x 4 matrix. |
| 502 | // Please refer to the definition of |
| 503 | // SurroundViewInputBufferPointers and |
| 504 | // SurroundViewResultPointer. |
| 505 | virtual bool Get3dSurroundView( |
| 506 | const std::vector<SurroundViewInputBufferPointers>& input_pointers, |
| 507 | const std::vector<std::vector<float>> view_matrix, |
| 508 | SurroundViewResultPointer* result_pointer) = 0; |
| 509 | |
| 510 | // Sets 3d overlays. |
| 511 | virtual bool Set3dOverlay(const std::vector<Overlay>& overlays) = 0; |
| 512 | |
| 513 | // for test only. |
| 514 | // TODO(xxqian): remove thest two fns. |
| 515 | virtual std::vector<SurroundViewInputBufferPointers> ReadImages( |
| 516 | const char* filename0, const char* filename1, const char* filename2, |
| 517 | const char* filename3) = 0; |
| 518 | |
| 519 | virtual void WriteImage(const SurroundViewResultPointer result_pointerer, |
| 520 | const char* filename) = 0; |
| 521 | }; |
| 522 | |
| 523 | SurroundView* Create(); |
| 524 | |
| 525 | } // namespace surround_view |
| 526 | } // namespace android_auto |
| 527 | |
| 528 | #endif // WIRELESS_ANDROID_AUTOMOTIVE_CAML_SURROUND_VIEW_CORE_LIB_H_ |