void gridStartIterateFiltered(int32_t x, int32_t y, uint32_t radius, PointTree::Filter *filter, Condition const &condition)
{
if (filter == NULL)
{
gridPointTree->query(x, y, radius);
}
else
{
gridPointTree->query(*filter, x, y, radius);
}
PointTree::ResultVector::iterator w = gridPointTree->lastQueryResults.begin(), i;
for (i = w; i != gridPointTree->lastQueryResults.end(); ++i)
{
BASE_OBJECT *obj = static_cast<BASE_OBJECT *>(*i);
if (!condition.test(obj)) // Check if we should skip this object.
{
filter->erase(gridPointTree->lastFilteredQueryIndices[i - gridPointTree->lastQueryResults.begin()]); // Stop the object from appearing in future searches.
}
else if (isInRadius(obj->pos.x - x, obj->pos.y - y, radius)) // Check that search result is less than radius (since they can be up to a factor of sqrt(2) more).
{
*w = *i;
++w;
}
}
gridPointTree->lastQueryResults.erase(w, i); // Erase all points that were a bit too far.
gridPointTree->lastQueryResults.push_back(NULL); // NULL-terminate the result.
gridIterator = &gridPointTree->lastQueryResults[0];
/*
// In case you are curious.
debug(LOG_WARNING, "gridStartIterateFiltered(%d, %d, %u) found %u objects", x, y, radius, (unsigned)gridPointTree->lastQueryResults.size() - 1);
*/
}
void QRFinder::FindFinderPatterns(cv::Mat& inputImg, Rect regionOfInterest, vector<FinderPattern*> & finderPatterns, vector<Drawable*> & debugVector)
{
struct timespec start,end;
SET_TIME(&start);
//Get parameters from config
edgeThreshold = config->GetIntegerParameter("EdgeThreshold");
debugLevel = config->GetIntegerParameter("QR Debug Level");
int verticalResolution = config->GetIntegerParameter("YResolution");
nonMaxEnabled = config->GetBooleanParameter("EdgeNonMax");
minimumFinderPatternScore = config->GetIntegerParameter("MinimumFPScore");
detectorSize = config->GetIntegerParameter("DetectorSize");
int yBorder = detectorSize;
vector<Point3i> exclusionZones;
//Calculate limits
int maxColumn = regionOfInterest.x + regionOfInterest.width;
maxColumn -= detectorSize;
int maxRow = regionOfInterest.y + regionOfInterest.height;
int xStart = regionOfInterest.x;
int yStart = regionOfInterest.y;
xStart += detectorSize;
yStart += detectorSize;
maxColumn -= detectorSize;
maxRow -= detectorSize;
if (debugLevel > 0)
debugVector.push_back(new DebugRectangle(Rect(Point2i(xStart,yStart),Point2i(maxColumn,maxRow)),Colors::Aqua,1));
//Find horizontal edges
SET_TIME(&start);
FindEdgesClosed(inputImg,regionOfInterest,edgeArray,edgeThreshold,nonMaxEnabled,detectorSize);
SET_TIME(&end);
double edgeTime_local = calc_time_double(start,end);
edgeTime = (edgeTime + edgeTime_local)/2.0;
config->SetLabelValue("EdgeTime",(float)edgeTime/1000.0f);
//If debug level set, find all vertical edges and draw them
if (debugLevel <= -2)
{
for (int x = 1; x < verticalEdgeArray.rows-1; x ++)
{
FindEdgesVerticalClosed(inputImg,x);
const short * verticalEdgePtr = verticalEdgeArray.ptr<short>(x);
for (int y = 0; y < (verticalEdgeArray.cols);y++)
{
short transition = verticalEdgePtr[y];
if (transition < 0)
{
debugVector.push_back(new DebugCircle(Point2i(x,y),0,Colors::Fuchsia,true));
}
else if (transition > 0)
{
debugVector.push_back(new DebugCircle(Point2i(x,y),0,Colors::Cyan,true));
}
}
}
}
//END
int bw[6] = { 0 };
int k = 0;
//LOGV(LOGTAG_QR,"ImgSize=[%d,%d] EdgeSize=[%d,%d]",inputImg.rows,inputImg.cols,edgeArray.rows,edgeArray.cols);
int yDirection = 1;
int yCenter = yStart + (maxRow - yStart)/2;
int y = yStart, absOffset = 0;
LOGV(LOGTAG_QR,"Beginning search. y[%d->%d], Center=%d, x[%d->%d]",yStart,maxRow,yCenter,xStart,maxColumn);
while (y < maxRow && y >= yStart)
{
y = yCenter + absOffset * yDirection;
if (yDirection == 1) absOffset += verticalResolution; //Increment every other frame
yDirection = -yDirection; //Change direction every frame
k = 0;
bw[0] = bw[1] = bw[2] = bw[3] = bw[4] = bw[5] = 0;
const short * edgeRowPtr = edgeArray.ptr<short>(y);
for (int x = xStart; x < maxColumn; x++)
{
if (isInRadius(exclusionZones,Point2i(x,y)))
continue;
int transition = edgeRowPtr[x]; //getTransition(Mi,x,threshold);
if (k == 0) //Haven't found edge yet
{
if (transition < 0) /* Light->dark transistion */
{
k++;
}
}
else //Found at least one edge
{
if ((k & 1) == 1) //Counting dark
{
if (transition > 0) //dark to light
{
++k;
}
}
else //Counting light
{
if (transition < 0) //light to dark
{
++k;
}
}
}
if (k > 0) ++bw[k-1];
if (FP_DEBUG_ENABLED && (debugLevel == -1 || debugLevel == -2))
{
if (transition < 0)
debugVector.push_back(new DebugCircle(Point2i(x,y),0,Colors::Lime,true));
else if (transition > 0)
debugVector.push_back(new DebugCircle(Point2i(x,y),0,Colors::Yellow,true));
}
if (k == 6)
{
int result = 0;
result = CheckRatios(bw,NULL);
if (result == 1)
{
//LOGV(LOGTAG_QR,"Ratio check pass");
//Center based on initial ratio
float patternCenterWidth = (float)bw[2];
int tempXCenter = (x - bw[4] - bw[3]) - (int)round(patternCenterWidth/2.0f);
float xOffset = (patternCenterWidth/6.0f);
//y coordinate of center. If check fails, returns 0.
int tempYCenterArray[] = {0,0,0};
int * verticalPatternSizes[3];
for (int i = 0;i < 3;i++)
verticalPatternSizes[i] = new int[5];
tempYCenterArray[0] = FindCenterVertical(inputImg, tempXCenter, y, bw, debugVector,verticalPatternSizes[0]);
tempYCenterArray[1] = FindCenterVertical(inputImg, tempXCenter - xOffset, y, bw, debugVector,verticalPatternSizes[1]);
tempYCenterArray[2] = FindCenterVertical(inputImg, tempXCenter + xOffset, y, bw, debugVector,verticalPatternSizes[2]);
int tempYCenter = 0;
int passCount = 0;
float avgYSize = 0;
int averageVerticalSize[5] = {0,0,0,0,0};
for (int yTest = 0; yTest < 3; yTest++)
{
if (tempYCenterArray[yTest] > 0)
{
passCount++;
tempYCenter += tempYCenterArray[yTest];
for (int i=0;i<5;i++)
{
averageVerticalSize[i] += (verticalPatternSizes[yTest])[i];
avgYSize += (verticalPatternSizes[yTest])[i];
}
}
}
if (passCount >= 2)
{
//LOGV(LOGTAG_QR,"Vertical test pass-1");
tempYCenter = (int)round((float)tempYCenter / (float)passCount);
avgYSize = (float)avgYSize / (float)passCount;
int allowedVariance = (int)avgYSize >> 2;
bool yVarianceTest = true;
for (int yTest = 0; yTest < 3; yTest++)
{
if (tempYCenterArray[yTest] > 0)
{
if (abs(tempYCenterArray[yTest] - tempYCenter) > allowedVariance)
{
yVarianceTest = false;
break;
}
}
}
if (yVarianceTest)
{
//LOGV(LOGTAG_QR,"Vertical test pass-2. Passcount=%d",passCount);
//Average the vertical pattern sizes
for (int i=0;i<5;i++)
{
averageVerticalSize[i] = idiv(averageVerticalSize[i],passCount);
}
//LOGV(LOGTAG_QR,"Averaged sizes. Center=%d",averageVerticalSize[2]);
int tempXCenterArray[] = {0,0,0};
int xSizeArray[] = {0,0,0};
int yOffset = idiv(averageVerticalSize[2],6.0f);
//LOGV(LOGTAG_QR,"Yoffset=%d,yCenter=%d",yOffset,tempYCenter);
tempXCenterArray[0] = FindCenterHorizontal(tempXCenter, tempYCenter-yOffset, bw, xSizeArray[0], debugVector);
tempXCenterArray[1] = FindCenterHorizontal(tempXCenter, tempYCenter, bw, xSizeArray[1], debugVector);
tempXCenterArray[2] = FindCenterHorizontal(tempXCenter, tempYCenter+yOffset, bw, xSizeArray[2], debugVector);
tempXCenter = 0;
passCount = 0;
float avgXSize = 0;
for (int xTest = 0; xTest < 3; xTest++)
{
if (tempXCenterArray[xTest] > 0)
{
passCount++;
tempXCenter += tempXCenterArray[xTest];
avgXSize += xSizeArray[xTest];
}
}
if (passCount >= 2)
{
//LOGV(LOGTAG_QR,"Horizontal test pass");
tempXCenter = (int)round((float)tempXCenter / (float)passCount);
avgXSize = (float)avgXSize/(float)passCount;
//allowedVariance = (int)round((float)avgYSize/1.5f);
float aspectRatio = avgXSize/avgYSize;
if (aspectRatio > 0.33f && aspectRatio < 3.0f)
{
//LOGV(LOGTAG_QR,"Size test pass");
Point2i finderPatternCenter = Point2i(tempXCenter,tempYCenter); //Center of finder pattern
int finderPatternSize = MAX(avgXSize,avgYSize);
int fpRadius = (int)round((float)finderPatternSize/2.0f);
int fpRadiusExclude = ipow(finderPatternSize,2);
//LOGD(LOGTAG_QR,"Creating new pattern[%d,%d]",avgXSize,avgYSize);
//Create a new pattern
FinderPattern * newPattern = new FinderPattern(finderPatternCenter,Size2i(avgXSize,avgYSize));
Size2f patternSearchSize = Size2f(avgXSize,avgYSize);
vector<Point2i> corners;
struct timespec fastStart,fastEnd;
SET_TIME(&fastStart);
fastQRFinder->LocateFPCorners(inputImg,newPattern,corners,debugVector);
// fastQRFinder->CheckAlignmentPattern(inputImg,finderPatternCenter,patternSearchSize,corners,debugVector);
SET_TIME(&fastEnd);
double fastFPTime_Local = calc_time_double(fastStart,fastEnd);
fastFPTime += fastFPTime_Local;
if (corners.size() == 4)
{
//if (validatePattern(newPattern,finderPatterns))
//{
newPattern->patternCorners = corners;
exclusionZones.push_back(Point3i(finderPatternCenter.x,finderPatternCenter.y, fpRadiusExclude));
finderPatterns.push_back(newPattern);
if (FP_DEBUG_ENABLED && debugLevel > 0)
{
debugVector.push_back(new DebugCircle(finderPatternCenter,fpRadius,Colors::MediumSpringGreen,1,true));
for (int i=0;i<corners.size();i++)
{
if (FP_DEBUG_ENABLED && debugLevel > 0)
debugVector.push_back(new DebugCircle(corners[i],10,Colors::DodgerBlue,2));
}
}
//}
//else
//{
// //LOGV(LOGTAG_QR,"Compare check failed");
// if (FP_DEBUG_ENABLED && debugLevel > 0)
// debugVector.push_back(new DebugCircle(finderPatternCenter,fpRadius,Colors::HotPink,2));
//}
}
else
{
//LOGV(LOGTAG_QR,"FAST check failed");
if (FP_DEBUG_ENABLED && debugLevel > 0)
debugVector.push_back(new DebugCircle(finderPatternCenter,fpRadius,Colors::Red,2));
delete newPattern;
}
}
else
{
//LOGV(LOGTAG_QR,"Size check failed");
//Size check failed
if (FP_DEBUG_ENABLED && debugLevel > 1)
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,tempYCenter),13, Colors::HotPink,1));
}
}
else
{
//LOGV(LOGTAG_QR,"Horizontal check failed");
//Vertical check succeeded, but horizontal re-check failed
if (FP_DEBUG_ENABLED && debugLevel > 1)
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,tempYCenter),12, Colors::OrangeRed,1));
}
}
else
{
//LOGV(LOGTAG_QR,"Variance test failed. AllowedVariance = %d, yCenters = %d,%d,%d [avg=%d], AvgYSize=%d",allowedVariance,tempYCenterArray[0],tempYCenterArray[1],tempYCenterArray[2],tempYCenter,avgYSize);
//Vertical variance test failed
if (FP_DEBUG_ENABLED && debugLevel > 1)
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,tempYCenter),14, Colors::MediumSpringGreen,1));
}
} else
{
//Ratios were correct but vertical check failed
if (FP_DEBUG_ENABLED && debugLevel > 2)
{
if (tempYCenter == 0) //ratio fail
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,y),10,Colors::Aqua,1));
else if (tempYCenter == -1) //topcheck fail
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,y),10,Colors::Orange,1));
else if (tempYCenter == -2) //bottomcheck fail
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,y),10,Colors::Lime,1));
}
}
}