| 1 |
/* +------------------------------------------------------------------------+ |
| 2 |
| Mobile Robot Programming Toolkit (MRPT) | |
| 3 |
| https://www.mrpt.org/ | |
| 4 |
| | |
| 5 |
| Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | |
| 6 |
| See: https://www.mrpt.org/Authors - All rights reserved. | |
| 7 |
| Released under BSD License. See: https://www.mrpt.org/License | |
| 8 |
+------------------------------------------------------------------------+ */ |
| 9 |
|
| 10 |
#include "img-precomp.h" // Precompiled headers |
| 11 |
// |
| 12 |
#include <mrpt/config.h> |
| 13 |
|
| 14 |
// --------------------------------------------------------------------------- |
| 15 |
// This file contains the SSE3/SSSE3 optimized functions for |
| 16 |
// mrpt::img::CImage |
| 17 |
// See the sources and the doxygen documentation page "sse_optimizations" for |
| 18 |
// more details. |
| 19 |
// --------------------------------------------------------------------------- |
| 20 |
#if MRPT_ARCH_INTEL_COMPATIBLE |
| 21 |
|
| 22 |
#include <mrpt/core/SSE_macros.h> |
| 23 |
#include <mrpt/core/SSE_types.h> |
| 24 |
#include <mrpt/img/CImage.h> |
| 25 |
#include <mrpt/system/memory.h> |
| 26 |
|
| 27 |
#include "CImage.SSEx.h" |
| 28 |
|
| 29 |
/** \addtogroup sse_optimizations |
| 30 |
* SSE optimized functions |
| 31 |
* @{ |
| 32 |
*/ |
| 33 |
|
| 34 |
// This is the actual function behind image_SSSE3_scale_half_3c8u(): |
| 35 |
template <bool MemIsAligned> |
| 36 |
void impl_image_SSSE3_scale_half_3c8u( |
| 37 |
const uint8_t* in, uint8_t* out, int w, int h, size_t step_in, size_t step_out) |
| 38 |
{ |
| 39 |
SSE_DISABLE_WARNINGS |
| 40 |
// clang-format off |
| 41 |
|
| 42 |
const __m128i m0 = _mm_set_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x0E, 0x0D, 0x0C, 0x08, 0x07, 0x06, 0x02, 0x01, 0x00); |
| 43 |
const __m128i m1 = _mm_set_epi8(0x0E, 0x0A, 0x09, 0x08, 0x04, 0x03, 0x02, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80); |
| 44 |
const __m128i m2 = _mm_set_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x0C, 0x0B, 0x0A, 0x06, 0x05, 0x04, 0x00, 0x80); |
| 45 |
const __m128i m3 = _mm_set_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x0F); |
| 46 |
|
| 47 |
// clang-format on |
| 48 |
SSE_RESTORE_SIGN_WARNINGS |
| 49 |
|
| 50 |
const int sw = w / 16; // This are the number of 3*16 blocks in each row |
| 51 |
const int sh = h / 2; |
| 52 |
const int rest_w = w - (16 * w); |
| 53 |
|
| 54 |
for (int i = 0; i < sh; i++) |
| 55 |
{ |
| 56 |
const __m128i* inp = reinterpret_cast<const __m128i*>(in); |
| 57 |
uint8_t* outp = out; |
| 58 |
|
| 59 |
for (int j = 0; j < sw; j++) |
| 60 |
{ |
| 61 |
// 16-byte blocks #0,#1,#2: |
| 62 |
__m128i d0 = mm_load_si128<MemIsAligned>(inp++); |
| 63 |
__m128i d1 = mm_load_si128<MemIsAligned>(inp++); |
| 64 |
|
| 65 |
// First 16 bytes: |
| 66 |
__m128i shuf0 = _mm_shuffle_epi8(d0, m0); |
| 67 |
__m128i shuf1 = _mm_shuffle_epi8(d1, m1); |
| 68 |
|
| 69 |
__m128i res0 = _mm_or_si128(shuf0, shuf1); |
| 70 |
|
| 71 |
_mm_storeu_si128(reinterpret_cast<__m128i*>(outp), |
| 72 |
res0); // aligned output |
| 73 |
outp += 16; |
| 74 |
|
| 75 |
// Last 8 bytes: |
| 76 |
__m128i d2 = mm_load_si128<MemIsAligned>(inp++); |
| 77 |
|
| 78 |
// Write lower 8 bytes only |
| 79 |
_mm_storel_epi64( |
| 80 |
reinterpret_cast<__m128i*>(outp), |
| 81 |
_mm_or_si128(_mm_shuffle_epi8(d2, m2), _mm_shuffle_epi8(d1, m3))); |
| 82 |
outp += 8; |
| 83 |
} |
| 84 |
|
| 85 |
// Extra pixels? (w mod 16 != 0) |
| 86 |
if (rest_w != 0) |
| 87 |
{ |
| 88 |
const uint8_t* in_rest = in + 3 * 16 * sw; |
| 89 |
for (int p = 0; p < rest_w / 2; p++) |
| 90 |
{ |
| 91 |
outp[0] = in_rest[0]; |
| 92 |
outp[1] = in_rest[1]; |
| 93 |
outp[2] = in_rest[2]; |
| 94 |
in_rest += 6; |
| 95 |
outp += 3; |
| 96 |
} |
| 97 |
} |
| 98 |
|
| 99 |
in += 2 * step_in; // Skip one row |
| 100 |
out += step_out; |
| 101 |
} |
| 102 |
} |
| 103 |
|
| 104 |
/** Subsample each 2x2 pixel block into 1x1 pixel, taking the first pixel & |
| 105 |
* ignoring the other 3 |
| 106 |
* - <b>Input format:</b> uint8_t, 3 channels (RGB or BGR) |
| 107 |
* - <b>Output format:</b> uint8_t, 3 channels (RGB or BGR) |
| 108 |
* - <b>Preconditions:</b> in & out may be aligned to 16bytes (faster) or not, |
| 109 |
* step may be k*16 (faster) or not. |
| 110 |
* - <b>Notes:</b> |
| 111 |
* - <b>Requires:</b> SSSE3 |
| 112 |
* - <b>Invoked from:</b> mrpt::img::CImage::scaleHalf() |
| 113 |
*/ |
| 114 |
void image_SSSE3_scale_half_3c8u( |
| 115 |
const uint8_t* in, uint8_t* out, int w, int h, size_t step_in, size_t step_out) |
| 116 |
{ |
| 117 |
if (mrpt::system::is_aligned<16>(in) && mrpt::system::is_aligned<16>(out) && |
| 118 |
is_multiple<16>(step_in) && is_multiple<16>(step_out)) |
| 119 |
{ |
| 120 |
impl_image_SSSE3_scale_half_3c8u<true>(in, out, w, h, step_in, step_out); |
| 121 |
} |
| 122 |
else |
| 123 |
{ |
| 124 |
impl_image_SSSE3_scale_half_3c8u<false>(in, out, w, h, step_in, step_out); |
| 125 |
} |
| 126 |
} |
| 127 |
|
| 128 |
// This is the actual function behind both: image_SSSE3_rgb_to_gray_8u() and |
| 129 |
// image_SSSE3_bgr_to_gray_8u(): |
| 130 |
template <bool IS_RGB, bool MemIsAligned> |
| 131 |
void impl_image_SSSE3_rgb_or_bgr_to_gray_8u( |
| 132 |
const uint8_t* in, uint8_t* out, int w, int h, size_t step_in, size_t step_out) |
| 133 |
{ |
| 134 |
SSE_DISABLE_WARNINGS |
| 135 |
// clang-format off |
| 136 |
|
| 137 |
// Masks: 0 1 2 3 4 5 6 7 8 9 A B C D E F |
| 138 |
// reds[0-7] from D0 |
| 139 |
const __m128i mask0 = _mm_setr_epi8(0x80, 0x00, 0x80, 0x03, 0x80, 0x06, 0x80, 0x09, 0x80, 0x0C, 0x80, 0x0F, 0x80, 0x80, 0x80, 0x80); |
| 140 |
// reds[0-7] from D1 |
| 141 |
const __m128i mask1 = _mm_setr_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x02, 0x80, 0x05); |
| 142 |
// greens[0-7] from D0 |
| 143 |
const __m128i mask2 = _mm_setr_epi8(0x80, 0x01, 0x80, 0x04, 0x80, 0x07, 0x80, 0x0A, 0x80, 0x0D, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80); |
| 144 |
// greens[0-7] from D1 |
| 145 |
const __m128i mask3 = _mm_setr_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 0x80, 0x03, 0x80, 0x06); |
| 146 |
// blues[0-7] from D0 |
| 147 |
const __m128i mask4 = _mm_setr_epi8(0x80, 0x02, 0x80, 0x05, 0x80, 0x08, 0x80, 0x0B, 0x80, 0x0E, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80); |
| 148 |
// blues[0-7] from D1 |
| 149 |
const __m128i mask5 = _mm_setr_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x01, 0x80, 0x04, 0x80, 0x07); |
| 150 |
// reds[8-15] from D1 |
| 151 |
const __m128i mask6 = _mm_setr_epi8(0x80, 0x08, 0x80, 0x0B, 0x80, 0x0E, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80); |
| 152 |
// reds[8-15] from D2 |
| 153 |
const __m128i mask7 = _mm_setr_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x01, 0x80, 0x04, 0x80, 0x07, 0x80, 0x0A, 0x80, 0x0D); |
| 154 |
// greens[8-15] from D1 |
| 155 |
const __m128i mask8 = _mm_setr_epi8(0x80, 0x09, 0x80, 0x0C, 0x80, 0x0F, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80); |
| 156 |
// greens[8-15] from D2 |
| 157 |
const __m128i mask9 = _mm_setr_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x02, 0x80, 0x05, 0x80, 0x08, 0x80, 0x0B, 0x80, 0x0E); |
| 158 |
// blues[8-15] from D1 |
| 159 |
const __m128i mask10 = _mm_setr_epi8(0x80, 0x0A, 0x80, 0x0D, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80); |
| 160 |
// blues[8-15] from D2 |
| 161 |
const __m128i mask11 = _mm_setr_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 0x80, 0x03, 0x80, 0x06, 0x80, 0x09, 0x80, 0x0C, 0x80, 0x0F); |
| 162 |
// Conversion factors for RGB->Y |
| 163 |
const __m128i VAL_R = _mm_setr_epi8(0x00, 0x1D, 0x00, 0x1D, 0x00, 0x1D, 0x00, 0x1D, 0x00, 0x1D, 0x00, 0x1D, 0x00, 0x1D, 0x00, 0x1D); |
| 164 |
const __m128i VAL_G = _mm_setr_epi8(0x00, 0x96, 0x00, 0x96, 0x00, 0x96, 0x00, 0x96, 0x00, 0x96, 0x00, 0x96, 0x00, 0x96, 0x00, 0x96); |
| 165 |
const __m128i VAL_B = _mm_setr_epi8(0x00, 0x4D, 0x00, 0x4D, 0x00, 0x4D, 0x00, 0x4D, 0x00, 0x4D, 0x00, 0x4D, 0x00, 0x4D, 0x00, 0x4D); |
| 166 |
// mask: |
| 167 |
const __m128i mask_low = _mm_setr_epi8(0x01, 0x03, 0x05, 0x07, 0x09, 0x0B, 0x0D, 0x0F, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80); |
| 168 |
|
| 169 |
// clang-format on |
| 170 |
SSE_RESTORE_SIGN_WARNINGS |
| 171 |
|
| 172 |
const __m128i m0 = IS_RGB ? mask4 : mask0; |
| 173 |
const __m128i m1 = IS_RGB ? mask5 : mask1; |
| 174 |
const __m128i m2 = mask2; |
| 175 |
const __m128i m3 = mask3; |
| 176 |
const __m128i m4 = IS_RGB ? mask0 : mask4; |
| 177 |
const __m128i m5 = IS_RGB ? mask1 : mask5; |
| 178 |
const __m128i m6 = IS_RGB ? mask10 : mask6; |
| 179 |
const __m128i m7 = IS_RGB ? mask11 : mask7; |
| 180 |
const __m128i m8 = mask8; |
| 181 |
const __m128i m9 = mask9; |
| 182 |
const __m128i m10 = IS_RGB ? mask6 : mask10; |
| 183 |
const __m128i m11 = IS_RGB ? mask7 : mask11; |
| 184 |
|
| 185 |
const int sw = w >> 4; // This are the number of 3*16 blocks in each row |
| 186 |
const int sh = h; |
| 187 |
|
| 188 |
for (int i = 0; i < sh; i++) |
| 189 |
{ |
| 190 |
const __m128i* inp = reinterpret_cast<const __m128i*>(in); |
| 191 |
uint8_t* outp = out; |
| 192 |
|
| 193 |
for (int j = 0; j < sw; j++) |
| 194 |
{ |
| 195 |
// We process RGB data in blocks of 3 x 16byte blocks: |
| 196 |
const __m128i d0 = mm_load_si128<MemIsAligned>(inp++); |
| 197 |
const __m128i d1 = mm_load_si128<MemIsAligned>(inp++); |
| 198 |
const __m128i d2 = mm_load_si128<MemIsAligned>(inp++); |
| 199 |
|
| 200 |
// First 8 bytes of gray levels: |
| 201 |
{ |
| 202 |
const __m128i BLUES_0_7 = _mm_or_si128(_mm_shuffle_epi8(d0, m0), _mm_shuffle_epi8(d1, m1)); |
| 203 |
const __m128i GREENS_0_7 = _mm_or_si128(_mm_shuffle_epi8(d0, m2), _mm_shuffle_epi8(d1, m3)); |
| 204 |
const __m128i REDS_0_7 = _mm_or_si128(_mm_shuffle_epi8(d0, m4), _mm_shuffle_epi8(d1, m5)); |
| 205 |
|
| 206 |
// _mm_mulhi_epu16(): Multiplies the 8 unsigned 16-bit integers |
| 207 |
// from a by the 8 unsigned 16-bit integers from b. |
| 208 |
// r0 := (a0 * b0)[31:16] |
| 209 |
// r1 := (a1 * b1)[31:16] |
| 210 |
//... |
| 211 |
// r7 := (a7 * b7)[31:16] |
| 212 |
// |
| 213 |
const __m128i GRAYS_0_7 = _mm_adds_epu16( |
| 214 |
_mm_mulhi_epu16(REDS_0_7, VAL_R), |
| 215 |
_mm_adds_epu16(_mm_mulhi_epu16(GREENS_0_7, VAL_G), _mm_mulhi_epu16(BLUES_0_7, VAL_B))); |
| 216 |
|
| 217 |
_mm_storel_epi64(reinterpret_cast<__m128i*>(outp), _mm_shuffle_epi8(GRAYS_0_7, mask_low)); |
| 218 |
outp += 8; |
| 219 |
} |
| 220 |
|
| 221 |
// Second 8 bytes of gray levels: |
| 222 |
{ |
| 223 |
const __m128i BLUES_8_15 = _mm_or_si128(_mm_shuffle_epi8(d1, m6), _mm_shuffle_epi8(d2, m7)); |
| 224 |
const __m128i GREENS_8_15 = |
| 225 |
_mm_or_si128(_mm_shuffle_epi8(d1, m8), _mm_shuffle_epi8(d2, m9)); |
| 226 |
const __m128i REDS_8_15 = |
| 227 |
_mm_or_si128(_mm_shuffle_epi8(d1, m10), _mm_shuffle_epi8(d2, m11)); |
| 228 |
|
| 229 |
const __m128i GRAYS_8_15 = _mm_adds_epu16( |
| 230 |
_mm_mulhi_epu16(REDS_8_15, VAL_R), |
| 231 |
_mm_adds_epu16( |
| 232 |
_mm_mulhi_epu16(GREENS_8_15, VAL_G), _mm_mulhi_epu16(BLUES_8_15, VAL_B))); |
| 233 |
|
| 234 |
_mm_storel_epi64(reinterpret_cast<__m128i*>(outp), _mm_shuffle_epi8(GRAYS_8_15, mask_low)); |
| 235 |
outp += 8; |
| 236 |
} |
| 237 |
} |
| 238 |
in += step_in; |
| 239 |
out += step_out; |
| 240 |
} |
| 241 |
|
| 242 |
} // end impl_image_SSSE3_rgb_or_bgr_to_gray_8u() |
| 243 |
|
| 244 |
/** Convert a RGB image (3cu8) into a GRAYSCALE (1c8u) image, using |
| 245 |
* Y=77*R+150*G+29*B |
| 246 |
* - <b>Input format:</b> uint8_t, 3 channels (BGR order) |
| 247 |
* - <b>Output format:</b> uint8_t, 1 channel |
| 248 |
* - <b>Preconditions:</b> in & out aligned to 16bytes (faster) or not, step = |
| 249 |
* k*16 |
| 250 |
* - <b>Notes:</b> |
| 251 |
* - <b>Requires:</b> SSSE3 |
| 252 |
* - <b>Invoked from:</b> mrpt::img::CImage::grayscale(), |
| 253 |
* mrpt::img::CImage::grayscaleInPlace() |
| 254 |
*/ |
| 255 |
void image_SSSE3_bgr_to_gray_8u( |
| 256 |
const uint8_t* in, uint8_t* out, int w, int h, size_t step_in, size_t step_out) |
| 257 |
{ |
| 258 |
ASSERTMSG_((step_in & 0x0f) == 0, "step of input image must be 16*k"); |
| 259 |
ASSERTMSG_((step_out & 0x0f) == 0, "step of output image must be 16*k"); |
| 260 |
|
| 261 |
if (mrpt::system::is_aligned<16>(in) && mrpt::system::is_aligned<16>(out)) |
| 262 |
{ |
| 263 |
impl_image_SSSE3_rgb_or_bgr_to_gray_8u<false, true>(in, out, w, h, step_in, step_out); |
| 264 |
} |
| 265 |
else |
| 266 |
{ |
| 267 |
impl_image_SSSE3_rgb_or_bgr_to_gray_8u<false, false>(in, out, w, h, step_in, step_out); |
| 268 |
} |
| 269 |
} |
| 270 |
|
| 271 |
/** Convert a RGB image (3cu8) into a GRAYSCALE (1c8u) image, using |
| 272 |
* Y=77*R+150*G+29*B |
| 273 |
* - <b>Input format:</b> uint8_t, 3 channels (RGB order) |
| 274 |
* - <b>Output format:</b> uint8_t, 1 channel |
| 275 |
* - <b>Preconditions:</b> in & out aligned to 16bytes (faster) or not, step = |
| 276 |
* k*16 |
| 277 |
* - <b>Notes:</b> |
| 278 |
* - <b>Requires:</b> SSSE3 |
| 279 |
* - <b>Invoked from:</b> mrpt::img::CImage::grayscale(), |
| 280 |
* mrpt::img::CImage::grayscaleInPlace() |
| 281 |
*/ |
| 282 |
void image_SSSE3_rgb_to_gray_8u( |
| 283 |
const uint8_t* in, uint8_t* out, int w, int h, size_t step_in, size_t step_out) |
| 284 |
{ |
| 285 |
ASSERTMSG_((step_in & 0x0f) == 0, "step of input image must be 16*k"); |
| 286 |
ASSERTMSG_((step_out & 0x0f) == 0, "step of output image must be 16*k"); |
| 287 |
|
| 288 |
if (mrpt::system::is_aligned<16>(in) && mrpt::system::is_aligned<16>(out)) |
| 289 |
{ |
| 290 |
impl_image_SSSE3_rgb_or_bgr_to_gray_8u<true, true>(in, out, w, h, step_in, step_out); |
| 291 |
} |
| 292 |
else |
| 293 |
{ |
| 294 |
impl_image_SSSE3_rgb_or_bgr_to_gray_8u<true, false>(in, out, w, h, step_in, step_out); |
| 295 |
} |
| 296 |
} |
| 297 |
|
| 298 |
/** @} */ |
| 299 |
|
| 300 |
#endif // end of MRPT_ARCH_INTEL_COMPATIBLE |