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++;
    }
}
示例#2
0
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;
}
示例#3
0
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();
}
示例#5
0
//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);
	}
}
示例#6
0
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;
}