public void Copy(ShortImage a, int shift)
{
ushort *pa = a.Data(0, 0);
byte * p = data;
for (int i = 0; i < width * height; i++)
{
*p++ = (byte)(*pa++ >> shift);
}
}
public void Add(ShortImage a)
{
ushort *p_a = a.Data(0, 0);
ushort *p = data;
for (int i = 0; i < width * height; i++)
{
*p = (ushort)((*p_a) + (*p));
p++;
p_a++;
}
}
public void Add(ShortImage a)
{
ushort *pa = a.Data(0, 0);
float * p = data;
for (int i = 0; i < width * height; i++)
{
*p = *p + *pa;
p++;
pa++;
}
}
public void Copy(ShortImage shortImage)
{
ushort *p = shortImage.Data(0, 0);
ARGB32 *pOut = data;
for (int i = 0; i < width * height; i++)
{
pOut->A = 255;
pOut->R = (byte)*p;
pOut->G = (byte)*p;
pOut++->B = (byte)*p++;
}
}
public void XMirror(ShortImage a)
{
ushort *pOut = data;
ushort *pIn = a.data;
for (int yy = 0; yy < height; yy++)
{
pIn = a.Data(width - 1, yy);
for (int xx = 0; xx < width; xx++)
{
*pOut++ = *pIn--;
}
}
}
public void YMirror(ShortImage a)
{
ushort *pOut = data;
ushort *pIn = a.data;
for (int yy = 0; yy < height; yy++)
{
pIn = a.Data(0, height - yy);
for (int xx = 0; xx < width; xx++)
{
*pOut++ = *pIn++;
}
}
}
//Converts a 16-bit grayscale depth frame into a 32-bit frame
public void CopyShortImageForGrayscaleDisplay(ShortImage shortImage, ushort maxVal)
{
ushort *pIn = shortImage.Data(0, 0);
double multiplier = 255.0 / maxVal;
ARGB32 *pOut = data;
for (int i = 0; i < width * height; i++)
{
byte intensity = (byte)(multiplier * *pIn++);//(255 * *pIn++ / maxVal);
pOut->A = 255;
pOut->R = intensity;
pOut->G = intensity;
pOut++->B = intensity;
}
}
public void Threshold(ShortImage a, ushort threshold)
{
ushort *pa = a.Data(0, 0);
byte * p = data;
for (int i = 0; i < width * height; i++)
{
if (*pa++ > threshold)
{
*p++ = 255;
}
else
{
*p++ = 0;
}
}
}
//this is a special function which respects the YUYV ordering when mirroring
public void XMirror_YUYSpecial(ShortImage a)
{
//Y1 U Y2 V ---> Y2 U Y1 V
byte *pOut = (byte *)data;
byte *pIn = (byte *)a.data;
for (int yy = 0; yy < height; yy++)
{
pIn = (byte *)a.Data(width - 2, yy);
for (int xx = 0; xx < width; xx += 2)
{
*pOut++ = *(pIn + 2);
*pOut++ = *(pIn + 1);
*pOut++ = *pIn;
*pOut++ = *(pIn + 3);
pIn -= 4;
}
}
}
public void Copy(ShortImage a, int shift)
{
ushort* pa = a.Data(0, 0);
byte* p = data;
for (int i = 0; i < width * height; i++)
*p++ = (byte)(*pa++ >> shift);
}
public void Threshold(ShortImage a, ushort threshold)
{
ushort* pa = a.Data(0, 0);
byte* p = data;
for (int i = 0; i < width * height; i++)
{
if (*pa++ > threshold)
*p++ = 255;
else
*p++ = 0;
}
}
/// <summary>
/// n should be odd otherwise this function behaves as n = n+1
/// </summary>
/// <param name="a"></param>
/// <param name="n"></param>
public void BlurNxNNonZero(ShortImage a, int n)
{
if (n % 2 == 0)
{
n++;
}
ushort *input;
ushort *output;
ushort *[] pbs = new ushort *[n];
int[] s = new int[n];
int[] c = new int[n];
int h, hc;
int sumS, sumC;
int lastElement = n - 1;
for (int y = 0; y < (height - (n - 1)); y++)
{
input = a.Data((int)(n / 2), y + 1);
output = this.Data((int)(n / 2), y + 1);
for (int cnt = 0; cnt < n; cnt++)
{
pbs[cnt] = a.Data((n - 1), y + cnt);
s[cnt] = 0;
c[cnt] = 0;
}
h = 0;
hc = 0;
for (int x = 0; x < (width - (n - 1)); x++)
{
h -= s[0];
hc -= c[0];
int i = 0;
for (i = 0; i < (n - 1); i++)
{
s[i] = s[i + 1];
c[i] = c[i + 1];
}
sumS = 0;
sumC = 0;
for (i = 0; i < n; i++)
{
ushort bsi = *pbs[i];
sumC += ((bsi > 0) ? 1 : 0);
sumS += bsi;
pbs[i]++;
}
c[lastElement] = sumC;
s[lastElement] = sumS;
h += sumS;
hc += sumC;
int g = 0;
if (hc > 0)
{
g = h / hc;
}
//if (g > ushort.MaxValue)
// g = ushort.MaxValue;
if (*input++ != (ushort)0)
{
*output++ = (ushort)g;
}
else
{
*output++ = (ushort)0;
}
}
}
}
public void Blur5x5NonZero(ShortImage a)
{
ushort *input;
ushort *output;
ushort *pb04;
ushort *pb14;
ushort *pb24;
ushort *pb34;
ushort *pb44;
int s0, s1, s2, s3, s4; //pixel values
int c0, c1, c2, c3, c4; //valid pixel counts (where value > 0)
int h, hc;
for (int y = 0; y < height - 4; y++)
{
input = a.Data(2, y + 1);
output = this.Data(2, y + 1);
pb04 = a.Data(4, y);
pb14 = a.Data(4, y + 1);
pb24 = a.Data(4, y + 2);
pb34 = a.Data(4, y + 3);
pb44 = a.Data(4, y + 4);
h = 0;
hc = 0;
s0 = 0; s1 = 0; s2 = 0; s3 = 0; s4 = 0;
c0 = 0; c1 = 0; c2 = 0; c3 = 0; c4 = 0;
for (int x = 0; x < width - 4; x++)
{
h -= s0;
hc -= c0;
s0 = s1;
s1 = s2;
s2 = s3;
s3 = s4;
c0 = c1;
c1 = c2;
c2 = c3;
c3 = c4;
c4 = (((*pb04) > 0) ? 1 : 0) + (((*pb14) > 0) ? 1 : 0) + (((*pb24) > 0) ? 1 : 0) + (((*pb34) > 0) ? 1 : 0) + (((*pb44) > 0) ? 1 : 0);
s4 = *pb04++ + *pb14++ + *pb24++ + *pb34++ + *pb44++;
h += s4;
hc += c4;
int g = 0;
if (hc > 0)
{
g = h / hc;
}
//if (g > ushort.MaxValue)
// g = ushort.MaxValue;
if (*input++ != (ushort)0)
{
*output++ = (ushort)g;
}
else
{
*output++ = (ushort)0;
}
}
}
}
public void Blur3x3NonZero(ShortImage a)
{
ushort *input;
ushort *output;
ushort *pb02;
ushort *pb12;
ushort *pb22;
int s0, s1, s2; //pixel values
int c0, c1, c2; //valid pixel counts (where value > 0)
int h, hc;
for (int y = 0; y < height - 2; y++)
{
input = a.Data(1, y + 1);
output = this.Data(1, y + 1);
pb02 = a.Data(2, y);
pb12 = a.Data(2, y + 1);
pb22 = a.Data(2, y + 2);
h = 0;
hc = 0;
s0 = 0; s1 = 0; s2 = 0;
c0 = 0; c1 = 0; c2 = 0;
for (int x = 0; x < width - 2; x++)
{
h -= s0;
hc -= c0;
s0 = s1;
s1 = s2;
c0 = c1;
c1 = c2;
c2 = (((*pb02) > 0) ? 1 : 0) + (((*pb12) > 0) ? 1 : 0) + (((*pb22) > 0) ? 1 : 0);
s2 = *pb02++ + *pb12++ + *pb22++;
h += s2;
hc += c2;
int g = 0;
if (hc > 0)
{
g = h / hc;
}
if (g > ushort.MaxValue)
{
g = ushort.MaxValue;
}
if (*input++ != (ushort)0)
{
*output++ = (ushort)g; //comment this check if you want to fill holes and smooth edges
}
else
{
*output++ = (ushort)0;
}
}
}
}
// BEWARE: threshold on absdiff, and mask level settings*
public unsafe void Decode(ByteImage[] capturedImages, ShortImage decoded, ByteImage mask, int nBits, int max)
{
decoded.Zero();
int capturedWidth = decoded.Width;
int capturedHeight = decoded.Height;
// stores decoded bit from previous level
var bits = new ByteImage(capturedWidth, capturedHeight);
for (int i = 0; i < nBits; i++)
{
var capturedImage = capturedImages[2 * i];
var invertedCapturedImage = capturedImages[2 * i + 1];
int bitValue = (int)Math.Pow(2.0, nBits - i - 1);
ushort *decodedp = decoded.Data(0, 0);
byte * capturedp = capturedImage.Data(0, 0);
byte * invertedp = invertedCapturedImage.Data(0, 0);
byte * maskp = mask.Data(0, 0);
byte * bitsp = bits.Data(0, 0);
for (int y = 0; y < capturedHeight; y++)
{
for (int x = 0; x < capturedWidth; x++)
{
// a bit is considered valid if the diff is greater than some threshold; this value is tricky to set given AGC
byte valid = (Math.Abs(*capturedp - *invertedp) > 10) ? (byte)255 : (byte)0;
byte bit = (*capturedp >= *invertedp) ? (byte)255 : (byte)0;
// Gray code bit
*bitsp = (byte)(bit ^ *bitsp);
if (*bitsp == 255)
{
*decodedp = (ushort)(*decodedp + bitValue);
}
// stop updating the mask for the least significant levels (but continue decoding)
// *FIX: this is pretty fragile, perhaps better to record how many bits of rows and column have been recorded and walk back from that
if (i < nBits - 4)
{
*maskp = (byte)(valid & (*maskp));
}
decodedp++;
capturedp++;
invertedp++;
maskp++;
bitsp++;
}
}
}
bits.Dispose();
// check that decoded values are within range
for (int y = 0; y < capturedHeight; y++)
{
for (int x = 0; x < capturedWidth; x++)
{
int d = decoded[x, y]; // can this be negative?
if ((d >= max) || (d < 0))
{
mask[x, y] = 0;
}
}
}
}
public void Copy(ShortImage shortImage)
{
ushort* p = shortImage.Data(0, 0);
ARGB32* pOut = data;
for (int i = 0; i < width * height; i++)
{
pOut->A = 255;
pOut->R = (byte)*p;
pOut->G = (byte)*p;
pOut++->B = (byte)*p++;
}
}
// BEWARE: threshold on absdiff, and mask level settings*
public unsafe void Decode(ByteImage[] capturedImages, ShortImage decoded, ByteImage mask, int nBits, int max)
{
decoded.Zero();
int capturedWidth = decoded.Width;
int capturedHeight = decoded.Height;
// stores decoded bit from previous level
var bits = new ByteImage(capturedWidth, capturedHeight);
for (int i = 0; i < nBits; i++)
{
var capturedImage = capturedImages[2 * i];
var invertedCapturedImage = capturedImages[2 * i + 1];
int bitValue = (int)Math.Pow(2.0, nBits - i - 1);
ushort* decodedp = decoded.Data(0, 0);
byte* capturedp = capturedImage.Data(0, 0);
byte* invertedp = invertedCapturedImage.Data(0, 0);
byte* maskp = mask.Data(0, 0);
byte* bitsp = bits.Data(0, 0);
for (int y = 0; y < capturedHeight; y++)
for (int x = 0; x < capturedWidth; x++)
{
// a bit is considered valid if the diff is greater than some threshold; this value is tricky to set given AGC
byte valid = (Math.Abs(*capturedp - *invertedp) > 10) ? (byte)255 : (byte)0;
byte bit = (*capturedp >= *invertedp) ? (byte)255 : (byte)0;
// Gray code bit
*bitsp = (byte)(bit ^ *bitsp);
if (*bitsp == 255)
*decodedp = (ushort)(*decodedp + bitValue);
// stop updating the mask for the least significant levels (but continue decoding)
// *FIX: this is pretty fragile, perhaps better to record how many bits of rows and column have been recorded and walk back from that
if (i < nBits - 4)
*maskp = (byte)(valid & (*maskp));
decodedp++;
capturedp++;
invertedp++;
maskp++;
bitsp++;
}
}
bits.Dispose();
// check that decoded values are within range
for (int y = 0; y < capturedHeight; y++)
for (int x = 0; x < capturedWidth; x++)
{
int d = decoded[x, y]; // can this be negative?
if ((d >= max) || (d < 0))
mask[x, y] = 0;
}
}