C++ vector::size方法代码示例

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< 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 );
    }

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 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;
	}

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();
}

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++;
    }
}

/**
 * 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;
}

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);
    }
}

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;

}

bool RemoveNoise::isEnoughAllVector(cv::vector<cv::Point2f>& start) const
{
    int count = (int) start.size();
    
    if(count < threshNum) return false;
    
    return true;
}

  /* 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);
  }

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();
}

/**
 * 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];
}