| 95 | } |
| 96 | |
| 97 | bool rectifyFrameFast(const DecodedFrame& src, const UndistortMapFast& fast, DecodedFrame& out) { |
| 98 | if (!fast.valid() || !src.isValid()) { |
| 99 | return false; |
| 100 | } |
| 101 | const int ch = interleavedChannels(src.format); |
| 102 | if (ch == 0) { |
| 103 | return false; // planar / 16-bit -> caller keeps the original frame. |
| 104 | } |
| 105 | // The table indexes a source of fast.src_width x fast.src_height; a frame of a |
| 106 | // different size would read the wrong pixels (the caller rebuilds on size change). |
| 107 | if (src.width != fast.src_width || src.height != fast.src_height) { |
| 108 | return false; |
| 109 | } |
| 110 | |
| 111 | const std::vector<uint8_t>& sp = *src.pixels; |
| 112 | const auto src_stride = static_cast<size_t>(src.width) * static_cast<size_t>(ch); |
| 113 | |
| 114 | out.width = fast.out_width; |
| 115 | out.height = fast.out_height; |
| 116 | out.format = src.format; |
| 117 | out.pts = src.pts; |
| 118 | out.frame_id = src.frame_id; |
| 119 | const size_t out_bytes = |
| 120 | static_cast<size_t>(fast.out_width) * static_cast<size_t>(fast.out_height) * static_cast<size_t>(ch); |
| 121 | if (out.pixels == nullptr) { |
| 122 | out.pixels = std::make_shared<std::vector<uint8_t>>(out_bytes); |
| 123 | } else { |
| 124 | out.pixels->resize(out_bytes); // reused buffer: no per-frame allocation on the steady state. |
| 125 | } |
| 126 | std::vector<uint8_t>& op = *out.pixels; |
| 127 | |
| 128 | const size_t n = static_cast<size_t>(fast.out_width) * static_cast<size_t>(fast.out_height); |
| 129 | for (size_t i = 0; i < n; ++i) { |
| 130 | uint8_t* dst = &op[i * static_cast<size_t>(ch)]; |
| 131 | const int32_t p0 = fast.src_p0[i]; |
| 132 | if (p0 < 0) { |
| 133 | for (int c = 0; c < ch; ++c) { |
| 134 | dst[c] = 0; // out of bounds -> black (write explicitly; the buffer is reused). |
| 135 | } |
| 136 | continue; |
| 137 | } |
| 138 | const float ax = fast.frac_x[i]; |
| 139 | const float ay = fast.frac_y[i]; |
| 140 | const float w00 = (1.0F - ax) * (1.0F - ay); |
| 141 | const float w10 = ax * (1.0F - ay); |
| 142 | const float w01 = (1.0F - ax) * ay; |
| 143 | const float w11 = ax * ay; |
| 144 | const uint8_t* p00 = &sp[static_cast<size_t>(p0) * static_cast<size_t>(ch)]; |
| 145 | const uint8_t* p10 = p00 + ch; |
| 146 | const uint8_t* p01 = p00 + src_stride; |
| 147 | const uint8_t* p11 = p01 + ch; |
| 148 | for (int c = 0; c < ch; ++c) { |
| 149 | const float val = w00 * p00[c] + w10 * p10[c] + w01 * p01[c] + w11 * p11[c]; |
| 150 | dst[c] = static_cast<uint8_t>(val + 0.5F); |
| 151 | } |
| 152 | } |
| 153 | return true; |
| 154 | } |