C++ Mat类代码示例

 /**
  * @brief getPixelValue returns the pixel value at location of point in matrix M
  * @param M input matrix
  * @param p indicates the location
  * @return
  */
 T  getPixelValue(Mat &M,Point p)
 {
         T val=M.data + M.step*p.y + p.x*M.elemSize();
         return val;
 }

static void convertFromCCS( const Mat& _src0, const Mat& _src1, Mat& _dst, int flags )
{
    if( _dst.rows > 1 && (_dst.cols > 1 || (flags & DFT_ROWS)) )
    {
        int i, count = _dst.rows, len = _dst.cols;
        bool is2d = (flags & DFT_ROWS) == 0;
        Mat src0row, src1row, dstrow;
        for( i = 0; i < count; i++ )
        {
            int j = !is2d || i == 0 ? i : count - i;
            src0row = _src0.row(i);
            src1row = _src1.row(j);
            dstrow = _dst.row(i);
            convertFromCCS( src0row, src1row, dstrow, 0 );
        }
        
        if( is2d )
        {
            src0row = _src0.col(0);
            dstrow = _dst.col(0);
            convertFromCCS( src0row, src0row, dstrow, 0 );
            if( (len & 1) == 0 )
            {
                src0row = _src0.col(_src0.cols - 1);
                dstrow = _dst.col(len/2);
                convertFromCCS( src0row, src0row, dstrow, 0 );
            }
        }
    }
    else
    {
        int i, n = _dst.cols + _dst.rows - 1, n2 = (n+1) >> 1;
        int cn = _src0.channels();
        int srcstep = cn, dststep = 1;
        
        if( !_dst.isContinuous() )
            dststep = (int)(_dst.step/_dst.elemSize());
        
        if( !_src0.isContinuous() )
            srcstep = (int)(_src0.step/_src0.elemSize1());
        
        if( _dst.depth() == CV_32F )
        {
            Complexf* dst = _dst.ptr<Complexf>();
            const float* src0 = _src0.ptr<float>();
            const float* src1 = _src1.ptr<float>();
            int delta0, delta1;
            
            dst->re = src0[0];
            dst->im = 0;
            
            if( (n & 1) == 0 )
            {
                dst[n2*dststep].re = src0[(cn == 1 ? n-1 : n2)*srcstep];
                dst[n2*dststep].im = 0;
            }
            
            delta0 = srcstep;
            delta1 = delta0 + (cn == 1 ? srcstep : 1);
            if( cn == 1 )
                srcstep *= 2;
            
            for( i = 1; i < n2; i++, delta0 += srcstep, delta1 += srcstep )
            {
                float t0 = src0[delta0];
                float t1 = src0[delta1];
                
                dst[i*dststep].re = t0;
                dst[i*dststep].im = t1;
                
                t0 = src1[delta0];
                t1 = -src1[delta1];
                
                dst[(n-i)*dststep].re = t0;
                dst[(n-i)*dststep].im = t1;
            }
        }
        else
        {
            Complexd* dst = _dst.ptr<Complexd>();
            const double* src0 = _src0.ptr<double>();
            const double* src1 = _src1.ptr<double>();
            int delta0, delta1;
            
            dst->re = src0[0];
            dst->im = 0;
            
            if( (n & 1) == 0 )
            {
                dst[n2*dststep].re = src0[(cn == 1 ? n-1 : n2)*srcstep];
                dst[n2*dststep].im = 0;
            }
            
            delta0 = srcstep;
            delta1 = delta0 + (cn == 1 ? srcstep : 1);
            if( cn == 1 )
                srcstep *= 2;
            
            for( i = 1; i < n2; i++, delta0 += srcstep, delta1 += srcstep )
            {
//.........这里部分代码省略.........

static void mulComplex( const Mat& src1, const Mat& src2, Mat& dst, int flags )
{
    dst.create(src1.rows, src1.cols, src1.type());
    int i, j, depth = src1.depth(), cols = src1.cols*2;
    
    CV_Assert( src1.size == src2.size && src1.type() == src2.type() &&
              (src1.type() == CV_32FC2 || src1.type() == CV_64FC2) );
    
    for( i = 0; i < dst.rows; i++ )
    {
        if( depth == CV_32F )
        {
            const float* a = src1.ptr<float>(i);
            const float* b = src2.ptr<float>(i);
            float* c = dst.ptr<float>(i);
            
            if( !(flags & CV_DXT_MUL_CONJ) )
                for( j = 0; j < cols; j += 2 )
                {
                    double re = (double)a[j]*b[j] - (double)a[j+1]*b[j+1];
                    double im = (double)a[j+1]*b[j] + (double)a[j]*b[j+1];
                    
                    c[j] = (float)re;
                    c[j+1] = (float)im;
                }
            else
                for( j = 0; j < cols; j += 2 )
                {
                    double re = (double)a[j]*b[j] + (double)a[j+1]*b[j+1];
                    double im = (double)a[j+1]*b[j] - (double)a[j]*b[j+1];
                    
                    c[j] = (float)re;
                    c[j+1] = (float)im;
                }
        }
        else
        {
            const double* a = src1.ptr<double>(i);
            const double* b = src2.ptr<double>(i);
            double* c = dst.ptr<double>(i);
            
            if( !(flags & CV_DXT_MUL_CONJ) )
                for( j = 0; j < cols; j += 2 )
                {
                    double re = a[j]*b[j] - a[j+1]*b[j+1];
                    double im = a[j+1]*b[j] + a[j]*b[j+1];
                    
                    c[j] = re;
                    c[j+1] = im;
                }
            else
                for( j = 0; j < cols; j += 2 )
                {
                    double re = a[j]*b[j] + a[j+1]*b[j+1];
                    double im = a[j+1]*b[j] - a[j]*b[j+1];
                    
                    c[j] = re;
                    c[j+1] = im;
                }
        }
    }
}    

// returns sequence of squares detected on the image.
// the sequence is stored in the specified memory storage
static void findSquares( const Mat& image, vector<vector<Point> >& squares )
{
    squares.clear();
    Mat pyr, timg, gray0(image.size(), CV_8U), gray;

    // down-scale and upscale the image to filter out the noise
    pyrDown(image, pyr, Size(image.cols/2, image.rows/2));
    pyrUp(pyr, timg, image.size());
    vector<vector<Point> > contours;

    // find squares in every color plane of the image
    for( int c = 0; c < 3; c++ )
    {
        int ch[] = {c, 0};
        mixChannels(&timg, 1, &gray0, 1, ch, 1);

        // try several threshold levels
        for( int l = 0; l < N; l++ )
        {
            // hack: use Canny instead of zero threshold level.
            // Canny helps to catch squares with gradient shading
            if( l == 0 )
            {
                // apply Canny. Take the upper threshold from slider
                // and set the lower to 0 (which forces edges merging)
                Canny(gray0, gray, 0, thresh, 5);
                // dilate canny output to remove potential
                // holes between edge segments
                dilate(gray, gray, Mat(), Point(-1,-1));
            }
            else
            {
                // apply threshold if l!=0:
                //     tgray(x,y) = gray(x,y) < (l+1)*255/N ? 255 : 0
                cv::threshold(gray0, gray, (l+1)*255/N, 255, THRESH_BINARY);
            }

            // find contours and store them all as a list
            findContours(gray, contours, RETR_LIST, CHAIN_APPROX_SIMPLE);

            vector<Point> approx;

            // test each contour
            for( size_t i = 0; i < contours.size(); i++ )
            {
                // approximate contour with accuracy proportional
                // to the contour perimeter
                approxPolyDP(Mat(contours[i]), approx, arcLength(Mat(contours[i]), true)*0.02, true);

                // square contours should have 4 vertices after approximation
                // relatively large area (to filter out noisy contours)
                // and be convex.
                // Note: absolute value of an area is used because
                // area may be positive or negative - in accordance with the
                // contour orientation
                if( approx.size() == 4 &&
                        fabs(contourArea(Mat(approx))) > 1000 &&
                        isContourConvex(Mat(approx)) )
                {
                    double maxCosine = 0;

                    for( int j = 2; j < 5; j++ )
                    {
                        // find the maximum cosine of the angle between joint edges
                        double cosine = fabs(angle(approx[j%4], approx[j-2], approx[j-1]));
                        maxCosine = MAX(maxCosine, cosine);
                    }

                    // if cosines of all angles are small
                    // (all angles are ~90 degree) then write quandrange
                    // vertices to resultant sequence
                    if( maxCosine < 0.3 )
                        squares.push_back(approx);
                }
            }
        }
    }
}

int _tmain(int argc, _TCHAR* argv[])
{
	//Initial detecting unity
	SurfFeatureDetector detector(400);
	vector<KeyPoint> lastKeyPoint;
	vector<KeyPoint> currentKeyPoint;
	//Read Video
	VideoCapture capture("..\\testVideo.mp4");
	//Create the Mat for storing current frame and last frame
	Mat frame;
	Mat lastFrame;
	if (!capture.isOpened())
	{
		cout<<"Video open failed!"<<endl;
		return 0;
	}
	namedWindow("Window");
	while(1)
	{
		//Compute the key points in each frame--------

		//Get Start Time to evaluate time period of the algorithm
		double t = (double)getTickCount();

		//Read a frame from video capture
		//And move it to previous frame
		lastFrame = frame.clone();
		capture>>frame;
		if(lastFrame.empty())
		{
			//Skip first frame;
			continue;
		}

		//进行特征点提取
		detector.detect(lastFrame,lastKeyPoint);
		detector.detect(frame,currentKeyPoint);
		//生成描述符
		SurfDescriptorExtractor extractor;
		Mat descriptor1;
		Mat descriptor2;
		extractor.compute(frame,lastKeyPoint,descriptor1);
		extractor.compute(frame,currentKeyPoint,descriptor2);

		//特征匹配
		FlannBasedMatcher matcher;
		vector<DMatch> matches;
		matcher.match( descriptor1, descriptor2, matches );
		//-- Quick calculation of max and min distances between keypoints
		double min_dist = 100;
		double max_dist = 0;
		for( int i = 0; i < descriptor1.rows; i++ )
		{ 
			double dist = matches[i].distance;
			if( dist < min_dist ) 
				min_dist = dist;
			if( dist > max_dist ) 
				max_dist = dist;
		}

		//-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist )
		//-- PS.- radiusMatch can also be used here.
		vector< DMatch > good_matches;

		for( int i = 0; i < descriptor1.rows; i++ )
		{ 
			if( matches[i].distance < 2*min_dist )
			{
				good_matches.push_back( matches[i]); 
			}
		}

		//Compute the Total Time
		t = ((double)getTickCount()-t)/getTickFrequency();
		cout<<"time is:"<<t<<endl;
		//-- Draw only "good" matches
		Mat img_matches;
		drawMatches( frame, keyPoint, frame, keyPoint2,
			good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),
			vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
		if (frame.empty())
		{
			cout << " < < <  Video End!  > > > "<<endl;
			break;
		}
		imshow("Window",frame);
		waitKey(10);
	}
	return 0;
	////画出KeyPoint
	//drawKeypoints(img,keyPoint,img);
	//drawKeypoints(smallimg,keyPoint2,smallimg);
	waitKey();
	return 0;
}

int main( int argc, char** argv )
{
    VideoCapture cap;
    TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03);
    Size subPixWinSize(10,10), winSize(31,31);

    const int MAX_COUNT = 500;
    bool needToInit = false;
    bool nightMode = false;

    if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
        cap.open(argc == 2 ? argv[1][0] - '0' : 0);
    else if( argc == 2 )
        cap.open(argv[1]);

    if( !cap.isOpened() )
    {
        cout << "Could not initialize capturing...\n";
        return 0;
    }

    help();

    namedWindow( "LK Demo", 1 );
    setMouseCallback( "LK Demo", onMouse, 0 );

    Mat gray, prevGray, image;
    vector<Point2f> points[2];

    for(;;)
    {
        Mat frame;
        cap >> frame;
        if( frame.empty() )
            break;

        frame.copyTo(image);
        cvtColor(image, gray, CV_BGR2GRAY);

        if( nightMode )
            image = Scalar::all(0);

        if( needToInit )
        {
            // automatic initialization
            goodFeaturesToTrack(gray, points[1], MAX_COUNT, 0.01, 10, Mat(), 3, 0, 0.04);
            cornerSubPix(gray, points[1], subPixWinSize, Size(-1,-1), termcrit);
            addRemovePt = false;
        }
        else if( !points[0].empty() )
        {
            vector<uchar> status;
            vector<float> err;
            if(prevGray.empty())
                gray.copyTo(prevGray);
            calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize,
                                 3, termcrit, 0, 0.001);
            size_t i, k;
            for( i = k = 0; i < points[1].size(); i++ )
            {
                if( addRemovePt )
                {
                    if( norm(point - points[1][i]) <= 5 )
                    {
                        addRemovePt = false;
                        continue;
                    }
                }

                if( !status[i] )
                    continue;

                points[1][k++] = points[1][i];
                circle( image, points[1][i], 3, Scalar(0,255,0), -1, 8);
            }
            points[1].resize(k);
        }

        if( addRemovePt && points[1].size() < (size_t)MAX_COUNT )
        {
            vector<Point2f> tmp;
            tmp.push_back(point);
            cornerSubPix( gray, tmp, winSize, cvSize(-1,-1), termcrit);
            points[1].push_back(tmp[0]);
            addRemovePt = false;
        }

        needToInit = false;
        imshow("LK Demo", image);

        char c = (char)waitKey(10);
        if( c == 27 )
            break;
        switch( c )
        {
        case 'r':
            needToInit = true;
            break;
        case 'c':
            points[1].clear();
//.........这里部分代码省略.........

int main(int argc, const char* argv[])
{
    GenerateCalibLabelSet();
    string oFolder = "/media/ssd/data/aflw/data/neg24x24";
    string listFolder = "/media/ssd/data/aflw/data";
    
    int min_FaceSize; float scaleStep; int spacing;
    min_FaceSize = 40; scaleStep = 1.18; spacing = 4;
    FaceDetector facedetector(min_FaceSize, scaleStep, spacing);
    facedetector.LoadConfigs("/home/fanglin/caffe/FaceDetection/faceConfig_2nd.txt");
    
    vector<string> folderNames;
    folderNames.push_back("/media/ssd/data/VOC2007/nonPerson");
    //folderNames.push_back("/media/ssd/data/VOC2007/personHeadMasked");
    
    vector<Mat> imgs;    

    for (int k = 0; k < folderNames.size(); k++) {
        string folderName = folderNames[k];
        vector<string> filePaths;
        GetFilePaths(folderName, ".jpg|.JPG", filePaths);
        //imgs.resize(oldSize+filePaths.size());
        for (size_t i = 0; i < filePaths.size(); i++) {
            Mat img = imread(filePaths[i]);
            if (!img.empty())
                imgs.push_back(img);
        }
    }
    
    ofstream negTrListPath(string(listFolder+"/neg24_train.txt").c_str());
    ofstream negValListPath(string(listFolder+"/neg24_val.txt").c_str());
    
    long long nTotalWindows = 0;
    long long nDetected = 0;
    for (int i = 0; i < imgs.size(); i++) {
        Mat img = imgs[i];
        vector<Rect> rects;
        vector<float> scores;
        tic();
        int nWs = facedetector.Detect(img, rects, scores);
        
        cout << "Total sliding windows " << nWs << endl;
        cout << "Detected faces " << rects.size() << endl; 
        
        
        Mat img_ext;
        Size extSize(img.cols/2, img.rows/2);        
        cv::copyMakeBorder(img, img_ext, extSize.height, extSize.height, 
        extSize.width, extSize.width, BORDER_REPLICATE, CV_RGB(0,0,0));
    
        for (int j = 0; j < rects.size(); j++) {
            Mat rsz, patch;
            Rect rect_ext = rects[j];
            rect_ext.x += extSize.width;
            rect_ext.y += extSize.height;      
            /*
            if (rects[j].x < 0 || rects[j].y < 0 || 
                    rects[j].br().x > img.cols -1 || rects[j].br().y > img.rows -1)
                continue;
             */
            patch = img_ext(rect_ext);

            cv::resize(patch, rsz, cv::Size(24, 24));
            stringstream ss;
            ss << oFolder << "/neg_24x24_" << i << "_" << j << ".bmp";
            imwrite(ss.str(), rsz);
            if (imgs.size() -i > 100)
                negTrListPath << ss.str() << " " << 0 << endl;
            else
                negValListPath << ss.str() << " " << 0 << endl;
        }
        
        for (int j = 0; j < rects.size(); j++) {
            // Expand by 20% vertically            
            cv::rectangle(img, rects[j], CV_RGB(255, 0, 0), 2);            
        }
        nTotalWindows += nWs;
        nDetected += rects.size();
        imshow("img", img);
        
        char c = cv::waitKey(1);
        if (c == 'q')
            break;
    }
    negTrListPath.close();
    negValListPath.close();
    
    cout << "Total negatives: " << nDetected << endl;
    cout << "Average faces per image " << 1.0*nDetected/imgs.size() << endl;
    return 0;
}

int mainch( int argc, char** argv )
{
 srand(time(NULL));
    DIR *dir;
    struct dirent *ent;
    std::string ext = "png";
    std::string s = "";
    string pwd = "";
   //   ofstream file("/Users/sanjeevsingh/Desktop/cv2.txt");


    // approxPolyDP(<#InputArray curve#>, <#OutputArray approxCurve#>, <#double epsilon#>, <#bool closed#>)
    std::vector<cv::Point> polygon;
    //    polygon.push_back(Point(400,236));
    //    polygon.push_back(Point(640,390));
    //    polygon.push_back(Point(640,240));
    //    polygon.push_back(Point(365,145));
    
    polygon.push_back(Point(411,348));
    polygon.push_back(Point(552,480));
    polygon.push_back(Point(640,352));
    polygon.push_back(Point(400,288));
    
//    polygon.push_back(Point(861,463));
//    polygon.push_back(Point(1092,706));
//    polygon.push_back(Point(1280,394));
//    polygon.push_back(Point(790,267));
    
    Rect minRect = boundingRect(polygon);
    
    Mat polyMat = Mat::zeros(480, 640, CV_8UC1);
  //  Mat polyMat = Mat::zeros(720, 1280, CV_8UC1);
    //fills the roi of the visible road
    fillConvexPoly(polyMat, polygon, Scalar(125), 8, 0);
    //rows = 480 = height, cols = 640 = width;
 //   Mat mask = polyMat(Rect(minRect.x, minRect.y, minRect.width-1, minRect.height-1));
    Mat mask = polyMat(Rect(minRect.x, minRect.y, 100, 100));

    Mat image = Mat::zeros( 480, 640, CV_8UC3 );
   // Mat image = Mat::zeros( 720, 1280, CV_8UC3 );
    Mat tpl = imread("/Users/sanjeevsingh/Desktop/drawn.png",0);
    Canny(tpl, tpl, 5, 50, 3);
    namedWindow("result", CV_WINDOW_AUTOSIZE );
    String filename;
    char* path = "/Volumes/MacintoshHD2/SideEye/videos/Vehicle/2/";
   // char* path =  "/Volumes/MacintoshHD2/SideEye/images/meng4/Vehicle/3/";
    if ((dir = opendir (path)) != NULL) {
        /* print all the files and directories within directory */
        while ((ent = readdir (dir)) != NULL) {
            std::stringstream sstm;
            std::string fname = ent->d_name;  // filename
            // if filename's last characters are extension
            
            if (fname.find(ext, (fname.length() - ext.length())) != std::string::npos){
                sstm << path<< fname;
                filename = sstm.str();
                image = imread(filename,CV_LOAD_IMAGE_COLOR);
               // image = imread("/Volumes/MacintoshHD2/SideEye/videos/Vehicle/2/100.png",CV_LOAD_IMAGE_COLOR);
        Mat currGray;
        Mat output;
        Mat roi;
        Mat result;
            //    currGray = image;
        cvtColor(image, currGray, CV_BGR2GRAY);
        roi = currGray(Rect(minRect.x, minRect.y - 60
                            , minRect.width-60
                            -1, minRect.height-1));
            //    roi = currGray(Rect(minRect.x, minRect.y - 30
              //                      , 100 , 100));
                roi.copyTo(output);//, mask);


  //  Mat img = imread("/Users/sanjeevsingh/Desktop/caredge.png",0);
    //Mat cimg;
    cvtColor(roi, result, CV_GRAY2BGR);

    
    // if the image and the template are not edge maps but normal grayscale images,
    // you might want to uncomment the lines below to produce the maps. You can also
    // run Sobel instead of Canny.
    
    Canny(output, output, 68, 100, 3);
    tpl = imread("/Users/sanjeevsingh/Desktop/drawn.png",0);
   // imshow("roi", output);
  //  imshow("tpl", tpl);
  //  waitKey(0);
    vector<vector<Point> > results;
    vector<float> costs;
                double templScale = 1;
                int maxMatches = 1; //20
                double minMatchDistance = 1.0;
                int padX = 3;
                int padY = 3;
                int scales = 5;
                double minScale = 0.6;
                double maxScale = 1.6;
                double orientationWeight = 0.5;
                double truncate = 20;

 //   int best = chamerMatching( roi, tpl, results, costs );
//.........这里部分代码省略.........

  void okaoClientCallback(
			  const sensor_msgs::ImageConstPtr& imgmsg,
			  const humans_msgs::HumansConstPtr& kinect
			  )
  {  
    int okao_i = 0;
    int no_okao_i = 0; 
    humans_msgs::Humans okao_human, no_okao_human;   
    cv_bridge::CvImagePtr cv_ptr;
    try
      {
	cv_ptr = cv_bridge::toCvCopy(imgmsg, sensor_msgs::image_encodings::BGR8);
	for(int i = 0; i < kinect->num; i++)
	  {
	    //POS head2d, head3d, neck2d;
	    Point top, bottom;
	    geometry_msgs::Point head2d, neck2d;//, top, bottom;
	    head2d.x 
	      = kinect->human[i].body.joints[HEAD].position_color_space.x;	   
	    head2d.y 
	      = kinect->human[i].body.joints[HEAD].position_color_space.y;

	    neck2d.x 
	      = kinect->human[i].body.joints[SPINE_S].position_color_space.x;	   
	    neck2d.y 
	      = kinect->human[i].body.joints[SPINE_S].position_color_space.y;

	    double diff_w =  fabs(head2d.y-neck2d.y);
	    double diff_h =  fabs(head2d.y-neck2d.y);

	    top.x = head2d.x - diff_w;
	    top.y = head2d.y - diff_h;

	    bottom.x = head2d.x + diff_w;
	    bottom.y = head2d.y + diff_h;
	    /*
	    cout << "cut (" << cut.x << "," << cut.y << ")"<<endl;
	    if( cut.x < 0 )
	      cut.x = 0;
	    else if( cut.x >= (cv_ptr->image.cols - diff_w*2) )
	      cut.x = cv_ptr->image.cols-diff_w*2-1;

	    if( cut.y < 0 )
	      cut.y = 0;
	    else if( cut.y >= (cv_ptr->image.rows - diff_h*2) )
	      cut.y = cv_ptr->image.rows-diff_h*2-1;
	    */
	    top.x = max(0, top.x);
	    top.y = max(0, top.y);
	    bottom.x = min(cv_ptr->image.cols-1, bottom.x);
	    bottom.y = min(cv_ptr->image.rows-1, bottom.y);

	    if (( top.x > bottom.x || top.y > bottom.y)||( top.x == bottom.x || top.y == bottom.y))
	      continue;
	    /*   
	    cout << "(" << top.x << "," << top.y << ")"
		 << "-"
		 << "(" << bottom.x << "," << bottom.y << ")"<<endl;
	    cout << "diff:" << "(" << diff_w << "," << diff_h<< ")" << endl;
	    cout << "image:" << "(" << cv_ptr->image.cols << "," << cv_ptr->image.rows << ")" << endl;
	    */
	    Mat cutRgbImage;
	    try
	      {
		cutRgbImage = Mat(cv_ptr->image, cv::Rect(top, bottom));
	      }
	    catch(cv_bridge::Exception& e)
	      {
		ROS_ERROR("cv_bridge exception: %s",e.what());
	      }
	    Mat rgbImage = cutRgbImage.clone();
	    if( rgbImage.cols > 1280 )
	      {
		cv::resize( rgbImage, rgbImage, 
			    cv::Size(1280, cutRgbImage.rows*1280/cutRgbImage.cols) );	
	      }
	    if( rgbImage.rows > 1024 )
	      {
		cv::resize( rgbImage, rgbImage, 
			    cv::Size(cutRgbImage.cols*1024/cutRgbImage.rows , 1024) );
	      }	

	    //rgbImage = cutRgbImage;	     
	    Mat grayImage;	   
	    cv::cvtColor(rgbImage,grayImage,CV_BGR2GRAY);

	    //test
	    cv::rectangle( cv_ptr->image, top,
			   bottom,
			   cv::Scalar(0,200,0), 5, 8);
	    
	    try
	      {
		cv::Mat img = grayImage;
		std::vector<unsigned char> 
		  buf(img.data, img.data + img.cols * img.rows * img.channels());

		std::vector<int> encodeParam(2);
		encodeParam[0] = CV_IMWRITE_PNG_COMPRESSION;
		encodeParam[1] = 3;
//.........这里部分代码省略.........

int main(int argc, const char** argv) {
    string haystack_path, needle_path;
    if (argc > 2) {
        haystack_path = argv[1];
        needle_path = argv[2];
    }
    if (haystack_path.empty()) {
        showHelp(argv[0]);
        return 1;
    }

    Mat haystack = imread(haystack_path, CV_LOAD_IMAGE_COLOR);
    Mat needle = imread(needle_path, CV_LOAD_IMAGE_COLOR);
    if (haystack.empty() || needle.empty()) {
        cerr << "Couldn't load images!" << endl;
        return 1;
    }

    Mat haystack_sum;
    tpl::integral(haystack, haystack_sum);

    Mat needle_sum;
    tpl::integral(needle, needle_sum);

    int ns = needle_sum.at<int>(needle_sum.rows - 1, needle_sum.cols - 1);

    // Contains candidate results.
    deque<item> deq;

    for (int y = 0; y < haystack_sum.rows - needle_sum.rows; ++y) {
        for (int x = 0; x < haystack_sum.cols - needle_sum.cols; ++x) {
            int s = haystack_sum.at<int>(y, x) +
                haystack_sum.at<int>(y + needle_sum.rows, x + needle_sum.cols) -
                haystack_sum.at<int>(y, x + needle_sum.cols) -
                haystack_sum.at<int>(y + needle_sum.rows, x);

            int d = abs(s - ns);
            item itm = {d, x, y, s};
            if (deq.size() > 0) {
                for (auto it = deq.begin(); it != deq.end(); ++it) {
                    // Need to store candidates to check further.
                    if (d < it->diff) {
                        deq.insert(it, itm);
                        break;
                    }
                }
            } else {
                deq.push_front(itm);
            }

            if (deq.size() > 50) {
                deq.pop_back();
            }
        }
    }

    const int nc = 3; // Number of channels.
    const int byte = 255; // One byte.
    const int max = needle.rows * needle.cols * byte * nc; // Maximum value that can be in comparing by brute force.

    float result = !deq.empty() && deq[0].diff == 0 ? 1 : 0;
    int min = INT_MAX;
    // Result point.
    int rx = !deq.empty() && deq[0].diff == 0 ? deq[0].x : -1;
    int ry = !deq.empty() && deq[0].diff == 0 ? deq[0].y : -1;

    // If perfect result has not been found -> need to find it by brute force.
    if (result != 1) {
        for (int d = 0; d < deq.size(); ++d) {
            unsigned long long s = 0;
            for (int j = 0; j < needle.rows; ++j) {
                for (int i = 0; i < needle.cols; ++i) {
                    Vec3b c1 = haystack.at<Vec3b>(Point(deq[d].x + i, deq[d].y + j));
                    Vec3b c2 = needle.at<Vec3b>(Point(i, j));
                    s += abs(c1.val[0] - c2.val[0]);
                    s += abs(c1.val[1] - c2.val[1]);
                    s += abs(c1.val[2] - c2.val[2]);
                }
            }

            if (s < min) {
                min = s;
                rx = deq[d].x;
                ry = deq[d].y;
                result = 1 - (float(min) / max);
            }
            if (s == 0) {
                break;
            }
        }
    }

    cout << "Result: " << result << endl;
    if (result) {
        cout << "Found at [" << rx << "," << ry << "]" << endl;
        rectangle(haystack, Point(rx, ry), Point(rx + needle.cols, ry + needle.rows), Scalar::all(0), 2, 8, 0);
        imshow("Result", haystack);
        waitKey(0);
    }

//.........这里部分代码省略.........

int main(int argc, char const *argv[])
{
  int board_width, board_height, num_imgs;
  float square_size;
  char* img_dir;
  char* leftimg_filename;
  char* rightimg_filename;
  char* out_file;

  static struct poptOption options[] = {
    { "board_width",'w',POPT_ARG_INT,&board_width,0,"Checkerboard width","NUM" },
    { "board_height",'h',POPT_ARG_INT,&board_height,0,"Checkerboard height","NUM" },
    { "square_size",'s',POPT_ARG_FLOAT,&square_size,0,"Checkerboard square size","NUM" },
    { "num_imgs",'n',POPT_ARG_INT,&num_imgs,0,"Number of checkerboard images","NUM" },
    { "img_dir",'d',POPT_ARG_STRING,&img_dir,0,"Directory containing images","STR" },
    { "leftimg_filename",'l',POPT_ARG_STRING,&leftimg_filename,0,"Left image prefix","STR" },
    { "rightimg_filename",'r',POPT_ARG_STRING,&rightimg_filename,0,"Right image prefix","STR" },
    { "out_file",'o',POPT_ARG_STRING,&out_file,0,"Output calibration filename (YML)","STR" },
    POPT_AUTOHELP
    { NULL, 0, 0, NULL, 0, NULL, NULL }
  };

  POpt popt(NULL, argc, argv, options, 0);
  int c;
  while((c = popt.getNextOpt()) >= 0) {}

  load_image_points(board_width, board_height, square_size, num_imgs, img_dir, leftimg_filename, rightimg_filename);

  printf("Starting Calibration\n");
  cv::Matx33d K1, K2, R;
  cv::Vec3d T;
  cv::Vec4d D1, D2;
  int flag = 0;
  flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
  flag |= cv::fisheye::CALIB_CHECK_COND;
  flag |= cv::fisheye::CALIB_FIX_SKEW;
  //flag |= cv::fisheye::CALIB_FIX_K2;
  //flag |= cv::fisheye::CALIB_FIX_K3;
  //flag |= cv::fisheye::CALIB_FIX_K4;
  cv::fisheye::stereoCalibrate(object_points, left_img_points, right_img_points,
      K1, D1, K2, D2, img1.size(), R, T, flag,
      cv::TermCriteria(3, 12, 0));

  cv::FileStorage fs1(out_file, cv::FileStorage::WRITE);
  fs1 << "K1" << Mat(K1);
  fs1 << "K2" << Mat(K2);
  fs1 << "D1" << D1;
  fs1 << "D2" << D2;
  fs1 << "R" << Mat(R);
  fs1 << "T" << T;
  printf("Done Calibration\n");

  printf("Starting Rectification\n");

  cv::Mat R1, R2, P1, P2, Q;
  cv::fisheye::stereoRectify(K1, D1, K2, D2, img1.size(), R, T, R1, R2, P1, P2, 
Q, CV_CALIB_ZERO_DISPARITY, img1.size(), 0.0, 1.1);

  fs1 << "R1" << R1;
  fs1 << "R2" << R2;
  fs1 << "P1" << P1;
  fs1 << "P2" << P2;
  fs1 << "Q" << Q;

  printf("Done Rectification\n");
  return 0;
}

Void Mat::SetSize(const Mat &m)
{
    SetSize(m.Rows(), m.Cols());
}

int main(int argc, char** argv)
{
    bool ok = false;
    Size boardSize, subPixSize;
    string xmlImages, savePath, ymlIntrinsic1, ymlIntrinsic2;
    vector<string> fileList;
    vector<Mat> images;
    vector<Mat> undistortedImages;
    Mat cameraMatrix1, cameraMatrix2, distCoeffs1, distCoeffs2;
    vector<vector<Point2f> > cornersCam1, cornersCam2;
    vector<Point3f> idealCorners;
    Mat R, T;
    Mat noDist = Mat::zeros(5,1, CV_32F);
    Size imageSize;
    vector<Mat> rvecs1, tvecs1, rvecs2, tvecs2;
    Mat R1, R2, P1, P2, Q;
    Rect validRoi[2];
    float squareSize;

    ok = readStereoParams(string(argv[1]), boardSize, squareSize, xmlImages, subPixSize, savePath, ymlIntrinsic1, ymlIntrinsic2);

    if (ok)
    {
        cout << "[Extrinsic] load image list!" << endl;
        ok = loadImageList(xmlImages, fileList);

        if (!ok)
            cout << "[Extrinsic] error! could not load image list!" << endl;
    }

    if (ok)
    {
        cout << "[Extrinsic] load images!" << endl;
        ok = loadImages(fileList, images);

        if (!ok)
            cout << "[Extrinsic] error! could not load images!" << endl;
    }

    if (ok)
    {
        boost::filesystem::path dir(savePath);
        boost::filesystem::create_directories(dir);

        cout << "[Extrinsic] load intrinsics and undistort images!" << endl;
        loadStereoIntrinsics(ymlIntrinsic1, ymlIntrinsic2,
                             cameraMatrix1, cameraMatrix2,distCoeffs1, distCoeffs2);

        ok = undistortStereoImages(images, undistortedImages,
                                   cameraMatrix1, cameraMatrix2,distCoeffs1, distCoeffs2);

        if (!ok)
            cout << "[Extrinsic] error! could not load intrinsics and undistort images!" << endl;
    }

    if (ok)
    {
        cout << "[Extrinsic] find corners!" << endl;

        ok = findCornersStereo(boardSize, subPixSize, undistortedImages, cornersCam1, cornersCam2);

        if (!ok)
            cout << "[Extrinsic] error! could not find corners!" << endl;
    }


    if (ok)
    {
        rvecs1.resize(cornersCam1.size());
        tvecs1.resize(cornersCam1.size());
        rvecs2.resize(cornersCam1.size());
        tvecs2.resize(cornersCam1.size());

        cout << "[Extrinsic] calculate extrinsic with " << cornersCam1.size() << " pairs!" << endl;
        calcBoardCornerPositions(boardSize, squareSize, idealCorners);

        for (int i = 0; i < cornersCam1.size(); i++)
        {
            solvePnP(idealCorners, cornersCam1[i], cameraMatrix1, noDist, rvecs1[i], tvecs1[i]);
            solvePnP(idealCorners, cornersCam2[i], cameraMatrix2, noDist, rvecs2[i], tvecs2[i]);
        }

        ok = calculateRT(tvecs1, tvecs2, R, T);

        if (!ok)
            cout << "[Extrinsic] error! could not calculate extrinsic!" << endl;
    }

    if (ok)
    {
        cout << "[Extrinsic] save extrinsic parameters!" << endl;
        imageSize = ((Mat)images[0]).size();
        stereoRectify(cameraMatrix1, noDist,
                      cameraMatrix2, noDist,
                      imageSize, R, T, R1, R2, P1, P2, Q,
                      CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]);

        string ymlPath(savePath);
        ymlPath += "extrinsics.yml";

//.........这里部分代码省略.........

    void setPixelValue(Mat &M,Point p,T value)
    {

        M.data+ M.step*p.y + p.x*M.elemSize()=value;
    }

void BOW::computeVideoFeatures( std::string fileName, vector<STFeature>& featureVec )
{
	const int procUnitLen = 20;
	const int intervalLen = 5;
	const int width = 320;
	const int height = 240;

	CProfiler prof;

	//Open video
	VideoCapture cap( fileName );
	Mat frame;
	Mat frameg;
	Mat frameg2;
	Mat hardCopy;

	vector<STFeature> tempFeat;
	vector<cv::Mat> frames;
	vector<cv::Mat> frameQueue;

	int frameCpt = 0;
	int nbFrameTotal = cap.get(CV_CAP_PROP_FRAME_COUNT);

	featureVec.clear();
	featureVec.reserve( 10000 );

	cout << "Processing " << fileName << "..." << endl;
	cout << "Total number of frames: " << nbFrameTotal << endl;

	int lastPercent = 0;
	double lastFrameRate = 0;
	double accFrameRate = 0;
	int accFrameRateCpt = 0;

	for(;;)
	{
		cap >> frame;
		if( frame.empty() )
			break;

		/*
		double pos = cap.get( CV_CAP_PROP_POS_AVI_RATIO );
		if( lastPercent != (int)(ceil(pos*100)) )
		{
			double meanFrameRate = accFrameRate/(double)accFrameRateCpt;
			accFrameRate = 0;
			accFrameRateCpt = 0;
			lastPercent = (int)(ceil(pos*100));
			cout << lastPercent << "%" << " - " << meanFrameRate << " fps" << endl;
		}
		else
		{
			accFrameRate += lastFrameRate;
			accFrameRateCpt++;
		}
		*/

		cv::cvtColor( frame, frameg, CV_RGB2GRAY );
		cv::resize( frameg, frameg2, cv::Size(width,height));
		hardCopy = frameg2.clone();

		//we reached processing unit length
		if( frameQueue.size() == procUnitLen )
		{
			prof.start();
			tempFeat.clear();

			/*
			* new arch
			*/
				if( _extractor != 0 )
				{
					_extractor->computeFeatures( frameQueue, frameCpt );
					tempFeat = _extractor->getFeatures();
				}
			/*
			*/

			featureVec.insert(featureVec.end(), tempFeat.begin(), tempFeat.end());
			prof.stop();
			frameQueue.clear();
			lastFrameRate = 1.0/prof.getSeconds();
		}
		frameQueue.push_back( hardCopy );
		//
		
		frameCpt++;
	}
	cout << "Done!" << endl;
}