C++ Mat::copyTo方法代码示例

//----------------------------------------------------------------------------------------------------------
void RunTracking::trackFilteredObject(TrackedPiece &piece, Mat &cameraFeed, Mat &threshold_image){

	vector <TrackedPiece> pieces;

	Mat temp;
	threshold_image.copyTo(temp);
	//these two vectors needed for output of findContours
	vector< vector<Point> > contours;
	vector<Vec4i> hierarchy;
	//find contours of filtered image using openCV findContours function
	findContours(temp,contours,hierarchy,CV_RETR_CCOMP,CV_CHAIN_APPROX_SIMPLE );
	//use moments method to find our filtered object
	bool objectFound = false;
	if (hierarchy.size() > 0) {
		int numObjects = hierarchy.size();
//		cout << "Num objects: " << numObjects << endl;
//		cout << "Max Num objects: " << MAX_NUM_OBJECTS << endl;
		// threshholed to calculate movement
		const int thresh = 40;
		//saves max area of each contour detected so only the largest one will be tracked
		double maxArea = 0;
		// temporary piece for contours found
		TrackedPiece tmp;

		//if number of objects greater than MAX_NUM_OBJECTS we have a noisy filter
		if(numObjects < MAX_NUM_OBJECTS){
			// for each object (contour) detected
			for (int index = 0; index >= 0; index = hierarchy[index][0]) {

				// get the moment of the contour
				Moments moment = moments((cv::Mat)contours[index]);
				// get the area from the moment
				double area = moment.m00;
//				cout << "Area " << index << " is: " << area << endl;
				
				// if the area is less than MIN_OBJECT_AREA then it is probably just noise
				// it must also be large than the max area found so far since we only want the largest area.
				if(area > MIN_OBJECT_AREA && area > maxArea){
					// set new max area
					maxArea = area;
					// Clear previous objects found so only one (the biggest) is detected
					pieces.clear();

					int xPos = moment.m10/area;
					int yPos = moment.m01/area;
					
					tmp.setXPos(xPos);
					tmp.setYPos(yPos);
					tmp.setName(piece.getName());
					tmp.setColor(piece.getColor());

					//cout << piece.getName() << ": x: " << xPos << " y: " << yPos << endl;
					//cout << "LastPos: x: " << piece.getLastxPos() << " y: " << piece.getLastyPos() << endl;

					pieces.push_back(tmp);

					objectFound = true;
				}
			}

			//let user know you found an object and check for movement
			if(objectFound ==true){

				// Update piece location (tmp piece should now be biggest contour found)
				piece.setXPos(tmp.getXPos());
				piece.setYPos(tmp.getYPos());

				/*
				 * Movement checking moved to timerTick
				 *
				// Check for movement (tmp piece should now be biggest contour found)
				if(tmp.getXPos() > (piece.getLastxPos() + thresh) || tmp.getXPos() < (piece.getLastxPos() - thresh))
				{
					piece.setLastxPos(tmp.getXPos());
					cout << piece.getName() << ": X movement" << endl;
				}
				if(tmp.getYPos() > (piece.getLastyPos() + thresh) || tmp.getYPos() < (piece.getLastyPos() - thresh))
				{
					piece.setLastyPos(tmp.getYPos());
					cout << piece.getName() << ": Y movement." << endl;
				}
				*/

				//draw object location on screen
				drawObject(pieces,cameraFeed);}

		}else putText(cameraFeed,"TOO MUCH NOISE! ADJUST FILTER",Point(0,50),1,2,Scalar(0,0,255),2);
	}
}

int main( int argc, char** argv ){

	//capture the video from web cam
	VideoCapture cap(0);
 
	// if not success, exit program
	if ( !cap.isOpened() ){  
        	cout << "Cannot open the web cam" << endl;
        	return -1;
	}

	//set height and width of capture frame
	cap.set(CV_CAP_PROP_FRAME_WIDTH,FRAME_WIDTH);
	cap.set(CV_CAP_PROP_FRAME_HEIGHT,FRAME_HEIGHT);

	//create a window called "Control"
	namedWindow("Control", CV_WINDOW_AUTOSIZE);

	//Create trackbars in "Control" window

	cvCreateTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179)
	cvCreateTrackbar("HighH", "Control", &iHighH, 179);

	cvCreateTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255)
 	cvCreateTrackbar("HighS", "Control", &iHighS, 255);

 	cvCreateTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255)
 	cvCreateTrackbar("HighV", "Control", &iHighV, 255);


    	while (true){

        	Mat imgOriginal;

        	bool bSuccess = cap.read(imgOriginal); // read a new frame from video

         	if (!bSuccess){ //if not success, break loop
             		cout << "Cannot read a frame from video stream" << endl;
             		break;
        	}

		//Convert the captured frame from BGR to HSV
  		Mat imgHSV;
  		cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV); 
 
		//Threshold the image
		Mat imgThresholded;

  		inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded);

  		//morphological opening (remove small objects from the foreground)
  		erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
  		dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); 

  		//morphological closing (fill small holes in the foreground)
  		dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); 
  		erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );

		//these two vectors needed for output of findContours
		vector< vector<Point> > contours;
		vector<Vec4i> hierarchy;

		Mat imgContour;
		imgThresholded.copyTo(imgContour);

		//find contours of filtered image using openCV findContours function
		findContours(imgContour,contours,hierarchy,CV_RETR_CCOMP,CV_CHAIN_APPROX_SIMPLE );
		
		//use moments method to find our filtered object
		double refArea = 0;
		if (hierarchy.size() > 0) {
			int numObjects = hierarchy.size();
			for (int index = 0; index >= 0; index = hierarchy[index][0]) {
				Moments moment = moments((cv::Mat)contours[index]);
				double area = moment.m00;
				if(area>MIN_OBJECT_AREA){ //jika area kontur lebih besar dari minimum area object maka gambar lingkaran dan tulis koordinat
					double x = moment.m10/area;
					double y = moment.m01/area;
					//double r = sqrt(area/3.14); //jari2 lingkaran
					double keliling = arcLength( contours[index], true );

					drawContours( imgOriginal, contours, index, Scalar(0,0,255), 1, 8, hierarchy);
					
					putText(imgOriginal, intToString(x) + "," + intToString(y), Point(x,y+20), FONT_HERSHEY_COMPLEX, 0.5, Scalar(0, 255, 0), 1, 8);
					putText(imgOriginal, "Luas: " + intToString(area), Point(x,y+40), FONT_HERSHEY_COMPLEX, 0.5, Scalar(0, 255, 0), 1, 8);
					putText(imgOriginal, "Keliling: " + intToString(keliling), Point(x,y+60), FONT_HERSHEY_COMPLEX, 0.5, Scalar(0, 255, 0), 1, 8);
						
				}//end if
			}//end for
		}//end if
		
		//imshow("Contour", imgContour);

		//show the thresholded image
  		imshow("Thresholded Image", imgThresholded);

		//show the original image 
  		imshow("Original", imgOriginal);

        	if (waitKey(5) == 27) {//wait for 'esc' key press for 30ms. If 'esc' key is pressed, break loop
//.........这里部分代码省略.........

//--------------------------------------【main( )函数】-----------------------------------------
//          描述：控制台应用程序的入口函数，我们的程序从这里开始执行
//-------------------------------------------------------------------------------------------------
int main( )
{

	//【1】以灰度模式读取原始图像并显示
	Mat srcImage = imread("1.jpg", 0);
	if(!srcImage.data ) { printf("读取图片错误，请确定目录下是否有imread函数指定图片存在~！ \n"); return false; } 
	imshow("原始图像" , srcImage);   

	ShowHelpText();

	//【2】将输入图像延扩到最佳的尺寸，边界用0补充
	int m = getOptimalDFTSize( srcImage.rows );
	int n = getOptimalDFTSize( srcImage.cols ); 
	//将添加的像素初始化为0.
	Mat padded;  
	copyMakeBorder(srcImage, padded, 0, m - srcImage.rows, 0, n - srcImage.cols, BORDER_CONSTANT, Scalar::all(0));

	//【3】为傅立叶变换的结果(实部和虚部)分配存储空间。
	//将planes数组组合合并成一个多通道的数组complexI
	Mat planes[] = {Mat_<float>(padded), Mat::zeros(padded.size(), CV_32F)};
	Mat complexI;
	merge(planes, 2, complexI);         

	//【4】进行就地离散傅里叶变换
	dft(complexI, complexI);           

	//【5】将复数转换为幅值，即=> log(1 + sqrt(Re(DFT(I))^2 + Im(DFT(I))^2))
	split(complexI, planes); // 将多通道数组complexI分离成几个单通道数组，planes[0] = Re(DFT(I), planes[1] = Im(DFT(I))
	magnitude(planes[0], planes[1], planes[0]);// planes[0] = magnitude  
	Mat magnitudeImage = planes[0];

	//【6】进行对数尺度(logarithmic scale)缩放
	magnitudeImage += Scalar::all(1);
	log(magnitudeImage, magnitudeImage);//求自然对数

	//【7】剪切和重分布幅度图象限
	//若有奇数行或奇数列，进行频谱裁剪      
	magnitudeImage = magnitudeImage(Rect(0, 0, magnitudeImage.cols & -2, magnitudeImage.rows & -2));
	//重新排列傅立叶图像中的象限，使得原点位于图像中心  
	int cx = magnitudeImage.cols/2;
	int cy = magnitudeImage.rows/2;
	Mat q0(magnitudeImage, Rect(0, 0, cx, cy));   // ROI区域的左上
	Mat q1(magnitudeImage, Rect(cx, 0, cx, cy));  // ROI区域的右上
	Mat q2(magnitudeImage, Rect(0, cy, cx, cy));  // ROI区域的左下
	Mat q3(magnitudeImage, Rect(cx, cy, cx, cy)); // ROI区域的右下
	//交换象限（左上与右下进行交换）
	Mat tmp;                           
	q0.copyTo(tmp);
	q3.copyTo(q0);
	tmp.copyTo(q3);
	//交换象限（右上与左下进行交换）
	q1.copyTo(tmp);                 
	q2.copyTo(q1);
	tmp.copyTo(q2);

	//【8】归一化，用0到1之间的浮点值将矩阵变换为可视的图像格式
	normalize(magnitudeImage, magnitudeImage, 0, 1, CV_MINMAX); 

	//【9】显示效果图
	imshow("频谱幅值", magnitudeImage);    
	waitKey();

	return 0;
}

void findFeaturePoint(Mat src, vector<Point>& point, vector<Rect>& rect_res,
		int FLAG) {
	LOGD_ERR("findFeaturePoint Enter");
	char name[10];
	sprintf(name, "Step %d", FLAG);
	Mat src_mask = Mat(src.size(), CV_8U);
	Mat thresholdRes = Mat(src.size(), CV_8U);
	Mat thresholdResCopy = Mat(src.size(), CV_8U);
	eclipseMask(src, src_mask);
	if (FLAG)
		equalizeLeftAndRightHalves(src_mask);

//imshow("After Equalize", src_mask);
	double minVal = 0;
	double maxVal = 0;

	minMaxLoc(src_mask, &minVal, &maxVal, NULL, NULL);
//threshold(src, threRes, maxVal-10, 255, THRESH_BINARY);
	threshold(src_mask, thresholdRes, minVal + 7.5, 255, THRESH_BINARY);
	thresholdRes.copyTo(thresholdResCopy);
//LOGD_ERR("Threshold Over");
//imshow(name, thresholdRes);
	vector<Vec4i> hierarchy;
	vector < vector<Point2i> > contours;
	findContours(thresholdResCopy.rowRange(0, thresholdRes.rows / 2), contours,
			hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
//LOGD_ERR("findContours Over");
	sort(contours.begin(), contours.end(), my_cmp);
//LOGD_ERR("sort Over");
//cout << contours.size() << endl;
//LOGD_ERR(contours.size());
	Rect rect;
	if (contours.size() < 2)
		return;
	switch (FLAG) {
	case 0:
		for (int i = 0; i < 2; i++) {

			rect = boundingRect(contours[i]);

			point.push_back(
					Point((rect.tl().x + rect.br().x) * 0.5,
							(rect.tl().y + rect.br().y) * 0.5));
			rect_res.push_back(rect);
			//rectangle(src, rect, M_RED);

		}
		break;
	case 1:
		Mat subThreshold;
		int MAX_RIGHT_I = 0, MAX_RIGHT_J = 0;
		int MAX_LEFT_I = 1000, MAX_LEFT_J = 1000;
		float curV;
		for (int k = 0; k < 2; k++) {

			rect = boundingRect(contours[k]);
			subThreshold =
					thresholdRes.colRange(rect.tl().x, rect.br().x).rowRange(
							rect.tl().y, rect.br().y);
			//imshow("SSS",subThreshold);
			for (int i = 0; i < subThreshold.size().height; i++)
				for (int j = 0; j < subThreshold.size().width; j++) {

					try {
						curV = subThreshold.data[i * subThreshold.size().width
								* subThreshold.channels() + j];
					} catch (cv::Exception& e) {
						cout << e.what() << endl;
					}

					if (curV <= 10) {
						if (j >= MAX_RIGHT_J) {

							MAX_RIGHT_I = i;
							MAX_RIGHT_J = j;
						}

						if (j <= MAX_LEFT_J) {

							MAX_LEFT_I = i;
							MAX_LEFT_J = j;

						}
					}

				}
			point.push_back(
					Point(MAX_LEFT_J + rect.tl().x, MAX_LEFT_I + rect.tl().y));
			point.push_back(
					Point(MAX_RIGHT_J + rect.tl().x,
							MAX_RIGHT_I + rect.tl().y));
			rect_res.push_back(rect);
			//rectangle(src, rect, M_RED);
		}
		break;
	}
	LOGD_ERR("findFeaturePoint Exit");
}

    void CV_BilateralFilterTest::reference_bilateral_filter(const Mat &src, Mat &dst, int d,
        double sigma_color, double sigma_space, int borderType)
    {
        int cn = src.channels();
        int i, j, k, maxk, radius;
        double minValSrc = -1, maxValSrc = 1;
        const int kExpNumBinsPerChannel = 1 << 12;
        int kExpNumBins = 0;
        float lastExpVal = 1.f;
        float len, scale_index;
        Size size = src.size();

        dst.create(size, src.type());

        CV_Assert( (src.type() == CV_32FC1 || src.type() == CV_32FC3) &&
            src.type() == dst.type() && src.size() == dst.size() &&
            src.data != dst.data );

        if( sigma_color <= 0 )
            sigma_color = 1;
        if( sigma_space <= 0 )
            sigma_space = 1;

        double gauss_color_coeff = -0.5/(sigma_color*sigma_color);
        double gauss_space_coeff = -0.5/(sigma_space*sigma_space);

        if( d <= 0 )
            radius = cvRound(sigma_space*1.5);
        else
            radius = d/2;
        radius = MAX(radius, 1);
        d = radius*2 + 1;
        // compute the min/max range for the input image (even if multichannel)

        minMaxLoc( src.reshape(1), &minValSrc, &maxValSrc );
        if(std::abs(minValSrc - maxValSrc) < FLT_EPSILON)
        {
            src.copyTo(dst);
            return;
        }

        // temporary copy of the image with borders for easy processing
        Mat temp;
        copyMakeBorder( src, temp, radius, radius, radius, radius, borderType );
        patchNaNs(temp);

        // allocate lookup tables
        vector<float> _space_weight(d*d);
        vector<int> _space_ofs(d*d);
        float* space_weight = &_space_weight[0];
        int* space_ofs = &_space_ofs[0];

        // assign a length which is slightly more than needed
        len = (float)(maxValSrc - minValSrc) * cn;
        kExpNumBins = kExpNumBinsPerChannel * cn;
        vector<float> _expLUT(kExpNumBins+2);
        float* expLUT = &_expLUT[0];

        scale_index = kExpNumBins/len;

        // initialize the exp LUT
        for( i = 0; i < kExpNumBins+2; i++ )
        {
            if( lastExpVal > 0.f )
            {
                double val =  i / scale_index;
                expLUT[i] = (float)std::exp(val * val * gauss_color_coeff);
                lastExpVal = expLUT[i];
            }
            else
                expLUT[i] = 0.f;
        }

        // initialize space-related bilateral filter coefficients
        for( i = -radius, maxk = 0; i <= radius; i++ )
            for( j = -radius; j <= radius; j++ )
            {
                double r = std::sqrt((double)i*i + (double)j*j);
                if( r > radius )
                    continue;
                space_weight[maxk] = (float)std::exp(r*r*gauss_space_coeff);
                space_ofs[maxk++] = (int)(i*(temp.step/sizeof(float)) + j*cn);
            }

        for( i = 0; i < size.height; i++ )
        {
            const float* sptr = (const float*)(temp.data + (i+radius)*temp.step) + radius*cn;
            float* dptr = (float*)(dst.data + i*dst.step);

            if( cn == 1 )
            {
                for( j = 0; j < size.width; j++ )
                {
                    float sum = 0, wsum = 0;
                    float val0 = sptr[j];
                    for( k = 0; k < maxk; k++ )
                    {
                        float val = sptr[j + space_ofs[k]];
                        float alpha = (float)(std::abs(val - val0)*scale_index);
                        int idx = cvFloor(alpha);
//.........这里部分代码省略.........

void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
                        InputArray _cameraMatrix, InputArray _distCoeffs,
                        OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
                        int iterationsCount, float reprojectionError, int minInliersCount,
                        OutputArray _inliers, int flags)
{
    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
    Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
    
    CV_Assert(opoints.isContinuous());
    CV_Assert(opoints.depth() == CV_32F);
    CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
    CV_Assert(ipoints.isContinuous());
    CV_Assert(ipoints.depth() == CV_32F);
    CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
    
    _rvec.create(3, 1, CV_64FC1);
    _tvec.create(3, 1, CV_64FC1);
    Mat rvec = _rvec.getMat();
    Mat tvec = _tvec.getMat();
    
    Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
    
    if (minInliersCount <= 0)
        minInliersCount = objectPoints.cols;
    cv::pnpransac::Parameters params;
    params.iterationsCount = iterationsCount;
    params.minInliersCount = minInliersCount;
    params.reprojectionError = reprojectionError;
    params.useExtrinsicGuess = useExtrinsicGuess;
    params.camera.init(cameraMatrix, distCoeffs);
	params.flags = flags;
    
    vector<int> localInliers;
    Mat localRvec, localTvec;
    rvec.copyTo(localRvec);
    tvec.copyTo(localTvec);
    
    if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
    {
        parallel_for(BlockedRange(0,iterationsCount), cv::pnpransac::PnPSolver(objectPoints, imagePoints, params,
                                                                               localRvec, localTvec, localInliers));
    }
    
    if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
    {
		if (flags != CV_P3P)
		{
			int i, pointsCount = (int)localInliers.size();
			Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2);
			for (i = 0; i < pointsCount; i++)
			{
				int index = localInliers[i];
				Mat colInlierImagePoints = inlierImagePoints(Rect(i, 0, 1, 1));
				imagePoints.col(index).copyTo(colInlierImagePoints);
				Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1));
				objectPoints.col(index).copyTo(colInlierObjectPoints);
			}
			solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, true, flags);
		}
		localRvec.copyTo(rvec);
        localTvec.copyTo(tvec);
        if (_inliers.needed())
            Mat(localInliers).copyTo(_inliers);
    }
    else
    {
        tvec.setTo(Scalar(0));
        Mat R = Mat::eye(3, 3, CV_64F);
        Rodrigues(R, rvec);
        if( _inliers.needed() )
            _inliers.release();
    }
    return;
}

//.........这里部分代码省略.........
		histogram_mask /* mask after histogram equalziation */,
		noise_free_mask /* mask after noise removal of salt and peper */,
		history[100],
		res
	;

	int counter_history = 0; /*sliding window history counter*/

//MOG Background subtractor initialziation
//constructor paramters : history size, number of gaussain mixtures, background ratio, noise strength)
	/*Ptr<BackgroundSubtractor> pMOG; 
	pMOG = new BackgroundSubtractorMOG(100,5,0,1);*/

//MOG2 Background subtractor initialziation
//constructor paramters : history size, threshold value, shadow detection 
	Ptr<BackgroundSubtractor> pMOG2; 
	pMOG2 = new BackgroundSubtractorMOG2(300,0, 0);

//camera initializer
	VideoCapture cap;
	cap.open(0);

	//sliding window history logic
	while (1) {
		
		//transfer feed to pre-intialized matrix
		cap >> frame;
		
		//image enhancement of gain (contrast) and bias (brightness) adustments
		//constructor : resulting (enhanced) image, ??, alpha/gain, beta/bias
		//frame.convertTo(frame, -1, 0.5, 0);

		//make a copy of frame for later comparision
		frame.copyTo(res);
		
		//exit if buffer exceeds 100
		if (counter_history == 100) {
			std::cout << "Applciation timed out. buffer exceeded 100 frames." << counter_history << endl;
			return 0;
		}
		
		//make a copy of frame for sliding window history
		waitKey(1); //buffer for not crashing system
		frame.copyTo(history[counter_history]);
		
		//wait untill 10th frame is obtained. to initialize history array
		if (counter_history > 10) {
			
			//output frame number for debugging purposes
			std::cout << "Frame feed No. " << counter_history << endl;
		
			////MOG implenetation
			////constructor : input image, resulting mask, learning_rate
			//	pMOG->operator()(frame, initial_mask, 0.8);
			//
			//	//enhancment of mask using histogram equlzation - constructor : input image (1 channel), output image (1 channel) 
			//	//Apply Histogram Equalization
			//	equalizeHist(initial_mask, histogram_mask);

			//	//noise reduction of salt and pepper using median blurring
			//	//constructor - input image (1 channel), output image (1 channel), k size (?)
			//		medianBlur(initial_mask, noise_free_mask, 5); //using input without histogram initialization
			//		//medianBlur(histogram_mask, noise_free_mask, 5); //using input with histogram initialization

			//	//test for checking channels in image matrix
			//	////std::cout << initial_mask.channels() << endl;

	//Called on each new image from the topic
  void rgbCallback(const sensor_msgs::ImageConstPtr& msg_ptr) {
    
	//Convert the image to OpenCV form
    try {
      ptr_rgb = cv_bridge::toCvCopy(msg_ptr, "bgr8");
      ptr_rgb->image.copyTo(*rgb_image);
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
    }
    waitKey(2);

	//Create a test image
     if(first)
      {
	imwrite("test.jpg", *rgb_image);
	//	cvSaveImage("test.jpg" ,cv_image);
	first = false;
      }
    //try {
    //ptr_hue = cv_bridge::toCvCopy(msg_ptr, "8UC1");
    //imshow(ptr_ccs->image);
    //ptr_hue->image.copyTo(*hue_image);
    //} catch (cv_bridge::Exception& e) {
    //	ROS_ERROR("cv_bridge exception: %s", e.what());
    //}

	//Create a HSV image from the RGB image
    cvtColor(*rgb_image, *hsv_image, CV_BGR2HSV);

	//Split the three image channels into separate matrices 
    vector <Mat> planes;
    split(*hsv_image, planes);
    *hue_image = planes[0];
    *sat_image = planes[1];
    *int_image = planes[2];

	//Upper and lower bounds on hue values
    int rangeMin = (mean_color - window_size)%255;
    int rangeMax = (mean_color + window_size)%255;

    //int otherRangeMin = (other_mean_color - window_size)%255;
    //int otherRangeMax = (other_mean_color + window_size)%255;
	
    if(rangeMin > rangeMax){
      int temp = rangeMax;
      rangeMax = rangeMin;
	//It looks like the should be =temp
      rangeMin = rangeMax;
    }
/*
    if(otherRangeMin > otherRangeMax){
      int temp = otherRangeMax;
      otherRangeMax = otherRangeMin;
	//It looks like the should be =temp
      otherRangeMin = otherRangeMax;
    }
*/	
	//Create a binary image from the threshold
    inRange(*hue_image, Scalar((double)rangeMin),Scalar((double)rangeMax), *back_img);
    *color_cc_image = Scalar(0);

	//Apply blur
    back_img->copyTo(*hue_image);
    Size ksize = Size(2 * blurKernel + 1,2 * blurKernel + 1);
    GaussianBlur(*back_img, *blurred_image, ksize, -1, -1);

    //attempts at adaptive thresholding
    Mat* sat_image;	  //input image -> satuation space
    //adaptiveThreshold(*blurred_image, *temp_blurred_image, 255, ADAPTIVE_THRESH_GAUSSIAN_C, THRESH_BINARY, 3, .5);
    //threshold(*blurred_image, *temp_blurred_image, THRESH_OTSU, 255, THRESH_BINARY); 

    threshold(*blurred_image, *temp_blurred_image, 110, 255, THRESH_BINARY); 
    convertScaleAbs(*temp_blurred_image, *back_img, 1, 0);
    hue_image->copyTo(*copy_image);

    if (display_image_){
      imshow(color_topic, *back_img);
    }
    getConnectedComponents();

    //Find Connected Components (this will populate the contour vector and perform ordering)
/*
    findContours(*back_img, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE, Point());
    if(!contours.empty())
      numCC = getThresholdConnectedComponents();
	
    //I'm adding this
    if (contours.empty()){
      inRange(*hue_image, Scalar((double)otherRangeMin),Scalar((double)otherRangeMax), *back_img);
      *color_cc_image = Scalar(0);
      back_img->copyTo(*hue_image);
      Size ksize = Size(2 * blurKernel + 1,2 * blurKernel + 1);
      GaussianBlur(*back_img, *blurred_image, ksize, -1, -1);
      Mat* sat_image;
      threshold(*blurred_image, *temp_blurred_image, 110, 255, THRESH_BINARY); 
      convertScaleAbs(*temp_blurred_image, *back_img, 1, 0);
      hue_image->copyTo(*copy_image);
      findContours(*back_img, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE, Point());
      if(!contours.empty())
//.........这里部分代码省略.........

	//Hue callback is not currently used
  void hueCallback(const sensor_msgs::ImageConstPtr& msg_ptr) {

    try {
      ptr_hue = cv_bridge::toCvCopy(msg_ptr, "8UC1");
      //imshow(ptr_hue->image);
      ptr_hue->image.copyTo(*hue_image);
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
    }
		
    int rangeMin = (mean_color - window_size)%255;
    int rangeMax = (mean_color + window_size)%255;
    //int otherRangeMin = (other_mean_color - window_size)%255;
    //int otherRangeMax = (other_mean_color + window_size)%255;

    if(rangeMin > rangeMax){
      int temp = rangeMax;
      rangeMax = rangeMin;
      rangeMin = rangeMax;
    }
/*
    if(otherRangeMin > otherRangeMax){
      int temp = otherRangeMax;
      otherRangeMax = otherRangeMin;
      otherRangeMin = otherRangeMax;
    }
*/
    cout << "range: " << rangeMin << " " << rangeMax << endl;
    inRange(*hue_image, Scalar((double)rangeMin),Scalar((double)rangeMax), *back_img);
    *color_cc_image = Scalar(0);
    back_img->copyTo(*hue_image);
    Size ksize = Size(2 * blurKernel + 1,2 * blurKernel + 1);
    GaussianBlur(*back_img, *blurred_image, ksize, -1, -1);

    //attempts at adaptive thresholding

    //adaptiveThreshold(*blurred_image, *temp_blurred_image, 255, ADAPTIVE_THRESH_GAUSSIAN_C, THRESH_BINARY, 3, .5);
    //threshold(*blurred_image, *temp_blurred_image, THRESH_OTSU, 255, THRESH_BINARY); 

    threshold(*blurred_image, *temp_blurred_image, 110, 255, THRESH_BINARY); 
    convertScaleAbs(*temp_blurred_image, *back_img, 1, 0);
    hue_image->copyTo(*copy_image);

    if (display_image_){
      imshow(color_topic, *back_img);
    }

    getConnectedComponents();

    //Find Connected Components
    //I added this-it used to be just getConnectedComponent
/*
    findContours(*back_img, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE, Point());
    if(!contours.empty())
      numCC = getThresholdConnectedComponents();

    if (contours.empty()){
      inRange(*hue_image, Scalar((double)otherRangeMin),Scalar((double)otherRangeMax), *back_img);
      *color_cc_image = Scalar(0);
      back_img->copyTo(*hue_image);
      Size ksize = Size(2 * blurKernel + 1,2 * blurKernel + 1);
      GaussianBlur(*back_img, *blurred_image, ksize, -1, -1);
      Mat* sat_image;
      threshold(*blurred_image, *temp_blurred_image, 110, 255, THRESH_BINARY); 
      convertScaleAbs(*temp_blurred_image, *back_img, 1, 0);
      hue_image->copyTo(*copy_image);
      findContours(*back_img, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE, Point());
      if(!contours.empty())
        numCC = getThresholdConnectedComponents();
    }
*/		
    for (int i = 0; (i < (int)comps.size()) && (comps[i].idx >= 0) && (comps[i].idx < (int)contours.size()); i++) {
      Scalar color( (rand()&255), (rand()&255), (rand()&255) );
      drawContours(*color_cc_image, contours, comps[i].idx, color, 3, 8, hierarchy,0, Point());
      drawContours(*hue_image, contours, 0, comps[i].idx, 3, 8, hierarchy,0, Point());
    }

    getMoments();

    blobTracker->updateKalmanFiltersConnectedComponents();
    if (numCC > 0)
      blobTracker->getFilteredBlobs(true);
    else
      blobTracker->getFilteredBlobs(false);
    //cv::imshow("cam",hue_image);
    //Draw Filtered Blobs
    
    RotatedRect box;
    Point pt;
    //Image_msgs::FeatureMoments moments;
    //moments.num = 0;
    for(int i=0; i<MAX_FEATURES; i++)
      {
	FeatureBlob ftemp;
	blobTracker->filters_[i].getFiltered(ftemp);
	//cout << "valid? " << ftemp.getValid() << endl;
	if(ftemp.getValid() && display_image_)
	  {
	    //cout << "Should be displaying something!" << endl;
//.........这里部分代码省略.........

// match histograms of 'src' to that of 'dst', according to both masks
void Preprocessor::histMatchRGB(cv::Mat& src, const cv::Mat& src_mask, const cv::Mat& dst, const cv::Mat& dst_mask)
{
#ifdef BTM_DEBUG
    namedWindow("original source",CV_WINDOW_AUTOSIZE);
    imshow("original source",src);
    namedWindow("original query",CV_WINDOW_AUTOSIZE);
    imshow("original query",dst);
#endif

    vector<Mat> chns;
    split(src,chns);
    vector<Mat> chns1;
    split(dst,chns1);
    Mat src_hist = Mat::zeros(1,256,CV_64FC1);
    Mat dst_hist = Mat::zeros(1,256,CV_64FC1);
    Mat src_cdf = Mat::zeros(1,256,CV_64FC1);
    Mat dst_cdf = Mat::zeros(1,256,CV_64FC1);
    Mat Mv(1,256,CV_8UC1);
    uchar* M = Mv.ptr<uchar>();

    for(int i=0;i<3;i++) {
        src_hist.setTo(0);
        dst_hist.setTo(0);
        src_cdf.setTo(0);
        src_cdf.setTo(0);

        double* _src_cdf = src_cdf.ptr<double>();
        double* _dst_cdf = dst_cdf.ptr<double>();
        double* _src_hist = src_hist.ptr<double>();
        double* _dst_hist = dst_hist.ptr<double>();

        do1ChnHist(chns[i],src_mask,_src_hist,_src_cdf);
        do1ChnHist(chns1[i],dst_mask,_dst_hist,_dst_cdf);

        uchar last = 0;


        for(int j=0;j<src_cdf.cols;j++) {
            double F1j = _src_cdf[j];

            for(uchar k = last; k<dst_cdf.cols; k++) {
                double F2k = _dst_cdf[k];
                if(abs(F2k - F1j) < HISTMATCH_EPSILON || F2k > F1j) {
                    M[j] = k;
                    last = k;
                    break;
                }
            }
        }

        Mat lut(1,256,CV_8UC1,M);
        LUT(chns[i],lut,chns[i]);
    }

    Mat res;
    merge(chns,res);

#ifdef BTM_DEBUG
    namedWindow("matched",CV_WINDOW_AUTOSIZE);
    imshow("matched",res);

    waitKey(0);
#endif

    res.copyTo(src);
}

//This colors the segmentations
void floodFillPostprocess( Mat& img, const Scalar& colorDiff=Scalar::all(1) )
{    
    
    CV_Assert( !img.empty() );
    RNG rng = theRNG();
    Mat src_gray, canny_output, mask;
    cvtColor( img, src_gray, CV_BGR2GRAY );
    
    Canny( src_gray, canny_output, thresh, 2*thresh, 3 );
    
    
    /*//If other line finders uncomment the switch below
    Mat dst;
    int scale = 1;
    int delta = 0;
    int ddepth = CV_16S;
    int kernel_size = 3; 
    	    Laplacian( src_gray, dst, ddepth, kernel_size, scale, delta, BORDER_DEFAULT );
    	    convertScaleAbs( dst, canny_output );
    /*
    
    switch (lineFinder)
    {
        case '0': 
            Canny( src_gray, canny_output, thresh, 2*thresh, 3 );
            break;
    
    // Other Line finding For mask ... Not useful but looks cool
      // Laplacian
        case '1':
            Laplacian( src_gray, dst, ddepth, kernel_size, scale, delta, BORDER_DEFAULT );
    	    convertScaleAbs( dst, canny_output );
    	    break;
    	case '2':   
    	    //Sobel
    	    /// Generate grad_x and grad_y
    	    Mat grad_x, grad_y;
    	    Mat abs_grad_x, abs_grad_y;
    	    
	    /// Gradient X
	    //Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT );
	    Sobel( src_gray, grad_x, ddepth, 1, 0, 3, scale, delta, BORDER_DEFAULT );   
	    convertScaleAbs( grad_x, abs_grad_x );

	    /// Gradient Y  
	    //Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT );
	    Sobel( src_gray, grad_y, ddepth, 0, 1, 3, scale, delta, BORDER_DEFAULT );   
	    convertScaleAbs( grad_y, abs_grad_y );

	    /// Total Gradient (approximate)
	    addWeighted( abs_grad_x, 0.5, abs_grad_y, 0.5, 0, canny_output );
	    break;
    }
    */
    
    copyMakeBorder( canny_output, mask, 1, 1, 1, 1, BORDER_REPLICATE, 1 );
    int numregions = 0;
    vector<Rect> regions;
    vector<Mat> Masks; 
    Rect region;
    //Mat temp2;
    //original.copyTo(temp2);
    for( int y = 0; y < img.rows; y++ )
    {
        for( int x = 0; x < img.cols; x++ )
        {
            if( mask.at<uchar>(y+1, x+1) == 0 )
            {
                Scalar newVal( rng(256), rng(256), rng(256) );
                floodFill( img, mask, Point(x,y), newVal, &region, colorDiff, colorDiff,  4 );  //FLOODFILL_MASK_ONLY +
                if ((region.height * region.width > 250))
                {
                if(rects)  rectangle( img, Point(region.x,region.y), Point(region.x + region.width, region.y + region.height), Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) ), 2, 8, 0 );
                regions.push_back(region);
                Masks.push_back(mask);                
                numregions++;
                }
                
            }
        }
    }
    //cout << "Number of regions: " << numregions << "\tSize of Region Vector: " << regions.size() << endl;
    //imshow( "meanshift", img );
    //imshow( "Regions ROIs", temp2);
    
//    createTrackbar( " Canny thresh:", "Mask", &thresh, max_thresh, meanShiftSegmentation );
    if(display)
    {
        //mask = Scalar::all(0);
        //imshow("mask", mask);
        
        Mat temp2;
        original.copyTo( temp2, canny_output);
        //imshow("Mask", temp2);
        //imshow("Mask After", mask);
        vector<Rect>::iterator it;
        string filename;
        vector<Mat>::iterator itt;
        itt = Masks.begin();
//.........这里部分代码省略.........

//.........这里部分代码省略.........
			landmarks.emplace_back(imageio::ModelLandmark("right.eye.corner_outer", landmarksX[19], landmarksY[19]));
			landmarks.emplace_back(imageio::ModelLandmark("right.eye.corner_inner", landmarksX[22], landmarksY[22]));
			landmarks.emplace_back(imageio::ModelLandmark("left.eye.corner_outer", landmarksX[28], landmarksY[28]));
			landmarks.emplace_back(imageio::ModelLandmark("left.eye.corner_inner", landmarksX[25], landmarksY[25]));
			landmarks.emplace_back(imageio::ModelLandmark("center.nose.tip", landmarksX[13], landmarksY[13]));
			landmarks.emplace_back(imageio::ModelLandmark("right.lips.corner", landmarksX[31], landmarksY[31]));
			landmarks.emplace_back(imageio::ModelLandmark("left.lips.corner", landmarksX[37], landmarksY[37]));
			
			int max_d = std::max(img2.rows, img2.cols); // should be the focal length? (don't forget the aspect ratio!). TODO Read in Hartley-Zisserman what this is
			//int max_d = 700;
			Mat camMatrix = (cv::Mat_<double>(3,3) << max_d, 0,		img2.cols/2.0,
				0,	 max_d, img2.rows/2.0,
				0,	 0,		1.0);

			pair<Mat, Mat> rotTransRodr = cameraEstimation.estimate(landmarks, camMatrix);
			Mat rvec = rotTransRodr.first;
			Mat tvec = rotTransRodr.second;

			Mat rotation_matrix(3, 3, CV_64FC1);
			cv::Rodrigues(rvec, rotation_matrix);
			rotation_matrix.convertTo(rotation_matrix, CV_32FC1);
			Mat translation_vector = tvec;
			translation_vector.convertTo(translation_vector, CV_32FC1);

			camMatrix.convertTo(camMatrix, CV_32FC1);

			for (const auto& p : landmarks) {
				cv::rectangle(img2, cv::Point(cvRound(p.getX()-2.0f), cvRound(p.getY()-2.0f)), cv::Point(cvRound(p.getX()+2.0f), cvRound(p.getY()+2.0f)), cv::Scalar(255, 0, 0));
			}
			//vector<Point2f> projectedPoints;
			//projectPoints(modelPoints, rvec, tvec, camMatrix, vector<float>(), projectedPoints); // same result as below
			Mat extrinsicCameraMatrix = Mat::zeros(4, 4, CV_32FC1);
			Mat extrRot = extrinsicCameraMatrix(cv::Range(0, 3), cv::Range(0, 3));
			rotation_matrix.copyTo(extrRot);
			Mat extrTrans = extrinsicCameraMatrix(cv::Range(0, 3), cv::Range(3, 4));
			translation_vector.copyTo(extrTrans);
			extrinsicCameraMatrix.at<float>(3, 3) = 1;

			Mat intrinsicCameraMatrix = Mat::zeros(4, 4, CV_32FC1);
			Mat intrinsicCameraMatrixMain = intrinsicCameraMatrix(cv::Range(0, 3), cv::Range(0, 3));
			camMatrix.copyTo(intrinsicCameraMatrixMain);
			intrinsicCameraMatrix.at<float>(3, 3) = 1;
			
			vector<Point3f> points3d;
			for (const auto& landmark : landmarks) {
				points3d.emplace_back(mm.getShapeModel().getMeanAtPoint(landmark.getName()));
			}
			for (const auto& v : points3d) {
				Mat vertex(v);
				Mat vertex_homo = Mat::ones(4, 1, CV_32FC1);
				Mat vertex_homo_coords = vertex_homo(cv::Range(0, 3), cv::Range(0, 1));
				vertex.copyTo(vertex_homo_coords);
				Mat v2 = rotation_matrix * vertex;
				Mat v3 = v2 + translation_vector;
				Mat v3_mat = extrinsicCameraMatrix * vertex_homo;

				Mat v4 = camMatrix * v3;
				Mat v4_mat = intrinsicCameraMatrix * v3_mat;

				Point3f v4p(v4);
				Point2f v4p2d(v4p.x/v4p.z, v4p.y/v4p.z); // if != 0
				Point3f v4p_homo(v4_mat(cv::Range(0, 3), cv::Range(0, 1)));
				Point2f v4p2d_homo(v4p_homo.x/v4p_homo.z, v4p_homo.y/v4p_homo.z); // if != 0
				cv::rectangle(img2, cv::Point(cvRound(v4p2d_homo.x-2.0f), cvRound(v4p2d_homo.y-2.0f)), cv::Point(cvRound(v4p2d_homo.x+2.0f), cvRound(v4p2d_homo.y+2.0f)), cv::Scalar(255, 0, 0));
			}

//////////////////////////////////////////////////////////////////////////
//	do convolution in Frequency domain
//	@return		final image after convolution
//////////////////////////////////////////////////////////////////////////
bool cvFFTConvolution(const Mat& channel, const Mat& kernel, Mat& conv)
{
	//create complex image and kernel
	//////////////////////////////////////////////////////////////////////////
	Mat complexImg(channel.rows, channel.cols, CV_32FC2);
	vector<Mat> vec_mat(2);
	vec_mat[0] = channel.clone();
	vec_mat[1] = channel.clone();
	vec_mat[1].setTo(0);
	merge(vec_mat, complexImg);

	//////////////////////////////////////////////////////////////////////////
	Mat complexKernel(kernel.rows, kernel.cols, CV_32FC2);
	kernel.copyTo(vec_mat[0]);
	kernel.copyTo(vec_mat[1]);
	vec_mat[1].setTo(0);
	vec_mat[1] = kernel.clone();
	merge(vec_mat, complexKernel);

	//////////////////////////////////////////////////////////////////////////
	/*int dft_M = getOptimalDFTSize(channel.rows*2-1);
	int dft_N = getOptimalDFTSize(channel.cols*2-1);*/

	Mat dft_A(channel.rows, channel.cols, CV_32FC2);
	dft_A.setTo(0);
	Mat dft_B(kernel.rows, kernel.cols, CV_32FC2);
	dft_B.setTo(0);

	//////////////////////////////////////////////////////////////////////////
	// do dft for image
	//////////////////////////////////////////////////////////////////////////
	Mat tmp = dft_A(Rect(0, 0, channel.cols, channel.rows));
	complexImg.copyTo(tmp);

	dft(dft_A, dft_A, CV_DXT_FORWARD);


	//////////////////////////////////////////////////////////////////////////
	// do dft for kernel
	//////////////////////////////////////////////////////////////////////////
	tmp = dft_B(Rect(0, 0, kernel.cols, kernel.rows));
	complexKernel.copyTo(tmp);

	//do dft
	dft(dft_B, dft_B, CV_DXT_FORWARD);

	//shift kernel to center
	Mat dft_BB = dft_B.clone();
	ShiftDFT(dft_B, dft_BB);
	dft_BB.copyTo(dft_B);

	//////////////////////////////////////////////////////////////////////////
	//	do convolution
	//////////////////////////////////////////////////////////////////////////
	mulSpectrums(dft_A, dft_B, dft_A, CV_DXT_MUL_CONJ);

	//do inverse dft
	dft(dft_A, dft_A, CV_DXT_INV_SCALE, channel.rows);

	split(dft_A, vec_mat);
	pow(vec_mat[0], 2, vec_mat[0]);
	pow(vec_mat[1], 2, vec_mat[1]);
	vec_mat[0] += vec_mat[1];
	pow(vec_mat[0], 2, vec_mat[0]);

	//copy back
	vec_mat[0].copyTo(conv);

	return true;
}

std::vector<KeyPoint> findPaper (Mat frame, double avgBright) {
  Mat colors[3];
  Mat colored;
  Mat copy;
  
  frame.copyTo(colored);
  cvtColor(colored,colored,CV_BGR2HSV);
  split(frame,colors);
  copy = frame;
   
  cvtColor(frame,frame,CV_BGR2GRAY);
  GaussianBlur(frame,frame,Size(3,3),0);

   
  for(int parameter = 10; parameter > 0; parameter--) {
   
    double offset = 1;
    std:vector<KeyPoint> points;
    std::vector<KeyPoint> allPoints;
    FAST(frame,allPoints,parameter);

    do {      
      points = std::vector<KeyPoint>(allPoints);
      std::vector<int> keep(points.size());
      for (std::vector<KeyPoint>::iterator it = points.begin(); it != points.end(); it++) {
	KeyPoint pt = *it;
	Vec3b at = colored.at<Vec3b>(pt.pt);
	int numRed = 0;
	int numWhite = 0;
	 double dr[8] = { 0, -1,-1, -1, 0,  1, 1, 1};
	 double dc[8] = { 1, 1,  0, -1,-1,-1, 0, 1};
	 std::vector<int> deltaRow (dr, dr+sizeof(dr)/sizeof(int));
	 std::vector<int> deltaCol (dc, dc+sizeof(dc)/sizeof(int));
  	  
	 for(unsigned int i = 0; i < deltaRow.size(); i++) {
	   KeyPoint delta = pt;
	   int rowAdd = offset;
	   int colAdd = offset;
	  
	   delta.pt.x = pt.pt.x + (int) (deltaRow[i] * rowAdd);
	   delta.pt.y = pt.pt.y + (int) (deltaCol[i] * colAdd);
	   if(delta.pt.x < 0 || delta.pt.y < 0 || delta.pt.x >= frame.cols || delta.pt.y >= frame.rows) {
	     continue;
	   }
	   int colorCode = 0;
	  Vec3b neighbor = colored.at<Vec3b>(delta.pt);
	  int count = 0;
	  for(int i = 0; i < 3; i++) {
	    if((int)neighbor[i] == 255){count++;} 
	  }
	  
	  
	  if(count >= 2){
	    continue;
	  }
	  
	  if((neighbor[0] > LOW_RED || neighbor[0] < HIGH_RED) && neighbor[1] > 73 && neighbor[2] > 50) {
	    numRed++;
	    colorCode = 1;
	  } else if( neighbor[2] > avgBright) {
	    numWhite++;
	    colorCode = 2;
	  }
	  
	  if(colorCode == 0) {
	    colored.at<Vec3b>(delta.pt)[0]= 3;
	    colored.at<Vec3b>(delta.pt)[1] = 100;
	    colored.at<Vec3b>(delta.pt)[2] = 50;
	  } else if (colorCode == 1) {
	    colored.at<Vec3b>(delta.pt)[0]= 100;
	    colored.at<Vec3b>(delta.pt)[1] = 255;
	    colored.at<Vec3b>(delta.pt)[2] = 255;
	  } else if (colorCode == 2) {
	    colored.at<Vec3b>(delta.pt)[0]= 10;
	    colored.at<Vec3b>(delta.pt)[1] = 255;
	    colored.at<Vec3b>(delta.pt)[2] = 255;
	    
	  }
	}  
	if(numRed != 5 ||( numWhite != 3 )) {
	  keep[it - points.begin()] = -1;
	}
      }
      
      int size = points.size();
      int trueIndex = 0;
      int loopIndex = 0;
      
      while(trueIndex < size) {
	if(keep[trueIndex] == -1) {
	  points.erase(points.begin()+loopIndex);
	  
	  trueIndex++;
	} else {
	  loopIndex++;
	  trueIndex++;
	}
      }
      offset++;
    } while(points.size() != 4 && offset < 20);
//.........这里部分代码省略.........

 void init(Mat _intrinsics, Mat _distCoeffs)
 {
     _intrinsics.copyTo(intrinsics);
     _distCoeffs.copyTo(distortion);
 }