void RemoveNoise::removeOutlier(cv::vector<cv::Point2f>& start, cv::vector<cv::Point2f>& end) const { float averageNorm = 0.0f; for(auto startIter=start.begin(),endIter=end.begin(); startIter!=start.end(); startIter++,endIter++) { averageNorm += cv::norm(*startIter - *endIter); } averageNorm /= start.size(); for(auto startIter=start.begin(), endIter=end.begin(); startIter!=start.end(); /* look at the end of for */) { if(cv::norm(*startIter - *endIter) > threshNorm * averageNorm){ startIter = start.erase(startIter); endIter = end.erase(endIter); continue; } startIter++, endIter++; } }
cv::vector<pocket> PointLocator::infer(cv::vector<cv::KeyPoint> orangeKeyPoints, cv::vector<cv::KeyPoint> greenKeyPoints, cv::vector<cv::KeyPoint> purpleKeyPoints, cv::vector<cv::KeyPoint> pinkKeyPoints) { //Define vector of pocket points to be passed cv::vector<pocket> pockets; //There should be a maximum of 2 points per colour. If there is more, reduce. //This depends on the quality of test video results. //Right now it just takes the first 2 points in vector to prevent crashes. //Takes only one point for orange and purple since they are side pockets //TODO if needed later. if (orangeKeyPoints.size() > 1){ orangeKeyPoints.erase(orangeKeyPoints.begin() + 1, orangeKeyPoints.end()); } if (greenKeyPoints.size() > 2){ greenKeyPoints.erase(greenKeyPoints.begin() + 2, greenKeyPoints.end()); } if (purpleKeyPoints.size() > 1){ purpleKeyPoints.erase(purpleKeyPoints.begin() + 1, purpleKeyPoints.end()); } if (pinkKeyPoints.size() > 3){ pinkKeyPoints.erase(pinkKeyPoints.begin() + 3, pinkKeyPoints.end()); } //Returns a vector of pocket type pockets = labelPockets(orangeKeyPoints, greenKeyPoints, purpleKeyPoints, pinkKeyPoints); return pockets; }
int findChessboard(cv::vector<cv::Mat> &rgb, cv::vector<cv::Mat> &depth, cv::vector<cv::vector<cv::vector<cv::Point2f> > > &imagePoints, const cv::Size patternSize, const int &fileNum){ for(int i = 0; i < rgb.size(); ++i){ cout << i << endl; if( cv::findChessboardCorners( rgb[i], patternSize, imagePoints[0][i], CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE ) && cv::findChessboardCorners( depth[i], patternSize, imagePoints[1][i] , CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE ) ) { std::cout << " ... All corners found." << std::endl; cv::cornerSubPix(rgb[i], imagePoints[0][i], cv::Size(11,11), cv::Size(-1,-1), cv::TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 30, 0.01)); cv::cornerSubPix(depth[i], imagePoints[1][i], cv::Size(11,11), cv::Size(-1,-1), cv::TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 30, 0.01)); // 検出点を描画する cv::drawChessboardCorners( rgb[i], patternSize, ( cv::Mat )( imagePoints[0][i] ), true ); cv::drawChessboardCorners( depth[i], patternSize, ( cv::Mat )( imagePoints[1][i] ), true ); cv::imshow( "rgb", rgb[i] ); cv::imshow("depth", depth[i]); cv::waitKey( 100 ); } else { std::cout << " ... at least 1 corner not found." << std::endl; rgb.erase(rgb.begin() + i); depth.erase(depth.begin() + i); imagePoints[0].erase(imagePoints[0].begin() + i); imagePoints[1].erase(imagePoints[1].begin() + i); cout << rgb.size() << endl;; // fileNum--; i--; cv::waitKey( 100 ); } } return rgb.size(); }
int findChessboards( cv::vector<cv::Mat> &lefts, cv::vector<cv::Mat> &rights, cv::vector<cv::vector<cv::vector<cv::Point2f>>> &imagePoints, const cv::Size patternSize, const int &fileNum) { for (size_t i = 0; i < lefts.size(); ++i) { if (cv::findChessboardCorners( lefts[i], patternSize, imagePoints[0][i], CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE) && cv::findChessboardCorners( rights[i], patternSize, imagePoints[1][i], CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE)) { cv::cornerSubPix( lefts[i], imagePoints[0][i], cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 30, 0.01)); cv::cornerSubPix( rights[i], imagePoints[1][i], cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 30, 0.01)); cv::drawChessboardCorners( lefts[i], patternSize, (cv::Mat)(imagePoints[0][i]), true); cv::drawChessboardCorners( rights[i], patternSize, (cv::Mat)(imagePoints[1][i]), true); cv::imshow("Left", lefts[i]); cv::imshow("Right", rights[i]); } else { std::cout << "cannot find all corners" << std::endl; lefts.erase(lefts.begin() + i); rights.erase(rights.begin() + i); imagePoints[0].erase(imagePoints[0].begin() + i); imagePoints[1].erase(imagePoints[1].begin() + i); i--; } cv::waitKey(100); } return lefts.size(); }
//Remove pink pocket from vector that is between 2 green points. void PointLocator::removePinkCandidate(cv::vector<cv::KeyPoint> &pinkKeyPoints, cv::KeyPoint firstPocket, cv::KeyPoint secondPocket){ //First check that there are actually pink pocket points if (!pinkKeyPoints.empty()){ float distance = -1; int min = 0; cv::KeyPoint middlePoint; middlePoint.pt.x = (firstPocket.pt.x + secondPocket.pt.x) / 2; middlePoint.pt.y = (firstPocket.pt.y + secondPocket.pt.y) / 2; for (int i = 0; i < pinkKeyPoints.size(); i++){ float newDistance = distBetweenKeyPoints(pinkKeyPoints[i], middlePoint); if ((distance + 1) < epsilon || newDistance < distance){ distance = newDistance; min = i; } } pinkKeyPoints.erase(pinkKeyPoints.begin() + min, pinkKeyPoints.begin() + min + 1); } }
cv::vector<pocket> PointLocator::labelPockets(cv::vector<cv::KeyPoint> orangeKeyPoints, cv::vector<cv::KeyPoint> greenKeyPoints, cv::vector<cv::KeyPoint> purpleKeyPoints, cv::vector<cv::KeyPoint> pinkKeyPoints){ //Define vector of pocket points to be passed cv::vector<pocket> pockets(4); int pocketCount = 0; int realPocketCount = 0; bool pinkTop = true; bool pinkLeft = true; bool pinkRight = true; if (greenKeyPoints.size() + orangeKeyPoints.size() + pinkKeyPoints.size() + purpleKeyPoints.size() >= 4){ defPerspective = true; } //Select green pockets: Case 1: 2 green pockets in view if (greenKeyPoints.size() == 2){ //Step 1: If only green pockets are seen, select destination locations based on their x values. if (orangeKeyPoints.size() == 0 && purpleKeyPoints.size() == 0){ if (greenKeyPoints[0].pt.x < greenKeyPoints[1].pt.x){ pockets[0].pocketPoints = greenKeyPoints[0]; pockets[1].pocketPoints = greenKeyPoints[1]; } else{ pockets[0].pocketPoints = greenKeyPoints[1]; pockets[1].pocketPoints = greenKeyPoints[0]; } } //Step 2: If green end pockets and if both purple and orange side pockets are in view //Is there more logic we can use to make sure this is right? Right now it is same as just orange pockets logic. else if (orangeKeyPoints.size() > 0 && purpleKeyPoints.size() > 0){ float distGreen0ToOrange = distBetweenKeyPoints(greenKeyPoints[0], orangeKeyPoints[0]); float distGreen1ToOrange = distBetweenKeyPoints(greenKeyPoints[1], orangeKeyPoints[0]); if (distGreen0ToOrange > distGreen1ToOrange){ pockets[0].pocketPoints = greenKeyPoints[0]; pockets[1].pocketPoints = greenKeyPoints[1]; } else{ pockets[0].pocketPoints = greenKeyPoints[1]; pockets[1].pocketPoints = greenKeyPoints[0]; } } //Step 3: If green end pockets and if only the orange side pocket is in view else if (orangeKeyPoints.size() > 0){ float distGreen0ToOrange = distBetweenKeyPoints(greenKeyPoints[0], orangeKeyPoints[0]); float distGreen1ToOrange = distBetweenKeyPoints(greenKeyPoints[1], orangeKeyPoints[0]); if (distGreen0ToOrange > distGreen1ToOrange){ pockets[0].pocketPoints = greenKeyPoints[0]; pockets[1].pocketPoints = greenKeyPoints[1]; } else{ pockets[0].pocketPoints = greenKeyPoints[1]; pockets[1].pocketPoints = greenKeyPoints[0]; } } //Step 4: If green end pockets and if only the purple side pocket is in view else if (purpleKeyPoints.size() > 0){ float distGreen0ToPurple = distBetweenKeyPoints(greenKeyPoints[0], purpleKeyPoints[0]); float distGreen1ToPurple = distBetweenKeyPoints(greenKeyPoints[1], purpleKeyPoints[0]); if (distGreen0ToPurple < distGreen1ToPurple){ pockets[0].pocketPoints = greenKeyPoints[0]; pockets[1].pocketPoints = greenKeyPoints[1]; } else{ pockets[0].pocketPoints = greenKeyPoints[1]; pockets[1].pocketPoints = greenKeyPoints[0]; } } //Removes pink keypoint candidate which is between green pockets. (Co-linear) removePinkCandidate(pinkKeyPoints, pockets[0].pocketPoints, pockets[1].pocketPoints); pinkTop = false; //Puts the pockets destination locations in since top left pocket will always be pockets[0] pockets[0].xLocation = xLeft; pockets[0].yLocation = yTop; pockets[1].xLocation = xRight; pockets[1].yLocation = yTop; //Updates Pocket count pocketCount = 2; realPocketCount = 2; } //Step 5: Select green pockets: Case 2: 1 green pocket in view if (greenKeyPoints.size() == 1){ pockets[0].pocketPoints = greenKeyPoints[0]; if (orangeKeyPoints.size() > 0 && purpleKeyPoints.size() > 0){ float distToOrange = distBetweenKeyPoints(greenKeyPoints[0], orangeKeyPoints[0]); float distToPurple = distBetweenKeyPoints(greenKeyPoints[0], purpleKeyPoints[0]); if (distToOrange < distToPurple){ pockets[0].xLocation = xRight; pockets[0].yLocation = yTop; } else{ pockets[0].xLocation = xLeft; pockets[0].yLocation = yTop; } } else if (orangeKeyPoints.size() > 0){ pockets[0].xLocation = xRight; pockets[0].yLocation = yTop; } else if (purpleKeyPoints.size() > 0){ pockets[0].xLocation = xLeft; pockets[0].yLocation = yTop; } //Updates Pocket count pocketCount = 1; realPocketCount = 1; } //Update orange and purple pockets after green pockets are in so we know that green pockets are first in vector. if (orangeKeyPoints.size() > 0){ pockets[pocketCount].pocketPoints = orangeKeyPoints[0]; pockets[pocketCount].xLocation = xRight; pockets[pocketCount].yLocation = yMid; pocketCount++; realPocketCount++; } if (purpleKeyPoints.size() > 0){ pockets[pocketCount].pocketPoints = purpleKeyPoints[0]; pockets[pocketCount].xLocation = xLeft; pockets[pocketCount].yLocation = yMid; pocketCount++; realPocketCount++; } //Removes pink candidates between green and orange and pink pockets if (greenKeyPoints.size() == 2){ if (orangeKeyPoints.size() > 0){ removePinkCandidate(pinkKeyPoints, pockets[1].pocketPoints, orangeKeyPoints[0]); pinkRight = false; } if (purpleKeyPoints.size() > 0){ removePinkCandidate(pinkKeyPoints, pockets[0].pocketPoints, purpleKeyPoints[0]); pinkLeft = false; } } else if (greenKeyPoints.size() == 1){ int removeLocation = 0; if (orangeKeyPoints.size() > 0 && purpleKeyPoints.size() > 0){ float distToOrange = distBetweenKeyPoints(orangeKeyPoints[0], pockets[0].pocketPoints); float distToPurple = distBetweenKeyPoints(purpleKeyPoints[0], pockets[0].pocketPoints); if (distToOrange > distToPurple){ removeLocation = 2; } else{ removeLocation = 1; } } else if (orangeKeyPoints.size() > 0 && (removeLocation == 0 || removeLocation == 1)){ removePinkCandidate(pinkKeyPoints, pockets[0].pocketPoints, orangeKeyPoints[0]); pinkRight = false; } if (purpleKeyPoints.size() > 0 && (removeLocation == 0 || removeLocation == 2)){ removePinkCandidate(pinkKeyPoints, pockets[0].pocketPoints, purpleKeyPoints[0]); pinkLeft = false; } } //Adds pink pockets to list of pockets based on other pockets identified. while (!pinkKeyPoints.empty() && pockets[3].xLocation == NULL && pocketCount < 4){ //Find the pink marker closest to the first pocket in list. //It is structured so this is always the right marker to choose because of elimination of markers from candidate list. float distance = -1; int min = 0; for (int i = 0; i < pinkKeyPoints.size(); i++){ float newDistance = distBetweenKeyPoints(pinkKeyPoints[i], pockets[0].pocketPoints); if ((distance + 1) < epsilon || newDistance < distance){ distance = newDistance; min = i; } } pockets[pocketCount].pocketPoints = pinkKeyPoints[min]; // if (pinkTop){ pockets[pocketCount].xLocation = xMid; pockets[pocketCount].yLocation = yTop; pocketCount++; pinkTop = false; } else if (pinkLeft){ pockets[pocketCount].xLocation = xLeft; pockets[pocketCount].yLocation = yMidTop; pocketCount++; pinkLeft = false; } else if (pinkRight){ pockets[pocketCount].xLocation = xRight; pockets[pocketCount].yLocation = yMidTop; pocketCount++; pinkRight = false; } //Remove pink marker from candidate list pinkKeyPoints.erase(pinkKeyPoints.begin() + min, pinkKeyPoints.begin() + min + 1); } //Use the pink marker furthest to the left /*if ((pocketCount == 2 || pocketCount == 3) && !pinkKeyPoints.empty()){ //Determine which pink side marker is being used. //Should be marker closest along line between first two pockets. float distance = -1; int min = 0; cv::Vec2f line = lineEqn(pockets[0].pocketPoints.pt.x, pockets[0].pocketPoints.pt.y, pockets[1].pocketPoints.pt.x, pockets[1].pocketPoints.pt.y); for (int i = 0; i < pinkKeyPoints.size(); i++){ float newDistance = pinkKeyPoints[i].pt.x; if ((distance + 1) < epsilon || newDistance < distance){ distance = newDistance; min = i; } } pockets[pocketCount].pocketPoints = pinkKeyPoints[min]; pockets[pocketCount].xLocation = xLeft; pockets[pocketCount].yLocation = yMidTop; pinkKeyPoints.erase(pinkKeyPoints.begin() + min, pinkKeyPoints.begin() + min + 1); pocketCount++; }*/ //If 2 or 3 pockets are picked up, use any pink side marker /*if (pocketCount == 2 || pocketCount == 3){ //Determine which pink side marker is being used. //Should be marker closest along line between first two pockets. float distance = -1; int min = 0; cv::Vec2f line = lineEqn(pockets[0].pocketPoints.pt.x, pockets[0].pocketPoints.pt.y, pockets[1].pocketPoints.pt.x, pockets[1].pocketPoints.pt.y); for (int i = 0; i < pinkKeyPoints.size(); i++){ float newDistance = distPointToLine(pinkKeyPoints[i].pt.x, pinkKeyPoints[i].pt.y, line); if ((distance + 1) < epsilon || newDistance < distance){ distance = newDistance; min = i; } } pockets[pocketCount].pocketPoints = pinkKeyPoints[min]; pockets[pocketCount].xLocation = (pockets[0].xLocation + pockets[1].xLocation) / 2; pockets[pocketCount].yLocation = (pockets[0].yLocation + pockets[1].yLocation) / 2; pocketCount++; }*/ //If 2 pockets are picked up, use a pink marker not linearly dependent with the pockets. //This is accomplished by finding the pink marker furthest from the line. /*if (realPocketCount == 2){ //Determine which pink side marker is being used. //Should be marker furthest along line between pockets. float distance = 0; int max = 0; cv::Vec2f line = lineEqn(pockets[0].pocketPoints.pt.x, pockets[0].pocketPoints.pt.y, pockets[1].pocketPoints.pt.x, pockets[1].pocketPoints.pt.y); for (int i = 0; i < pinkKeyPoints.size(); i++){ float newDistance = distPointToLine(pinkKeyPoints[i].pt.x, pinkKeyPoints[i].pt.y, line); if ( newDistance > distance){ distance = newDistance; max = i; } pockets[pocketCount].pocketPoints = pinkKeyPoints[max]; //Remove pink Keypoint so it doesn't get used as 4th point in the transform. if (!pinkKeyPoints.empty()){ pinkKeyPoints.erase(pinkKeyPoints.begin() + max, pinkKeyPoints.begin() + max + 1); } pocketCount++; //Need to determine coordinates for point in perspective transform addNonLinearPointLocation(pockets); } }*/ /*//If 3 pockets are picked up, use any pink side marker if (pocketCount == 3){ //Determine which pink side marker is being used. //Should be marker closest along line between first two pockets. float distance = -1; int min = 0; cv::Vec2f line = lineEqn(pockets[0].pocketPoints.pt.x, pockets[0].pocketPoints.pt.y, pockets[1].pocketPoints.pt.x, pockets[1].pocketPoints.pt.y); for (int i = 0; i < pinkKeyPoints.size(); i++){ float newDistance = distPointToLine(pinkKeyPoints[i].pt.x, pinkKeyPoints[i].pt.y, line); if ((distance + 1) < epsilon || newDistance < distance){ distance = newDistance; min = i; } pockets[pocketCount].pocketPoints = pinkKeyPoints[min]; if (!pinkKeyPoints.empty()){ pinkKeyPoints.erase(pinkKeyPoints.begin() + min); } //Need to determine coordinates for point in perspective transform //First calculate distance from both known pocket points float distToPocket0 = distBetweenKeyPoints(pockets[0].pocketPoints, pockets[pocketCount - 1].pocketPoints); float distToPocket1 = distBetweenKeyPoints(pockets[1].pocketPoints, pockets[pocketCount - 1].pocketPoints); //For the case where both pockets are top pockets, pink pocket must be directly below these. if (inferPurple && inferOrange){ pockets[pocketCount].yLocation = yMidTop; if (distToPocket0 < distToPocket1) pockets[pocketCount].xLocation = xLeft; else pockets[pocketCount].xLocation = xRight; } if (!inferOrange){ if (distToPocket0 < distToPocket1){ pockets[pocketCount].xLocation = xMid; pockets[pocketCount].yLocation = yTop; } else{ //May get here in test video on accident //Then logic is broken. pockets[pocketCount].xLocation = xRight; pockets[pocketCount].yLocation = yMidBot; } } if (!inferPurple){ if (distToPocket0 < distToPocket1){ pockets[pocketCount].xLocation = xMid; pockets[pocketCount].yLocation = yTop; } else{ //Should never get here in test video pockets[pocketCount].xLocation = xRight; pockets[pocketCount].yLocation = yMidBot; } } //Increase pocket count once all locations are set. pocketCount++; } }*/ /*while (pockets.size() >= 2 && pockets.size() < 4 && !pinkKeyPoints.empty()){ if (pinkKeyPoints.size() > 0){ int i = 0; float distance = -1; else{ //Find equation for line for (int j = 0; j < pinkKeyPoints.size(); j++){ float newDistance = sqrt(); if (distance == 0 || newDistance < distance){ } } pockets[pocketCount] = pinkKeyPoints(i); if (!pinkKeyPoints.empty()){ pinkKeyPoints.erase(pinkKeyPoints.begin() + i); } pocketCount++; } } }*/ /*if (pocketCount == 3){ cv::KeyPoint tempPoint = cv::KeyPoint(); tempPoint.pt.x = (pockets[0].pocketPoints.pt.x + pockets[1].pocketPoints.pt.x) / 2; tempPoint.pt.y = (pockets[0].pocketPoints.pt.y + pockets[1].pocketPoints.pt.y) / 2; pockets[pocketCount].pocketPoints = tempPoint; pockets[pocketCount].xLocation = (pockets[0].xLocation + pockets[1].xLocation) / 2; pockets[pocketCount].yLocation = (pockets[0].yLocation + pockets[1].yLocation) / 2; pocketCount++; }*/ return pockets; }