| 33 | } // namespace |
| 34 | |
| 35 | std::optional<DecodedFrame> rectifyFrame(const DecodedFrame& src, const UndistortMap& map) { |
| 36 | if (!map.valid() || !src.isValid()) { |
| 37 | return std::nullopt; |
| 38 | } |
| 39 | const int ch = interleavedChannels(src.format); |
| 40 | if (ch == 0) { |
| 41 | return std::nullopt; // unsupported format -> caller keeps original. |
| 42 | } |
| 43 | |
| 44 | const int sw = src.width; |
| 45 | const int sh = src.height; |
| 46 | const std::vector<uint8_t>& sp = *src.pixels; |
| 47 | const auto src_stride = static_cast<size_t>(sw) * static_cast<size_t>(ch); |
| 48 | |
| 49 | DecodedFrame out; |
| 50 | out.width = map.out_width; |
| 51 | out.height = map.out_height; |
| 52 | out.format = src.format; |
| 53 | out.pts = src.pts; |
| 54 | out.frame_id = src.frame_id; |
| 55 | out.pixels = std::make_shared<std::vector<uint8_t>>( |
| 56 | static_cast<size_t>(map.out_width) * static_cast<size_t>(map.out_height) * static_cast<size_t>(ch), 0); |
| 57 | std::vector<uint8_t>& op = *out.pixels; |
| 58 | const auto out_stride = static_cast<size_t>(map.out_width) * static_cast<size_t>(ch); |
| 59 | |
| 60 | for (int v = 0; v < map.out_height; ++v) { |
| 61 | for (int u = 0; u < map.out_width; ++u) { |
| 62 | const size_t midx = static_cast<size_t>(v) * static_cast<size_t>(map.out_width) + static_cast<size_t>(u); |
| 63 | const float fx = map.src_x[midx]; |
| 64 | const float fy = map.src_y[midx]; |
| 65 | |
| 66 | // Bilinear: the 2x2 neighborhood must be fully inside the source. |
| 67 | const float x0f = std::floor(fx); |
| 68 | const float y0f = std::floor(fy); |
| 69 | const int x0 = static_cast<int>(x0f); |
| 70 | const int y0 = static_cast<int>(y0f); |
| 71 | if (x0 < 0 || y0 < 0 || x0 + 1 >= sw || y0 + 1 >= sh) { |
| 72 | continue; // out of bounds -> leave black (buffer is zero-filled). |
| 73 | } |
| 74 | |
| 75 | const float ax = fx - x0f; |
| 76 | const float ay = fy - y0f; |
| 77 | const float w00 = (1.0F - ax) * (1.0F - ay); |
| 78 | const float w10 = ax * (1.0F - ay); |
| 79 | const float w01 = (1.0F - ax) * ay; |
| 80 | const float w11 = ax * ay; |
| 81 | |
| 82 | const uint8_t* p00 = |
| 83 | &sp[static_cast<size_t>(y0) * src_stride + static_cast<size_t>(x0) * static_cast<size_t>(ch)]; |
| 84 | const uint8_t* p10 = p00 + ch; |
| 85 | const uint8_t* p01 = p00 + src_stride; |
| 86 | const uint8_t* p11 = p01 + ch; |
| 87 | uint8_t* dst = &op[static_cast<size_t>(v) * out_stride + static_cast<size_t>(u) * static_cast<size_t>(ch)]; |
| 88 | for (int c = 0; c < ch; ++c) { |
| 89 | const float val = w00 * p00[c] + w10 * p10[c] + w01 * p01[c] + w11 * p11[c]; |
| 90 | dst[c] = static_cast<uint8_t>(val + 0.5F); |
| 91 | } |
| 92 | } |