public void Track(NPPImage_32fC1 imgTrack, NPPImage_32fC1 imgRef, NPPImage_32fC2 preShift, int i, float2 baseShiftRef, float baseRotationRef, float2 baseShifttoTrack, float baseRotationtoTrack, float threshold)
{
if (imgTrack.WidthRoi != imgRef.WidthRoi || imgTrack.HeightRoi != imgRef.HeightRoi ||
imgTrack.WidthRoi != currentWidth || imgTrack.HeightRoi != currentHeight)
{
throw new ArgumentOutOfRangeException();
}
int level = imgTrack.Width / imgTrack.WidthRoi;
convertToTilesBorder.RunSafe(imgRef, imgRefSortedTiles, currentTileSize, currentMaxShift, CurrentBlockCountX, CurrentBlockCountY, baseShiftRef, baseRotationRef); //template
forward[i].Exec(imgRefSortedTiles.DevicePointer, imgRefCplx.DevicePointer);
convertToTiles.RunSafe(imgTrack, imgToTrackSortedTiles, preShift, currentTileSize, currentMaxShift, CurrentBlockCountX, CurrentBlockCountY, baseShifttoTrack, baseRotationtoTrack); //image in paper
//DumpFloat(imgToTrackSortedTiles, currentTileSize + 2* currentMaxShift, currentTileSize + 2 * currentMaxShift, CurrentBlockCountX * CurrentBlockCountY, tileIdx, "tilesTrack_" + level + "_" + debugCallCounter + ".bin");
//DumpFloat(imgRefSortedTiles, currentTileSize + 2 * currentMaxShift, currentTileSize + 2 * currentMaxShift, CurrentBlockCountX * CurrentBlockCountY, tileIdx, "tilesRef_" + level + "_" + debugCallCounter + ".bin");
forward[i].Exec(imgToTrackSortedTiles.DevicePointer, imgToTrackCplx.DevicePointer);
conjKernel.RunSafe(imgRefCplx, imgToTrackCplx);
backward[i].Exec(imgToTrackCplx.DevicePointer, imgCrossCorrelation.DevicePointer);
imgCrossCorrelation.DivC(CurrentBlockSize * CurrentBlockSize);
squaredSumKernel.RunSafe(imgRefSortedTiles, squaredSumsOfTiles, currentMaxShift, currentTileSize, CurrentBlockCountX * CurrentBlockCountY);
//DumpFloat(squaredSumsOfTiles, 1, 1, CurrentBlockCountX * CurrentBlockCountY, tileIdx, "squaredSums_" + level + "_" + debugCallCounter + ".bin");
boxFilterXKernel.RunSafe(imgToTrackSortedTiles, imgRefSortedTiles, currentMaxShift, currentTileSize, CurrentBlockCountX * CurrentBlockCountY);
boxFilterYKernel.RunSafe(imgRefSortedTiles, imgToTrackSortedTiles, currentMaxShift, currentTileSize, CurrentBlockCountX * CurrentBlockCountY);
//DumpFloat(imgToTrackSortedTiles, currentTileSize + 2 * currentMaxShift, currentTileSize + 2 * currentMaxShift, CurrentBlockCountX * CurrentBlockCountY, tileIdx, "boxFilter_" + level + "_" + debugCallCounter + ".bin");
normalizedCCKernel.RunSafe(imgCrossCorrelation, squaredSumsOfTiles, imgToTrackSortedTiles, shiftImages, currentMaxShift, currentTileSize, CurrentBlockCountX * CurrentBlockCountY);
//DumpFloat(shiftImages, (2 * currentMaxShift + 1), (2 * currentMaxShift + 1), CurrentBlockCountX * CurrentBlockCountY, tileIdx, "tilesShift_" + level + "_" + debugCallCounter + ".bin");
patchShift.SetRoi(0, 0, CurrentBlockCountX, CurrentBlockCountY);
findMinimumKernel.RunSafe(shiftImages, patchShift, currentMaxShift, CurrentBlockCountX, CurrentBlockCountY, threshold);
NPPImage_32fC1 preShiftFloat = new NPPImage_32fC1(preShift.DevicePointer, 2 * CurrentBlockCountX, CurrentBlockCountY, preShift.Pitch);
NPPImage_32fC1 patchShiftFloat = new NPPImage_32fC1(patchShift.DevicePointer, 2 * CurrentBlockCountX, CurrentBlockCountY, patchShift.Pitch);
preShiftFloat.Add(patchShiftFloat);
debugCallCounter++;
}
public double4 ScanAngles(NPPImage_32fC1 img, double incr, double range, double zero)
{
NppiRect saveRoi = new NppiRect(img.PointRoi, img.SizeRoi);
NppiRect roi = new NppiRect();
roi.x = 0;
roi.y = 0;
roi.width = imgToTrackRotated.WidthRoi;
roi.height = imgToTrackRotated.HeightRoi;
img.SetRoi(roi);
double maxVal = -double.MaxValue;
double maxAng = 0;
double maxX = 0;
double maxY = 0;
//first perform a coarse search
for (double ang = zero - range; ang <= zero + range; ang += 5 * incr)
{
Matrix3x3 mat = Matrix3x3.RotAroundCenter(ang, imgToTrackRotated.Width, imgToTrackRotated.Height);
imgToTrackRotated.Set(0);
img.WarpAffine(imgToTrackRotated, mat.ToAffine(), InterpolationMode.Cubic);
forward.Exec(imgToTrackRotated.DevicePointerRoi, imgToTrackCplx.DevicePointer);
conjKernel.RunSafe(imgRefCplx, imgToTrackCplx);
backward.Exec(imgToTrackCplx.DevicePointer, imgToTrackRotated.DevicePointerRoi);
imgToTrackRotated.Div(imgToTrackRotated.WidthRoi * imgToTrackRotated.HeightRoi);
imgToTrackRotated.MaxIndex(val, x, y);
float v = val;
int hx = x;
int hy = y;
//Console.WriteLine("Found Max at " + ang.ToString("0.000") + " deg (" + hx + ", " + hy + ") = " + v);
if (v > maxVal)
{
maxVal = v;
maxAng = ang;
maxX = x;
maxY = y;
//Console.WriteLine("Max set!");
}
}
zero = maxAng;
range = 10 * incr;
//now perform a fine search but only around the previously found peak
for (double ang = zero - range; ang <= zero + range; ang += incr)
{
Matrix3x3 mat = Matrix3x3.RotAroundCenter(ang, imgToTrackRotated.Width, imgToTrackRotated.Height);
imgToTrackRotated.Set(0);
img.WarpAffine(imgToTrackRotated, mat.ToAffine(), InterpolationMode.Cubic);
int fftWidth = width / 2 + 1;
forward.Exec(imgToTrackRotated.DevicePointerRoi, imgToTrackCplx.DevicePointer);
conjKernel.RunSafe(imgRefCplx, imgToTrackCplx);
backward.Exec(imgToTrackCplx.DevicePointer, imgToTrackRotated.DevicePointerRoi);
imgToTrackRotated.Div(imgToTrackRotated.WidthRoi * imgToTrackRotated.HeightRoi);
imgToTrackRotated.MaxIndex(val, x, y);
float v = val;
int hx = x;
int hy = y;
if (v > maxVal)
{
maxVal = v;
maxAng = ang;
maxX = x;
maxY = y;
//Console.WriteLine("Found Max at " + ang.ToString("0.000") + " deg (" + hx + ", " + hy + ") = " + v);
//Console.WriteLine("Max set!");
}
}
if (maxX > imgToTrackRotated.WidthRoi / 2)
{
maxX -= imgToTrackRotated.WidthRoi;
}
if (maxY > imgToTrackRotated.HeightRoi / 2)
{
maxY -= imgToTrackRotated.HeightRoi;
}
img.SetRoi(saveRoi);
return(new double4(-maxX, -maxY, maxAng, maxVal));
}