cv::vector< double > getMomentMatchBasedProbs( const cv::vector< cv::vector< cv::Point > > &contours ) { cv::vector< double > probs( contours.size() ); for( int i = 0; i < contours.size(); i++ ) { probs[i] = 1.0 - fmin( matchShapes( contours[i], matchContour_, CV_CONTOURS_MATCH_I2, 0.0 ) / matchThreshold_, 1.0 ); } return( probs ); }
cv::vector< double > getSurfMatchBasedProbs( const cv::vector< cv::vector< cv::Point > > &contours, cv_bridge::CvImagePtr cvPtr ) { cv::vector< double > probs( contours.size() ); for( int i = 0; i < contours.size(); i++ ) { probs[i] = getSingleSurfProb( contours[i], cvPtr, i ); } return( probs ); }
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++; } }
bool VisualFeatureExtraction::cutFeatures(cv::vector<cv::KeyPoint> &kpts, cv::Mat &features, unsigned short maxFeats) const { // store hash values in a map std::map<size_t, unsigned int> keyp_hashes; cv::vector<cv::KeyPoint>::iterator itKeyp; cv::Mat sorted_features; unsigned int iLine = 0; for (itKeyp = kpts.begin(); itKeyp < kpts.end(); itKeyp++, iLine++) keyp_hashes[(*itKeyp).hash()] = iLine; // sort values according to the response std::sort(kpts.begin(), kpts.end(), greater_than_response()); // create a new descriptor matrix with the sorted keypoints sorted_features.create(0, features.cols, features.type()); sorted_features.reserve(features.rows); for (itKeyp = kpts.begin(); itKeyp < kpts.end(); itKeyp++) sorted_features.push_back(features.row(keyp_hashes[(*itKeyp).hash()])); features = sorted_features.clone(); // select the first maxFeats features if (kpts.size() > maxFeats) { vector<KeyPoint> cutKpts(kpts.begin(), kpts.begin() + maxFeats); kpts = cutKpts; features = features.rowRange(0, maxFeats).clone(); } return 0; }
void setWorldPoints(cv::vector<cv::vector<cv::Point3f> > &worldPoints, const cv::Size patternSize, const int &fileNum){ worldPoints.clear(); worldPoints.resize(fileNum); for(int i = 0; i < fileNum; i++ ) { for(int j = 0; j < patternSize.height; j++ ) for(int k = 0; k < patternSize.width; k++ ) worldPoints[i].push_back(cv::Point3f(k*g_squareSize, j*g_squareSize, 0)); } }
int GrayCodes::grayToDec(cv::vector<bool> gray)//convert a gray code sequence to a decimal number { int dec = 0; bool tmp = gray[0]; if(tmp) dec += (int) pow((float)2, int(gray.size() - 1)); for(int i = 1; i < gray.size(); i++){ tmp=Utilities::XOR(tmp,gray[i]); if(tmp) dec+= (int) pow((float)2,int (gray.size() - i - 1) ); } return dec; }
cv::vector<cv::DMatch> Matching::getSymetryMatches( const cv::vector<cv::DMatch> &matches1, const cv::vector<cv::DMatch> &matches2){ cv::vector<cv::DMatch> symmetryMatches; for(int i = 0; i < matches1.size(); i++){ for (int j = 0; j < matches2.size(); j++){ if(matches1[i].queryIdx == matches2[j].trainIdx && matches1[i].trainIdx == matches2[j].queryIdx ){ symmetryMatches.push_back(cv::DMatch(matches1[i].queryIdx, matches1[i].trainIdx, matches1[i].distance)); break; } } } return symmetryMatches; }
void AGPathDetection::detectPaths(cv::vector<cv::vector<AGImage>> &imagesMatrix) { this->testingMode = false; // this->testSelectedImage(imagesMatrix[1][0]); // this->testSelectedImage(imagesMatrix[1][0]); for (int x = 0; x < imagesMatrix.size(); x++) { for (int y = 0; y < imagesMatrix.front().size(); y++) { Mat skeleton; this->prepareForPathDetecting(imagesMatrix[x][y].image, skeleton); this->searchForPathsInImageUsingSkeleton(imagesMatrix[x][y], skeleton); this->testDetectedPathsInImage(imagesMatrix[x][y]); } } }
bool RemoveNoise::isEnoughHalfVector(cv::vector<cv::Point2f>& start) const { int xCenter = frameSize.width / 2; int count = 0; for(auto startIter=start.begin(); startIter!=start.end(); startIter++){ if((left? startIter->x <= xCenter: xCenter < startIter->x)){ count++; } } if(count < threshNum / 2) return false; return true; }
// 透視投影変換行列の推定 void calcProjectionMatrix(cv::vector<cv::Point3d>& op, cv::vector<cv::Point2d>& ip, cv::Mat& dst) { cv::Mat A; A.create(cv::Size(12, op.size()*2), CV_64FC1); for (int i = 0, j = 0; i < op.size()*2; i+=2, ++j) { A.at<double>(i, 0) = 0.0; A.at<double>(i, 1) = 0.0; A.at<double>(i, 2) = 0.0; A.at<double>(i, 3) = 0.0; A.at<double>(i, 4) = -op[j].x; A.at<double>(i, 5) = -op[j].y; A.at<double>(i, 6) = -op[j].z; A.at<double>(i, 7) = -1.0; A.at<double>(i, 8) = ip[j].y*op[j].x; A.at<double>(i, 9) = ip[j].y*op[j].y; A.at<double>(i, 10) = ip[j].y*op[j].z; A.at<double>(i, 11) = ip[j].y; A.at<double>(i+1, 0) = op[j].x; A.at<double>(i+1, 1) = op[j].y; A.at<double>(i+1, 2) = op[j].z; A.at<double>(i+1, 3) = 1.0; A.at<double>(i+1, 4) = 0.0; A.at<double>(i+1, 5) = 0.0; A.at<double>(i+1, 6) = 0.0; A.at<double>(i+1, 7) = 0.0; A.at<double>(i+1, 8) = -ip[j].x*op[j].x; A.at<double>(i+1, 9) = -ip[j].x*op[j].y; A.at<double>(i+1, 10) = -ip[j].x*op[j].z; A.at<double>(i+1, 11) = -ip[j].x; } cv::Mat pvect; cv::SVD::solveZ(A, pvect); cv::Mat pm(3, 4, CV_64FC1); for (int i = 0; i < 12; i++) { pm.at<double>(i/4, i%4) = pvect.at<double>( i ); } dst = pm; }
void drawDetections(const cv::vector<cv::Point2f>& detections, const cv::Scalar& color, cv::Mat image) { for (size_t i = 0; i < detections.size(); ++i) { circle(image, detections[i], 3, color, 1, 8, 0); } }
/** * Another function to validate ellipses. Based on having an error measure of * points of contours with respect to their respective position * on the fitted ellipse. */ bool MyEllipses::errorMeasureEllipse(cv::RotatedRect anEllipse, cv::vector<cv::Point> setOfPoints){ /* Equation of an ellipse (x-h)^2/a^2 + (y-k)^2/b^2 = 1 where, (h,k) are the coordinates of the center of the ellipse a is the length of the semi-major or minor axis b is the length of the semi-major or minor axis */ float h = anEllipse.center.x; float k = anEllipse.center.y; float a = anEllipse.size.width / 2; float b = anEllipse.size.height / 2; float diff = 0; int size = setOfPoints.size(); for( int i = 0; i < size; i++){ float xx = (float) (setOfPoints[i].x) - h; float yy = (float) (setOfPoints[i].y) - k; float common = a*b/(float) (sqrt((double) ((b*xx)*(b*xx) + (a*yy)*(a*yy)))); float xIntersection = xx*common; float yIntersection = yy*common; //distance = sqrt((xx-x)2 + (yy-y)2) float currentDiff = (float) sqrt((xx-xIntersection)*(xx-xIntersection) + (yy-yIntersection)*(yy-yIntersection)); diff = diff + currentDiff; } diff = diff/size; return diff < 100; }
bool RemoveNoise::isEnoughAllVector(cv::vector<cv::Point2f>& start) const { int count = (int) start.size(); if(count < threshNum) return false; return true; }
//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); } }
/* function AverageLine */ void lineAverage(cv::vector<cv::Vec2f> lines, cv::Mat& src) { float rho=0,theta=0; for( size_t i = 0; i < lines.size(); i++ ) { rho += lines[i][0];theta += lines[i][1]; } rho/=lines.size();theta/=lines.size(); cv::Point pt1, pt2; double a = cos(theta), b = sin(theta); double x0 = a*rho, y0 = b*rho; pt1.x = cvRound(x0 + 1000*(-b)); pt1.y = cvRound(y0 + 1000*(a)); pt2.x = cvRound(x0 - 1000*(-b)); pt2.y = cvRound(y0 - 1000*(a)); //cv::line( src, pt1, pt2, cv::Scalar(80,10,55), 3, CV_AA); float rho1=0,rho2=0,theta1=0,theta2=0; float i1=0,i2=0; for( size_t i = 0; i < lines.size(); i++ ) { if (lines[i][1]>theta) { rho1 += lines[i][0];theta1 += lines[i][1];i1++;} else { rho2 += lines[i][0];theta2 += lines[i][1];i2++;} } rho1/=i1;theta1/=i1; a = cos(theta1), b = sin(theta1); x0 = a*rho1, y0 = b*rho1; pt1.x = cvRound(x0 + 1000*(-b)); pt1.y = cvRound(y0 + 1000*(a)); pt2.x = cvRound(x0 - 1000*(-b)); pt2.y = cvRound(y0 - 1000*(a)); cv::line( src, pt1, pt2, cv::Scalar(0,100,255), 3, CV_AA); rho2/=i2;theta2/=i2; a = cos(theta2), b = sin(theta2); x0 = a*rho2, y0 = b*rho2; pt1.x = cvRound(x0 + 1000*(-b)); pt1.y = cvRound(y0 + 1000*(a)); pt2.x = cvRound(x0 - 1000*(-b)); pt2.y = cvRound(y0 - 1000*(a)); cv::line( src, pt1, pt2, cv::Scalar(0,100,0), 3, CV_AA); }
//ランダムに6点を抽出 void get_six_points(cv::vector<cv::Point2d>& calib_p, cv::vector<cv::Point3d>& calib_P, cv::vector<cv::Point2d>& src_p, cv::vector<cv::Point3d>& src_P) { int i=0; srand(time(NULL)); /* 乱数の初期化 */ cv::Vector<int> exists; while(i <= 6){ int maxValue = (int)src_p.size(); int v = rand() % maxValue; bool e2=false; for(int s=0; s<i; s++){ if(exists[s] == v) e2 = true; } if(!e2){ exists.push_back(v); calib_P.push_back(src_P[v]); calib_p.push_back(src_p[v]); i++; } } }
void loadImages(cv::vector<cv::Mat> &rgb, cv::vector<cv::Mat> &depth, const int &fileNum){ rgb.clear(); depth.clear(); for(int i = 0; i < fileNum; ++i){ stringstream rgbfilename, depthfilename; rgbfilename << FLAGS_folder <<"/" << FLAGS_color << i << FLAGS_type; depthfilename << FLAGS_folder << "/" << FLAGS_depth << i << FLAGS_type; cout << "loading : " << rgbfilename.str() << " and " << depthfilename.str() << endl; // load RGB image cv::Mat tempRGB = cv::imread(rgbfilename.str(), 0); rgb.push_back(tempRGB); // load depth image cv::Mat tempDepth = cv::imread(depthfilename.str(), CV_LOAD_IMAGE_ANYDEPTH); tempDepth.convertTo(tempDepth, CV_8U, 255.0/1000.0); // cv::Mat maxDist = cv::Mat::ones(tempDepth.rows, tempDepth.cols, CV_8U) * MAX_DEPTH; // cv::Mat minDist = cv::Mat::ones(tempDepth.rows, tempDepth.cols, CV_8U) * MIN_DEPTH; // cv::min(tempDepth, maxDist, tempDepth); // tempDepth -= minDist; cv::resize(tempDepth, tempDepth, cv::Size(), 2.0,2.0); cv::Mat roiTempDepth; cv::resize(tempDepth(cv::Rect(40, 43,498,498 / 4 * 3)), roiTempDepth, cv::Size(640, 480)); depth.push_back(roiTempDepth); std::cout << "loaded" << std::endl; cv::imshow("rgb",rgb[i]); cv::imshow("depth",depth[i]); cv::waitKey(100); } }
/** * A function that takes a list of ellipses with their respective quality and returns the best one. */ cv::RotatedRect MyEllipses::getBestEllipse(cv::vector<cv::RotatedRect> ellipses, cv::vector<double> qualityOfEllipses){ int size = ellipses.size(); std::cout<<size; double maxQuality = 0; int index = 0; for(int i = 0; i < size; i++){ if( qualityOfEllipses[i]>maxQuality ){ maxQuality = qualityOfEllipses[i]; index = i; } } return ellipses[index]; }
/* * Takes the vector of detected contours from and image and determines whether the points of a contour are clustered around an endpoint making the contour invalid. * Any contours deemed to be invalid are not included in the returned vector containing contours deemed valid * *@param contours - a vector containing all the detected contours *@param lines - a vector containing all detected straight lines * *@return - a vector of valid contours */ cv::vector< cv::vector<cv::Point> > ImageProcessor::removeRedundantContours(cv::vector< cv::vector<cv::Point> > & contours, cv::vector<cv::Vec4i> lines){ cv::vector< cv::vector<cv::Point> > valid_contours; //a vector to contain all contours that are determined to be valid for(int i = 0; i < (int)contours.size(); i++){ float invalid_point_count = 0.0f; //count of points in a contour that are clustering around an endpoint cv::vector<cv::Point> contour_vec = contours[i]; for(int k = 0; k < (int)contour_vec.size(); k++){ //compute the distance between each point in the contour and the endpoints of each straight line cv::Point point = contour_vec[k]; for(int j = 0; j < (int)lines.size(); j++){ //distances of countour point from each endpoint double dist1 = distance(point, cv::Point(lines[j][0], lines[j][1])); double dist2 = distance(point, cv::Point(lines[j][2], lines[j][3])); //distance between both endpoints of line[j] double endpoint_dist = distance(cv::Point(lines[j][0], lines[j][1]), cv::Point(lines[j][2], lines[j][3])); double contour_inLine_dist = dist1 + dist2; //if the distance between the contour point and a line endpoint, then increment the number of detected invalid points if(dist1 < MAX_DIST || dist2 < MAX_DIST || (contour_inLine_dist - endpoint_dist) < 1.0 ){ invalid_point_count++; break; } } } //compute the percentage of invalid points in the contour, current contour is valid and pushed onto back of valid_contour vector //if less than 10% of the points are found to be invalid. float invalid_percentage = invalid_point_count / (float) contour_vec.size(); if(invalid_percentage < PERCENTAGE){ valid_contours.push_back(contours[i]); } } return valid_contours; }
cv::vector< double > getAreaBasedProbs( const cv::vector< cv::vector< cv::Point > > &contours ) { int largestContour = -1; double maxArea = 0.0; cv::vector< double > probs( contours.size() ); for( int i = 0; i < contours.size(); i++ ) { probs[i] = cv::contourArea( contours[i] ); if( maxArea < probs[i] ) { maxArea = probs[i]; largestContour = i; } } for( int i = 0; i < contours.size(); i++ ) { probs[i] /= probs[largestContour]; } return( probs ); }
void SiftGPUWrapper::detect(const cv::Mat& image, cv::vector<cv::KeyPoint>& keypoints, std::vector<float>& descriptors, const Mat& mask) const { if (error) { keypoints.clear(); ROS_FATAL("SiftGPU cannot be used. Detection of keypoints failed"); } //get image cvMatToSiftGPU(image, data); int num_features = 0; SiftGPU::SiftKeypoint* keys = 0; ROS_DEBUG("SIFTGPU: cols: %d, rows: %d", image.cols, image.rows); if (siftgpu->RunSIFT(image.cols, image.rows, data, GL_LUMINANCE, GL_UNSIGNED_BYTE)) { num_features = siftgpu->GetFeatureNum(); ROS_INFO("Number of features found: %i", num_features); keys = new SiftGPU::SiftKeypoint[num_features]; descriptors.resize(128 * num_features); //descriptors = new float[128 * num_features]; siftgpu->GetFeatureVector(&keys[0], &descriptors[0]); } else { ROS_WARN("SIFTGPU->RunSIFT() failed!"); } //copy to opencv structure keypoints.clear(); for (int i = 0; i < num_features; ++i) { KeyPoint key(keys[i].x, keys[i].y, keys[i].s, keys[i].o); keypoints.push_back(key); } // FILE *fp = fopen("bla.pgm", "w"); // WritePGM(fp, data, image.cols, image.rows); // fclose(fp); }
void loadImages(cv::vector<cv::Mat> &lefts, cv::vector<cv::Mat> &rights, const int &fileNum) { cv::namedWindow("Left", CV_WINDOW_AUTOSIZE | CV_WINDOW_FREERATIO); cv::namedWindow("Right", CV_WINDOW_AUTOSIZE | CV_WINDOW_FREERATIO); lefts.clear(); rights.clear(); for (int i = 0; i < fileNum; i++) { std::stringstream lfile, rfile; lfile << FLAGS_dir << "/left_" << i << FLAGS_suffix; rfile << FLAGS_dir << "/right_" << i << FLAGS_suffix; std::cout << "load: " << lfile.str() << ", " << rfile.str() << std::endl; cv::Mat left = cv::imread(lfile.str(), 0); cv::Mat right = cv::imread(rfile.str(), 0); lefts.push_back(left); rights.push_back(right); cv::imshow("Left", left); cv::imshow("Right", right); cv::waitKey(100); } }
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(); }
/* * Convert the points that contain contour coordinates and store them in a Vec4i data type * *@param contours - vector containining contour information * *@return the contour information as a vector of Vec4i data types */ cv::vector<cv::Vec4i> ImageProcessor::pointsToVec4i(const cv::vector< cv::vector<cv::Point> > & contours){ cv::vector<cv::Vec4i> vector; for(int i = 0; i < (int)contours.size(); i++){ cv::vector<cv::Point> contour = contours[i]; for(int j = 1; j < (int)contour.size();j++){ cv::Point p1, p2; cv::Vec4i vec; p1 = contour[j - 1]; p2 = contour[j]; vec[0] = p1.x; vec[1] = p1.y; vec[2] = p2.x; vec[3] = p2.y; vector.push_back(vec); } } return vector; }
//元画像の特徴点と、再計算した特徴点の誤差を求める double inspection_error_value(cv::Mat& cameraMat, cv::vector<cv::Point3d>& P, cv::vector<cv::Point2d>& groundTruth) { if(cameraMat.cols != 4 || cameraMat.rows != 3){ return 0.0; } cv::vector<cv::Point2d> p; for(int i=0; i<(int)P.size(); i++){ double x = (cameraMat.at<double>(0,0)*P[i].x + cameraMat.at<double>(0,1)*P[i].y + cameraMat.at<double>(0,2)*P[i].z + cameraMat.at<double>(0,3)) / (cameraMat.at<double>(2,0)*P[i].x + cameraMat.at<double>(2,1)*P[i].y + cameraMat.at<double>(2,2)*P[i].z + cameraMat.at<double>(2,3)); double y = (cameraMat.at<double>(1,0)*P[i].x + cameraMat.at<double>(1,1)*P[i].y + cameraMat.at<double>(1,2)*P[i].z + cameraMat.at<double>(1,3)) / (cameraMat.at<double>(2,0)*P[i].x + cameraMat.at<double>(2,1)*P[i].y + cameraMat.at<double>(2,2)*P[i].z + cameraMat.at<double>(2,3)); p.push_back(cv::Point2d(x,y)); } double sum = 0.0; for(int i=0; i<(int)p.size(); i++){ double error = pow(pow(groundTruth[i].x-p[i].x,2.0)+pow(groundTruth[i].y-p[i].y,2.0),0.5); sum = sum + error; } return sum/(int)p.size(); }
int TrainingSet::getTrainingSet(cv::vector<cv::Mat> &images, cv::vector<int> &labels, cv::vector<QString> &names){ QStringList directoriesList = getAvailableFaces(); labels.clear(); images.clear(); names.clear(); for(int i=0; i<directoriesList.size(); i++){ QString person = directoriesList.at(i); QDir faceDirectory; faceDirectory.setPath(mainDirectory.path() + "/" + person); QStringList photosList = faceDirectory.entryList(QDir::Files); foreach(QString photo, photosList){ images.push_back(cv::imread(faceDirectory.path().toStdString() + "/" + photo.toStdString(), CV_LOAD_IMAGE_GRAYSCALE)); labels.push_back(i); } names.push_back(person); }
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(); }
cv::Mat MSFM::MSFMSurfaceO2(cv::Mat& image, cv::vector<cv::Point>& initials, cv::Point2d &h) { cv::Mat u_surface = MAX_VAL*cv::Mat::ones(image.rows, image.cols, CV_64FC1); cv::Mat state = cv::Mat::zeros(image.rows, image.cols, CV_8UC1); std::multimap<double, cv::Point> trial_set; std::map<int, std::multimap<double, cv::Point>::iterator> mapa_trial; std::multimap<double, cv::Point>::iterator trial_set_it; std::map<int, std::multimap<double, cv::Point>::iterator>::iterator mapa_trial_it; std::pair<double, cv::Point> pr_trial; std::pair<int, std::multimap<double, cv::Point>::iterator> pr_mapa; int key, i; cv::Point winner, neigh; // Initialization for (i = 0; i < (int) initials.size(); i++) { key = initials[i].y + image.rows*initials[i].x; if (mapa_trial.find(key) == mapa_trial.end()) { distance2M(initials[i]) = 0.0; state2M(initials[i]) = P_TRIAL; pr_trial = std::pair<double, cv::Point>(0.0, initials[i]); trial_set_it = trial_set.insert(pr_trial); pr_mapa = std::pair<int, std::multimap<double, cv::Point>::iterator>(key, trial_set_it); mapa_trial.insert(pr_mapa); } } // LOOP while (!trial_set.empty()) { trial_set_it = trial_set.begin(); key = trial_set_it->second.y + image.rows*trial_set_it->second.x; mapa_trial_it = mapa_trial.find(key); if (mapa_trial_it == mapa_trial.end()) { printf("ERROR: bad map alloc"); exit(-1); } if (mapa_trial_it->second != trial_set_it) { printf("ERROR: bad trial/map alloc"); exit(-1); } winner = trial_set_it->second; trial_set.erase(trial_set_it); mapa_trial.erase(mapa_trial_it); state2M(winner) = P_ALIVE; // UPWIND PROCEDURE for (int i=-1; i<2; i+=2) { neigh = cv::Point(winner.x + i, winner.y); if (contains2M(neigh)) this->StencilS1O2(image, u_surface, state, trial_set, mapa_trial, neigh, h); neigh = cv::Point(winner.x, winner.y + i); if (contains2M(neigh)) this->StencilS1O2(image, u_surface, state, trial_set, mapa_trial, neigh, h); neigh = cv::Point(winner.x + i, winner.y + i); if (contains2M(neigh)) this->StencilS2O2(image, u_surface, state, trial_set, mapa_trial, neigh, h); neigh = cv::Point(winner.x - i, winner.y + i); if (contains2M(neigh)) this->StencilS2O2(image, u_surface, state, trial_set, mapa_trial, neigh, h); } } return u_surface; }
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; }
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; }