265unsigned int MappingF1::appearanceCost5x5(
const unsigned int xTarget,
const unsigned int yTarget,
const Scalar xSource,
const Scalar ySource,
const uint8_t* frame,
const uint8_t* mask,
const unsigned int framePaddingElements,
const unsigned int maskPaddingElements,
const unsigned int borderFactor)
const
267 static_assert(tChannels >= 1u,
"Invalid channel number!");
269 ocean_assert(mask !=
nullptr && mask !=
nullptr);
273 ocean_assert(xTarget >= 2u && xTarget <=
width_ - 3u);
274 ocean_assert(yTarget >= 2u && yTarget <=
height_ - 3u);
279 const unsigned int maskStrideElements =
width_ + maskPaddingElements;
282 ocean_assert(mask[yTarget * maskStrideElements + xTarget] != 0xFFu);
289 ocean_assert(mask[ySourceRounded * maskStrideElements + xSourceRounded] == 0xFFu);
292 constexpr unsigned int patchSize = 5u;
293 constexpr unsigned int patchSize_2 = patchSize / 2u;
295 constexpr unsigned int patchArea = patchSize * patchSize;
299 if (mask[yTarget * maskStrideElements + xTarget] == 0x00u || mask[yTarget * maskStrideElements + xTarget] >= 3u)
301 ocean_assert(xTarget >= patchSize_2 && yTarget >= patchSize_2 && xTarget <=
width_ - patchSize_2 - 1u && yTarget <=
height_ - patchSize_2 - 1u);
303 return Advanced::AdvancedSumSquareDifferences::patch8BitPerChannel<tChannels, patchSize>(frame, frame,
width_,
width_, xTarget, yTarget, xSource, ySource, framePaddingElements, framePaddingElements) * spatialCostNormalization<tChannels>() / patchArea;
306 if (xTarget >= patchSize_2 && yTarget >= patchSize_2 && xTarget <=
width_ - patchSize_2 - 1u && yTarget <=
height_ - patchSize_2 - 1u)
308 return ssd5x5Mask<tChannels>(frame, frame, mask,
width_,
height_,
width_,
height_, xTarget, yTarget, xSource, ySource, framePaddingElements, framePaddingElements, maskPaddingElements, borderFactor) * spatialCostNormalization<tChannels>() / patchArea;
312 ocean_assert(
false &&
"This should never happen!");
317inline unsigned int MappingF1::appearanceReferenceCost5x5(
const unsigned int xTarget,
const unsigned int yTarget,
const Scalar xSource,
const Scalar ySource,
const uint8_t* frame,
const uint8_t* mask,
const uint8_t* reference,
const unsigned int framePaddingElements,
const unsigned int maskPaddingElements,
const unsigned int referencePaddingElements,
const unsigned int borderFactor)
const
319 static_assert(tChannels >= 1u,
"Invalid channel number!");
321 ocean_assert(mask !=
nullptr && mask !=
nullptr && reference !=
nullptr);
325 ocean_assert(xTarget >= 2u && xTarget <=
width_ - 3u);
326 ocean_assert(yTarget >= 2u && yTarget <=
height_ - 3u);
331 const unsigned int maskStrideElements =
width_ + maskPaddingElements;
334 ocean_assert(mask[yTarget * maskStrideElements + xTarget] != 0xFF);
341 ocean_assert(mask[ySourceRounded * maskStrideElements + xSourceRounded] == 0xFFu);
344 constexpr unsigned int patchSize = 5u;
345 constexpr unsigned int patchSize_2 = patchSize / 2u;
347 constexpr unsigned int patchArea = patchSize * patchSize;
351 if (mask[yTarget * maskStrideElements + xTarget] == 0x00u || mask[yTarget * maskStrideElements + xTarget] >= 3u)
353 ocean_assert(xTarget >= patchSize_2 && yTarget >= patchSize_2 && xTarget <=
width_ - patchSize_2 - 1u && yTarget <=
height_ - patchSize_2 - 1u);
355 return sumSquareDifferencesThreePatch8BitPerChannel<tChannels, patchSize>(frame, reference, frame,
width_,
width_,
width_, framePaddingElements, referencePaddingElements, framePaddingElements, 1u, 5u, xTarget, yTarget, xTarget, yTarget, xSource, ySource) * spatialCostNormalization<tChannels>() / patchArea;
358 if (xTarget >= patchSize_2 && yTarget >= patchSize_2 && xTarget <=
width_ - patchSize_2 - 1u && yTarget <=
height_ - patchSize_2 - 1u)
360 return (5u * ssd5x5Mask<tChannels>(reference, frame, mask,
width_,
height_,
width_,
height_, xTarget, yTarget, xSource, ySource, referencePaddingElements, framePaddingElements, maskPaddingElements, borderFactor)
361 + ssd5x5Mask<tChannels>(frame, frame, mask,
width_,
height_,
width_,
height_, xTarget, yTarget, xSource, ySource, framePaddingElements, framePaddingElements, maskPaddingElements, borderFactor)) * spatialCostNormalization<tChannels>() / patchArea;
365 ocean_assert(
false &&
"This should never happen!");
371 ocean_assert(targetMask !=
nullptr);
372 ocean_assert(maxCost >
Scalar(0));
374 const unsigned int targetMaskStrideElements =
width_ + targetMaskPaddingElements;
380 ocean_assert(targetMask[yTarget * targetMaskStrideElements + xTarget] != 0xFF);
384 const uint8_t* targetMaskPointer = targetMask + (int(yTarget) - 1) *
int(targetMaskStrideElements) + xTarget;
388 if (yTarget > 0 && *targetMaskPointer != 0xFF)
390 ocean_assert(mappingPointer->
x() >= 0 && mappingPointer->
x() <
Scalar(
width_));
391 ocean_assert(mappingPointer->
y() >= 0 && mappingPointer->
y() <
Scalar(
height_));
392 ocean_assert(*mappingPointer ==
mappingF_[(yTarget - 1u) *
width_ + xTarget]);
393 ocean_assert(*targetMaskPointer == targetMask[(yTarget - 1u) * targetMaskStrideElements + xTarget]);
397 const Scalar localCost =
sqr(mappingPointer->
x() - xSource) +
sqr(mappingPointer->
y() - ySource + 1);
404 if (localCost < cost)
411 ocean_assert(
width_ >= 1u);
412 targetMaskPointer += targetMaskStrideElements - 1u;
413 mappingPointer +=
width_ - 1u;
416 if (xTarget > 0 && *targetMaskPointer != 0xFF)
418 ocean_assert(mappingPointer->
x() >= 0 && mappingPointer->
x() <
Scalar(
width_));
419 ocean_assert(mappingPointer->
y() >= 0 && mappingPointer->
y() <
Scalar(
height_));
420 ocean_assert(*mappingPointer ==
mappingF_[yTarget *
width_ + xTarget - 1u]);
421 ocean_assert(*targetMaskPointer == targetMask[yTarget * targetMaskStrideElements + xTarget - 1u]);
425 const Scalar localCost =
sqr(mappingPointer->
x() - xSource + 1) +
sqr(mappingPointer->
y() - ySource);
432 if (localCost < cost)
439 targetMaskPointer += 2;
443 if (xTarget + 1 <
width_ && *targetMaskPointer != 0xFF)
445 ocean_assert(mappingPointer->
x() >= 0 && mappingPointer->
x() <
Scalar(
width_));
446 ocean_assert(mappingPointer->
y() >= 0 && mappingPointer->
y() <
Scalar(
height_));
447 ocean_assert(*mappingPointer ==
mappingF_[yTarget *
width_ + xTarget + 1u]);
448 ocean_assert(*targetMaskPointer == targetMask[yTarget * targetMaskStrideElements + xTarget + 1u]);
452 const Scalar localCost =
sqr(mappingPointer->
x() - xSource - 1) +
sqr(mappingPointer->
y() - ySource);
459 if (localCost < cost)
466 targetMaskPointer += targetMaskStrideElements - 1u;
467 mappingPointer +=
width_ - 1u;
470 if (yTarget + 1 <
height_ && *targetMaskPointer != 0xFF)
472 ocean_assert(mappingPointer->
x() >= 0 && mappingPointer->
x() <
Scalar(
width_));
473 ocean_assert(mappingPointer->
y() >= 0 && mappingPointer->
y() <
Scalar(
height_));
474 ocean_assert(*mappingPointer ==
mappingF_[(yTarget + 1u) *
width_ + xTarget]);
475 ocean_assert(*targetMaskPointer == targetMask[(yTarget + 1u) * targetMaskStrideElements + xTarget]);
479 const Scalar localCost =
sqr(mappingPointer->
x() - xSource) +
sqr(mappingPointer->
y() - ySource - 1);
486 if (localCost < cost)
512inline unsigned int MappingF1::ssd5x5Mask(
const uint8_t* frame0,
const uint8_t* frame1,
const uint8_t* mask0,
const unsigned int width0,
const unsigned int height0,
const unsigned int width1,
const unsigned int height1,
const unsigned int xPosition0,
const unsigned int yPosition0,
const Scalar xPosition1,
const Scalar yPosition1,
const unsigned int frame0PaddingElements,
const unsigned int frame1PaddingElements,
const unsigned int mask0PaddingElements,
const unsigned int borderFactor)
514 static_assert(tChannels >= 1u,
"Invalid channel number!");
516 ocean_assert(frame0 !=
nullptr && frame1 !=
nullptr && mask0 !=
nullptr);
517 ocean_assert_and_suppress_unused(width0 >= 5u && height0 >= 5u, height0);
518 ocean_assert_and_suppress_unused(width1 >= 6u && height1 >= 6u, height1);
520 ocean_assert(xPosition0 >= 2u && xPosition0 <= width0 - 2u);
521 ocean_assert(yPosition0 >= 2u && yPosition0 <= height0 - 2u);
523 ocean_assert(xPosition1 >=
Scalar(2) && xPosition1 <
Scalar(width1 - 3u));
524 ocean_assert(yPosition1 >=
Scalar(2) && yPosition1 <
Scalar(height1 - 3u));
526 ocean_assert(borderFactor >= 1u);
528 constexpr unsigned int patchSize = 5u;
529 constexpr unsigned int patchArea = patchSize * patchSize;
531 constexpr unsigned int framePatch0PaddingElements = 0u;
532 constexpr unsigned int maskPatch0PaddingElements = 0u;
534 uint8_t framePatch0[patchArea * tChannels];
535 FrameConverter::patchFrame<uint8_t>(frame0, framePatch0, width0, tChannels, xPosition0, yPosition0, patchSize, frame0PaddingElements, framePatch0PaddingElements);
537 uint8_t maskPatch0[patchArea];
538 FrameConverter::patchFrame<uint8_t>(mask0, maskPatch0, width0, 1u, xPosition0, yPosition0, patchSize, mask0PaddingElements, maskPatch0PaddingElements);
540 uint8_t framePatch1[patchArea * tChannels];
541 Advanced::AdvancedFrameInterpolatorBilinear::interpolateSquarePatch8BitPerChannel<tChannels, patchSize, CV::PC_TOP_LEFT>(frame1, width1, frame1PaddingElements, framePatch1,
Vector2(xPosition1, yPosition1));
543 unsigned int ssd = 0u;
545 for (
unsigned int n = 0u; n < patchArea; ++n)
547 unsigned int ssdLocal = 0u;
549 for (
unsigned int c = 0u; c < tChannels; ++c)
551 const int value = int(framePatch0[n * tChannels + c]) - int(framePatch1[n * tChannels + c]);
552 ssdLocal += value * value;
555 if (maskPatch0[n] == 0xFFu)
557 ssd += ssdLocal * borderFactor;
589inline unsigned int MappingF1::sumSquareDifferencesThreePatch8BitPerChannel<3u, 5u>(
const uint8_t* image0,
const uint8_t* image1,
const uint8_t* image2,
const unsigned int width0,
const unsigned int width1,
const unsigned int width2,
unsigned int image0PaddingElements,
const unsigned int image1PaddingElements,
const unsigned int image2PaddingElements,
const unsigned int factor02,
const unsigned int factor12,
const unsigned int centerX0,
const unsigned int centerY0,
const unsigned int centerX1,
const unsigned int centerY1,
const Scalar centerX2,
const Scalar centerY2)
591 constexpr unsigned int tChannels = 3u;
592 constexpr unsigned int tPatchSize = 5u;
593 constexpr unsigned int tPatchSize_2 = tPatchSize / 2u;
595 ocean_assert(image0 !=
nullptr && image1 !=
nullptr && image2 !=
nullptr);
597 ocean_assert(centerX0 >= tPatchSize_2 && centerX0 < width0 - tPatchSize_2);
598 ocean_assert(centerY0 >= tPatchSize_2);
600 ocean_assert(centerX1 >= tPatchSize_2 && centerX1 < width1 - tPatchSize_2);
601 ocean_assert(centerY1 >= tPatchSize_2);
603 ocean_assert(centerX2 >=
Scalar(tPatchSize_2) && centerX2 <
Scalar(width2 - tPatchSize_2 - 1u));
604 ocean_assert(centerY2 >=
Scalar(tPatchSize_2));
606 const unsigned int image0StrideElements = width0 * tChannels + image0PaddingElements;
607 const unsigned int image1StrideElements = width1 * tChannels + image1PaddingElements;
608 const unsigned int image2StrideElements = width2 * tChannels + image2PaddingElements;
610 const uint8_t* imageTopLeft0 = image0 + (centerY0 - tPatchSize_2) * image0StrideElements + (centerX0 - tPatchSize_2) * tChannels;
611 const uint8_t* imageTopLeft1 = image1 + (centerY1 - tPatchSize_2) * image1StrideElements + (centerX1 - tPatchSize_2) * tChannels;
612 const uint8_t* imageTopLeft2 = image2 + ((
unsigned int)(centerY2) - tPatchSize_2) * image2StrideElements + ((
unsigned int)(centerX2) - tPatchSize_2) * tChannels;
614 const Scalar scalarFx2 = centerX2 -
Scalar((
unsigned int)(centerX2));
615 const Scalar scalarFy2 = centerY2 -
Scalar((
unsigned int)(centerY2));
617 ocean_assert(scalarFx2 >= 0 && scalarFx2 <= 1);
618 ocean_assert(scalarFy2 >= 0 && scalarFy2 <= 1);
620 const unsigned int fx2 = (
unsigned int)(
Scalar(128) * scalarFx2 +
Scalar(0.5));
621 const unsigned int fy2 = (
unsigned int)(
Scalar(128) * scalarFy2 +
Scalar(0.5));
633 const unsigned int fx2_ = 128u - fx2;
634 const unsigned int fy2_ = 128u - fy2;
636 const __m128i f2x_y_ = _mm_set1_epi16(
short(fx2_ * fy2_));
637 const __m128i f2xy_ = _mm_set1_epi16(
short(fx2 * fy2_));
638 const __m128i f2x_y = _mm_set1_epi16(
short(fx2_ * fy2));
639 const __m128i f2xy = _mm_set1_epi16(
short(fx2 * fy2));
646 __m128i image0_row0 = _mm_shuffle_epi8(_mm_lddqu_si128((__m128i*)imageTopLeft0),
SSE::set128i(0x0E0D0C0B0A0908A0ull, 0x0706050403020100ull));
648 __m128i image2_row0Front = _mm_lddqu_si128((__m128i*)imageTopLeft2);
649 __m128i image2_row1Front = _mm_lddqu_si128((__m128i*)(imageTopLeft2 + image2StrideElements));
652 __m128i image1_row0 = _mm_shuffle_epi8(_mm_lddqu_si128((__m128i*)imageTopLeft1),
SSE::set128i(0x0E0D0C0B0A0908A0ull, 0x0706050403020100ull));
654 __m128i image2_row0Back = _mm_lddqu_si128((__m128i*)(imageTopLeft2 + 8u));
655 __m128i image2_row1Back = _mm_lddqu_si128((__m128i*)(imageTopLeft2 + image2StrideElements + 8u));
668 __m128i image0_row1 = _mm_shuffle_epi8(_mm_lddqu_si128((__m128i*)(imageTopLeft0 + 1u * image0StrideElements)),
SSE::set128i(0x0E0D0C0B0A0908A0ull, 0x0706050403020100ull));
670 __m128i image2_row2Front = _mm_lddqu_si128((__m128i*)(imageTopLeft2 + 2u * image2StrideElements));
673 __m128i image1_row1 = _mm_shuffle_epi8(_mm_lddqu_si128((__m128i*)(imageTopLeft1 + 1u * image1StrideElements)),
SSE::set128i(0x0E0D0C0B0A0908A0ull, 0x0706050403020100ull));
675 __m128i image2_row2Back = _mm_lddqu_si128((__m128i*)(imageTopLeft2 + 2u * image2StrideElements + 8u));
688 __m128i image0_row2 = _mm_shuffle_epi8(_mm_lddqu_si128((__m128i*)(imageTopLeft0 + 2u * image0StrideElements)),
SSE::set128i(0x0E0D0C0B0A0908A0ull, 0x0706050403020100ull));
690 __m128i image2_row3Front = _mm_lddqu_si128((__m128i*)(imageTopLeft2 + 3u * image2StrideElements));
693 __m128i image1_row2 = _mm_shuffle_epi8(_mm_lddqu_si128((__m128i*)(imageTopLeft1 + 2u * image1StrideElements)),
SSE::set128i(0x0E0D0C0B0A0908A0ull, 0x0706050403020100ull));
695 __m128i image2_row3Back = _mm_lddqu_si128((__m128i*)(imageTopLeft2 + 3u * image2StrideElements + 8u));
707 __m128i image0_row3 = _mm_shuffle_epi8(_mm_lddqu_si128((__m128i*)(imageTopLeft0 + 3u * image0StrideElements)),
SSE::set128i(0x0E0D0C0B0A0908A0ull, 0x0706050403020100ull));
709 __m128i image2_row4Front = _mm_lddqu_si128((__m128i*)(imageTopLeft2 + 4u * image2StrideElements));
712 __m128i image1_row3 = _mm_shuffle_epi8(_mm_lddqu_si128((__m128i*)(imageTopLeft1 + 3u * image1StrideElements)),
SSE::set128i(0x0E0D0C0B0A0908A0ull, 0x0706050403020100ull));
714 __m128i image2_row4Back = _mm_lddqu_si128((__m128i*)(imageTopLeft2 + 4u * image2StrideElements + 8u));
723 __m128i image0_row4 = _mm_shuffle_epi8(_mm_lddqu_si128((__m128i*)(imageTopLeft0 + 4u * image0StrideElements)),
SSE::set128i(0x0E0D0C0B0A0908A0ull, 0x0706050403020100ull));
725 __m128i image2_row5Front = _mm_lddqu_si128((__m128i*)(imageTopLeft2 + 5u * image2StrideElements));
728 __m128i image1_row4 = _mm_shuffle_epi8(_mm_lddqu_si128((__m128i*)(imageTopLeft1 + 4u * image1StrideElements)),
SSE::set128i(0x0E0D0C0B0A0908A0ull, 0x0706050403020100ull));
730 __m128i image2_row5Back = _mm_srli_si128(_mm_lddqu_si128((__m128i*)(imageTopLeft2 + 5u * image2StrideElements + 8u - 6u)), 6);