WSeries_Calibration/QtCameraHardWareCopilot/OptimizeCal.cpp

#include "OptimizeCal.h"


const unsigned char icvSaturate8u_cv[] =
{
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   1,   2,   3,   4,   5,   6,   7,   8,   9,  10,  11,  12,  13,  14,  15,
	 16,  17,  18,  19,  20,  21,  22,  23,  24,  25,  26,  27,  28,  29,  30,  31,
	 32,  33,  34,  35,  36,  37,  38,  39,  40,  41,  42,  43,  44,  45,  46,  47,
	 48,  49,  50,  51,  52,  53,  54,  55,  56,  57,  58,  59,  60,  61,  62,  63,
	 64,  65,  66,  67,  68,  69,  70,  71,  72,  73,  74,  75,  76,  77,  78,  79,
	 80,  81,  82,  83,  84,  85,  86,  87,  88,  89,  90,  91,  92,  93,  94,  95,
	 96,  97,  98,  99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111,
	112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127,
	128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143,
	144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159,
	160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175,
	176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191,
	192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207,
	208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223,
	224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239,
	240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
	255
};


void FastCopy(unsigned char* pSrc, unsigned char* pDst, int nWidth)
{
	//set a _mm256 to save the src Data Width
	while (nWidth > 0)
	{
		__m256i srcData = _mm256_loadu_si256((__m256i*)pSrc);
		__m256i src2Data = _mm256_loadu_si256((__m256i*)(pSrc + 32));
		__m256i* pDstData = (__m256i*)pDst;
		__m256i* pDst2Data = (__m256i*)(pDst + 32);
		_mm256_storeu_si256(pDstData, srcData);
		_mm256_storeu_si256(pDst2Data, src2Data);
		pSrc += 64;
		pDst += 64;
		nWidth -= 64;
	}
}

void distanceATS_L1_8u(unsigned char* const src, int nWidth, int nHeight, int nPitch, unsigned char* dst)
{

	int width = nWidth, height = nHeight;

	int a;
	unsigned char lut[256];
	int x, y;

	const unsigned char* sbase = src;
	unsigned char* dbase = dst;
	int srcstep = (int)nPitch;
	int dststep = (int)nPitch;

	////////////////////// forward scan ////////////////////////
	for (x = 0; x < 256; x++)
		lut[x] = cv::saturate_cast<uchar>(x + 1);

	//init first pixel to max (we're going to be skipping it)
	dbase[0] = (unsigned char)(sbase[0] == 0 ? 0 : 255);

	//first row (scan west only, skip first pixel)
	for (x = 1; x < width; x++)
		dbase[x] = (unsigned char)(sbase[x] == 0 ? 0 : lut[dbase[x - 1]]);

	for (y = 1; y < height; y++)
	{
		sbase += srcstep;
		dbase += dststep;

		//for left edge, scan north only
		a = sbase[0] == 0 ? 0 : lut[dbase[-dststep]];
		dbase[0] = (unsigned char)a;

		for (x = 1; x < width; x++)
		{
			a = sbase[x] == 0 ? 0 : lut[std::min(a, (int)dbase[x - dststep])];
			dbase[x] = (unsigned char)a;
		}
	}

	////////////////////// backward scan ///////////////////////

	a = dbase[width - 1];

	// do last row east pixel scan here (skip bottom right pixel)
	for (x = width - 2; x >= 0; x--)
	{
		a = lut[a];
		dbase[x] = (unsigned char)(CV_CALC_MIN_8U(a, dbase[x]));
	}

	// right edge is the only error case
	for (y = height - 2; y >= 0; y--)
	{
		dbase -= dststep;

		// do right edge
		a = lut[dbase[width - 1 + dststep]];
		a = dbase[width - 1] = (unsigned char)(std::min(a,(int)dbase[width - 1]));

		for (x = width - 2; x >= 0; x--)
		{
			int b = dbase[x + dststep];
			a = lut[std::min(a, b)];
			a = std::min(a, (int)dbase[x]);
			dbase[x] = (unsigned char)(a);
		}
	}
}

void FastCopy_Any(const unsigned char* src_ptr, unsigned char* dst_ptr, int width)
{
	SIMD_ALIGNED(unsigned char vin[128]);
	SIMD_ALIGNED(unsigned char vout[128]);
	memset(vin, 0, sizeof(vin)); /* for YUY2 and msan */
	int r = width & 63;
	int n = width & ~63;
	if (n > 0) {
		FastCopy((unsigned char*)src_ptr, dst_ptr, n);
	}
	memcpy(vin, src_ptr + (n >> 0) * 1, SS(r, 0) * 1);
	FastCopy(vin, vout, 63 + 1);
	memcpy(dst_ptr + n * 1, vout, r * 1);
}


void OptimizeCalMatAdd_8U(unsigned char* pRelated, int nRelatedWidth, int nRelatedHeight, int nRelatedPitch,
	unsigned char* pTarget, int nTargetWidth, int nTargetHeight, int nTargetPitch,
	int nLeft, int nTop,
	int nChannel
)
{
	int nCap = 256 / (8 * nChannel);
	unsigned char* pTargetPart = pTarget + nTop * nTargetPitch + nLeft;
	
	__m256i mmRelate,mmTarget;
	__m256i mmResult;
	for (int i = 0; i < nRelatedHeight; i++)
	{
		for (int wid = 0; wid < nRelatedWidth; wid+= nCap)
		{
			if (nRelatedWidth - wid < nCap)
			{
				int nlast = nRelatedWidth - wid;
				memcpy(mmRelate.m256i_u8, pRelated + i * nRelatedPitch + wid * nChannel, nlast * nChannel);
				memcpy(mmTarget.m256i_u8, pTargetPart + i * nTargetPitch + wid * nChannel, nlast * nChannel);
			}
			else
			{
				_mm256_loadu_epi8(pTargetPart + i * nTargetPitch + wid * nChannel);
				_mm256_loadu_epi8(pRelated + i * nRelatedPitch + wid * nChannel);
			}

			mmResult = _mm256_adds_epu8(mmRelate, mmTarget);

			if (nRelatedWidth - wid < nCap)
			{
				int nlast = nRelatedWidth - wid;
				memcpy(pTargetPart + i * nTargetPitch + wid* nChannel, mmResult.m256i_u8, nlast * nChannel);
			}
			else
				_mm256_storeu_epi8(pTargetPart + i * nTargetPitch + wid* nChannel, mmResult);
		}

	}
}