int resid(CvMat* Y, CvMat* u, CvMat** Q, int num_Q_mats, CvMat* e, CvMat* J)
{
	int i, mat_id;
	CvMat* q;
	CvMat* x0;
	CvMat* x;
	CvMat* temp_mat1;
	CvMat* temp_mat2;
	CvMat* temp_mat3_1;
	CvMat* temp_mat3_2;

	/* Create the initial e and J matrices */
//	e = cvCreateMat(0, 0, CV_64FC1);
//	J = cvCreateMat(0, 0, CV_64FC1);

	for(mat_id = 0 ; mat_id < num_Q_mats ; mat_id++)
	{

		/* Copy data into the new matrix */
		q  = sfmGetCols(Q[mat_id], 0, 2);
		/* Copy data to form the x0 matrix */
		x0 = sfmGetCols(Q[mat_id],3);

		/* x = q*Y + x0 */
		x=sfmMatMul(q, Y);
		cvAdd(x, x0, x);
		/*
		//debug
		printf("....x\n");
		CvMat_printdb(stdout,"%7.3f ",x);
*/
		/* Temporary matrix to aid evaluations of e */
		temp_mat1 = cvCreateMat(2, 1, CV_64FC1);
		temp_mat2 = cvCreateMat(2, 1, CV_64FC1);

		/* Evaluate and save temporary data */
		temp_mat1->data.db[0] = x->data.db[0] / x->data.db[2];
		temp_mat1->data.db[1] = x->data.db[1] / x->data.db[2];

		temp_mat2->data.db[0] = u->data.db[0*u->cols + mat_id];
		temp_mat2->data.db[1] = u->data.db[1*u->cols + mat_id];

		/* e = [e; x(1:2)/x(3)-u(1:2,k)] */
		cvSub(temp_mat1,temp_mat2,temp_mat1);
		PutMatV(temp_mat1,e,mat_id*2);

		/*
		//debug
		printf("....e\n");
		CvMat_printdb(stdout,"%7.3f ",e);
*/
		/* Temporary matrix to aid evaluations of Y */
		cvReleaseMat(&temp_mat2);
		cvReleaseMat(&temp_mat1);
		//memory leak
		temp_mat1   = cvCreateMat(1, 3, CV_64FC1);
		temp_mat2   = cvCreateMat(1, 3, CV_64FC1);
		temp_mat3_1 = cvCreateMat(1, 3, CV_64FC1);
		temp_mat3_2 = cvCreateMat(1, 3, CV_64FC1);

		for(i = 0 ; i < 3 ; i++)
		{
			/* Copy data into the temp matrices */
			temp_mat1->data.db[i] = (q->data.db[i] * x->data.db[2]) / 
				                    (x->data.db[2] * x->data.db[2]); /* x(3)*q(1,:) / x(3)^2 */

			temp_mat2->data.db[i] = (q->data.db[i + 3] * x->data.db[2]) / 
				                    (x->data.db[2] * x->data.db[2]); /* x(3)*q(2,:) / x(3)^2 */

			temp_mat3_1->data.db[i] = (q->data.db[i + 6] * x->data.db[0]) / 
				                      (x->data.db[2] * x->data.db[2]); /* x(1)*q(3,:) / x(3)^2 */

			temp_mat3_2->data.db[i] = (q->data.db[i + 6] * x->data.db[1]) / 
				                      (x->data.db[2] * x->data.db[2]); /* x(2)*q(3,:) / x(3)^2 */
		}
		/*
		//for debug
		printf("....temp_mat1\n");
		CvMat_printdb(stdout,"%7.3f ",temp_mat1);
		printf("....temp_mat2\n");
		CvMat_printdb(stdout,"%7.3f ",temp_mat2);

		printf("....temp_mat3_1\n");
		CvMat_printdb(stdout,"%7.3f ",temp_mat3_1);
		printf("....temp_mat3_2\n");
		CvMat_printdb(stdout,"%7.3f ",temp_mat3_2);
*/

		/* x(3)*q(1,:)-x(1)*q(3,:) */
		cvSub(temp_mat1, temp_mat3_1, temp_mat1);

		/* x(3)*q(2,:)-x(2)*q(3,:) */
		cvSub(temp_mat2, temp_mat3_2, temp_mat2);

		/* [x(3)*q(1,:)-x(1)*q(3,:) x(3)*q(2,:)-x(2)*q(3,:)]/x(3)^2 */
		temp_mat1 = cat(temp_mat1, temp_mat2, 1);		
		
		/* J = [J; [x(3)*q(1,:)-x(1)*q(3,:) x(3)*q(2,:)-x(2)*q(3,:)]/x(3)^2] */
		PutMatV(temp_mat1,J,mat_id*2);
		/*
//.........这里部分代码省略.........

 ~pstable_l2_func() {
   cvReleaseMat(&a);
   cvReleaseMat(&b);
   cvReleaseMat(&r1);
   cvReleaseMat(&r2);
 }

static void*
imread_( const String& filename, int flags, int hdrtype, Mat* mat=0 )
{
    IplImage* image = 0;
    CvMat *matrix = 0;
    Mat temp, *data = &temp;

    ImageDecoder decoder = findDecoder(filename);
    if( !decoder.obj )
        return 0;
    decoder->setSource(filename);
    if( !decoder->readHeader() )
        return 0;

    CvSize size;
    size.width = decoder->width();
    size.height = decoder->height();

    int type = decoder->type();
    if( flags != -1 )
    {
        if( (flags & CV_LOAD_IMAGE_ANYDEPTH) == 0 )
            type = CV_MAKETYPE(CV_8U, CV_MAT_CN(type));

        if( (flags & CV_LOAD_IMAGE_COLOR) != 0 ||
           ((flags & CV_LOAD_IMAGE_ANYCOLOR) != 0 && CV_MAT_CN(type) > 1) )
            type = CV_MAKETYPE(CV_MAT_DEPTH(type), 3);
        else
            type = CV_MAKETYPE(CV_MAT_DEPTH(type), 1);
    }

    if( hdrtype == LOAD_CVMAT || hdrtype == LOAD_MAT )
    {
        if( hdrtype == LOAD_CVMAT )
        {
            matrix = cvCreateMat( size.height, size.width, type );
            temp = cvarrToMat(matrix);
        }
        else
        {
            mat->create( size.height, size.width, type );
            data = mat;
        }
    }
    else
    {
        image = cvCreateImage( size, cvIplDepth(type), CV_MAT_CN(type) );
        temp = cvarrToMat(image);
    }

    if( !decoder->readData( *data ))
    {
        cvReleaseImage( &image );
        cvReleaseMat( &matrix );
        if( mat )
            mat->release();
        return 0;
    }

    return hdrtype == LOAD_CVMAT ? (void*)matrix :
        hdrtype == LOAD_IMAGE ? (void*)image : (void*)mat;
}

//.........这里部分代码省略.........
    /* Loop for test some random object positions */
    for( counter = start_from; counter < test_case_count; counter++ )
    {
        ts->update_context( this, counter, true );
        progress = update_progress( progress, counter, test_case_count, 0 );
        
        /* set all rotation matrix to zero */
        cvZero( true_rotationX );
        cvZero( true_rotationY );
        cvZero( true_rotationZ );
        
        /* fill random rotation matrix */
        angleX = (float)(cvTsRandReal(rng)*2*CV_PI);
        angleY = (float)(cvTsRandReal(rng)*2*CV_PI);
        angleZ = (float)(cvTsRandReal(rng)*2*CV_PI);

        true_rotationX->data.fl[0 *3+ 0] = 1;
        true_rotationX->data.fl[1 *3+ 1] = (float)cos(angleX);
        true_rotationX->data.fl[2 *3+ 2] = true_rotationX->data.fl[1 *3+ 1];
        true_rotationX->data.fl[1 *3+ 2] = -(float)sin(angleX);
        true_rotationX->data.fl[2 *3+ 1] = -true_rotationX->data.fl[1 *3+ 2];

        true_rotationY->data.fl[1 *3+ 1] = 1;
        true_rotationY->data.fl[0 *3+ 0] = (float)cos(angleY);
        true_rotationY->data.fl[2 *3+ 2] = true_rotationY->data.fl[0 *3+ 0];
        true_rotationY->data.fl[0 *3+ 2] = -(float)sin(angleY);
        true_rotationY->data.fl[2 *3+ 0] = -true_rotationY->data.fl[0 *3+ 2];

        true_rotationZ->data.fl[2 *3+ 2] = 1;
        true_rotationZ->data.fl[0 *3+ 0] = (float)cos(angleZ);
        true_rotationZ->data.fl[1 *3+ 1] = true_rotationZ->data.fl[0 *3+ 0];
        true_rotationZ->data.fl[0 *3+ 1] = -(float)sin(angleZ);
        true_rotationZ->data.fl[1 *3+ 0] = -true_rotationZ->data.fl[0 *3+ 1];

        cvMatMul( true_rotationX, true_rotationY, tmp_matrix);
        cvMatMul( tmp_matrix, true_rotationZ, true_rotation);

        /* fill translation vector */
        true_translation->data.fl[2] = (float)(cvRandReal(rng)*(2*flFocalLength-40) + 40);
        true_translation->data.fl[0] = (float)((cvRandReal(rng)*2-1)*true_translation->data.fl[2]);
        true_translation->data.fl[1] = (float)((cvRandReal(rng)*2-1)*true_translation->data.fl[2]);

        /* calculate perspective projection */
        for ( i = 0; i < 8; i++ )
        {
            float vec[3];
            CvMat Vec = cvMat( 3, 1, CV_MAT32F, vec );
            CvMat Obj_point = cvMat( 3, 1, CV_MAT32F, &obj_points[i].x );

            cvMatMul( true_rotation, &Obj_point, &Vec );

            vec[0] += true_translation->data.fl[0];
            vec[1] += true_translation->data.fl[1];
            vec[2] += true_translation->data.fl[2];

            img_points[i].x = flFocalLength * vec[0] / vec[2];
            img_points[i].y = flFocalLength * vec[1] / vec[2];
        }

        /*img_points[0].x = 0 ; img_points[0].y =   0;
        img_points[1].x = 80; img_points[1].y = -93;
        img_points[2].x = 245;img_points[2].y =  -77;
        img_points[3].x = 185;img_points[3].y =  32;
        img_points[4].x = 32; img_points[4].y = 135;
        img_points[5].x = 99; img_points[5].y = 35;
        img_points[6].x = 247; img_points[6].y = 62;
        img_points[7].x = 195; img_points[7].y = 179;
        */

        object = cvCreatePOSITObject( obj_points, 8 );
        cvPOSIT( object, img_points, flFocalLength, criteria,
                 rotation->data.fl, translation->data.fl );
        cvReleasePOSITObject( &object );

        code = cvTsCmpEps2( ts, rotation, true_rotation, flEpsilon, false, "rotation matrix" );
        if( code < 0 )
            goto _exit_;

        code = cvTsCmpEps2( ts, translation, true_translation, flEpsilon, false, "translation vector" );
        if( code < 0 )
            goto _exit_;
    }

_exit_:

    cvFree( &obj_points );
    cvFree( &img_points );

    cvReleaseMat( &true_rotationX );
    cvReleaseMat( &true_rotationY );
    cvReleaseMat( &true_rotationZ );
    cvReleaseMat( &tmp_matrix );
    cvReleaseMat( &true_rotation );
    cvReleaseMat( &rotation );
    cvReleaseMat( &translation );
    cvReleaseMat( &true_translation );

    if( code < 0 )
        ts->set_failed_test_info( code );
}

//.........这里部分代码省略.........
        CvScalar _templ_sdv = cvScalarAll(0);
        CV_CALL( sqsum = cvCreateMat( img->rows + 1, img->cols + 1,
                                      CV_MAKETYPE( CV_64F, cn )));
        CV_CALL( cvIntegral( img, sum, sqsum, 0 ));
        CV_CALL( cvAvgSdv( templ, &templ_mean, &_templ_sdv ));

        templ_norm = CV_SQR(_templ_sdv.val[0]) + CV_SQR(_templ_sdv.val[1]) +
                    CV_SQR(_templ_sdv.val[2]) + CV_SQR(_templ_sdv.val[3]);

        if( templ_norm < DBL_EPSILON && method == CV_TM_CCOEFF_NORMED )
        {
            cvSet( result, cvScalarAll(1.) );
            EXIT;
        }
        
        templ_sum2 = templ_norm +
                     CV_SQR(templ_mean.val[0]) + CV_SQR(templ_mean.val[1]) +
                     CV_SQR(templ_mean.val[2]) + CV_SQR(templ_mean.val[3]);

        if( num_type != 1 )
        {
            templ_mean = cvScalarAll(0);
            templ_norm = templ_sum2;
        }
        
        templ_sum2 /= inv_area;
        templ_norm = sqrt(templ_norm);
        templ_norm /= sqrt(inv_area); // care of accuracy here

        q0 = (double*)sqsum->data.ptr;
        q1 = q0 + templ->cols*cn;
        q2 = (double*)(sqsum->data.ptr + templ->rows*sqsum->step);
        q3 = q2 + templ->cols*cn;
    }

    p0 = (double*)sum->data.ptr;
    p1 = p0 + templ->cols*cn;
    p2 = (double*)(sum->data.ptr + templ->rows*sum->step);
    p3 = p2 + templ->cols*cn;

    sum_step = sum ? sum->step / sizeof(double) : 0;
    sqsum_step = sqsum ? sqsum->step / sizeof(double) : 0;

    for( i = 0; i < result->rows; i++ )
    {
        float* rrow = (float*)(result->data.ptr + i*result->step);
        idx = i * sum_step;
        idx2 = i * sqsum_step;

        for( j = 0; j < result->cols; j++, idx += cn, idx2 += cn )
        {
            double num = rrow[j], t;
            double wnd_mean2 = 0, wnd_sum2 = 0;
            
            if( num_type == 1 )
            {
                for( k = 0; k < cn; k++ )
                {
                    t = p0[idx+k] - p1[idx+k] - p2[idx+k] + p3[idx+k];
                    wnd_mean2 += CV_SQR(t);
                    num -= t*templ_mean.val[k];
                }

                wnd_mean2 *= inv_area;
            }

            if( is_normed || num_type == 2 )
            {
                for( k = 0; k < cn; k++ )
                {
                    t = q0[idx2+k] - q1[idx2+k] - q2[idx2+k] + q3[idx2+k];
                    wnd_sum2 += t;
                }

                if( num_type == 2 )
                    num = wnd_sum2 - 2*num + templ_sum2;
            }

            if( is_normed )
            {
                t = sqrt(MAX(wnd_sum2 - wnd_mean2,0))*templ_norm;
                if( t > DBL_EPSILON )
                {
                    num /= t;
                    if( fabs(num) > 1. )
                        num = num > 0 ? 1 : -1;
                }
                else
                    num = method != CV_TM_SQDIFF_NORMED || num < DBL_EPSILON ? 0 : 1;
            }

            rrow[j] = (float)num;
        }
    }
        
    __END__;

    cvReleaseMat( &sum );
    cvReleaseMat( &sqsum );
}

 ~DefHist()
 {
     if(m_pHist)cvReleaseMat(&m_pHist);
 }

void Cell::destroyCell ()
{
  if (getFlagStr() != INVALID_STR)
  {
    if (_rhs != NULL)
    {
      if (getRhsDim() > 1)
        delete[] _rhs;

      else
        delete _rhs;

      _rhs = NULL;
    }

    if (_detwindow != NULL)
    {
      delete[] _detwindow;
      _detwindow = NULL;
    }

    if (getFlagStr() == STR_ANCHOR)
    {
      if (_anchor != NULL)
      {
        for (int i = 0; i < getAnchorDim(); i++)
        {
          if (getAnchor()[i].array != NULL)
          {
            delete[] getAnchor()[i].array;
            getAnchor()[i].array = NULL;
          }
        }

        delete[] _anchor;
        _anchor = NULL;
      } 

      if (_score != NULL)
      {
        for (int i = 0; i < getScoreDim(); i++)
        {
          if (_score[i] != NULL)
          {
            cvReleaseMat (&_score[i]);
            _score[i] = NULL;
          }
        }
      
        delete[] _score;
        _score = NULL;
      }
    }

    if (_Ix != NULL)
    {
      for (int i = 0; i < getIxDim(); i++)
      {
        if (_Ix[i] != NULL)
        {
          cvReleaseMat (&_Ix[i]);
          _Ix[i] = NULL;
        }
      }

      delete[] _Ix;
      _Ix = NULL;
    }

    if (_Iy != NULL)
    {
      for (int i = 0; i < getIyDim(); i++)
      {
        if (_Iy[i] != NULL)
        {
          cvReleaseMat (&_Iy[i]);
          _Iy[i] = NULL;
        }
      }

      delete[]_Iy;
      _Iy = NULL;
    }
  }

  setFlagStr (INVALID_STR);
}

int main( int argc, char** argv ) {
  struct feature* feat1, * feat2, * feat;
  struct feature** nbrs;
  struct kd_node* kd_root;
  double d0, d1;
  int n1, n2, k, i, m = 0;

  if( argc != 3 )
      fatal_error( "usage: %s <keydescr1> <keydescr2>", argv[0] );

  n1 = import_features( argv[1], feat_type, &feat1 );
  n2 = import_features( argv[2], feat_type, &feat2 );

  if( n1 < 0 )
      fatal_error( "unable to load key descriptors from %s", argv[1] );
  if( n2 < 0 )
      fatal_error( "unable to load key descriptors from %s", argv[2] );

  printf("%d features presenti nel modello.\n", n1);
  printf("%d features presenti nell'immagine.\n", n2);
  kd_root = kdtree_build( feat2, n2 );
  for( i = 0; i < n1; i++ ) {
      feat = feat1 + i;
      k = kdtree_bbf_knn( kd_root, feat, 2, &nbrs, KDTREE_BBF_MAX_NN_CHKS );
      if( k == 2 ) {
        // Controlla
          d0 = descr_dist_sq( feat, nbrs[0] );
          d1 = descr_dist_sq( feat, nbrs[1] );
          if( d0 < d1 * NN_SQ_DIST_RATIO_THR ) {
              m++;
              feat1[i].fwd_match = nbrs[0];
          }
      }
      free( nbrs );
    }

#ifdef RANSAC
  {
    CvMat* H;
    IplImage* xformed;
    struct feature ** inliers;
    int n_inliers;
    H = ransac_xform( feat1, n1, FEATURE_FWD_MATCH, lsq_homog, 4, 0.01, homog_xfer_err, 8.0, &inliers, &n_inliers );
    if( H )
    {
      if ( cvmGet( H, 1, 2 ) < 120.0 &&
           cvmGet( H, 0, 2 ) < 120.0 &&
           (
            fabs(cvmGet( H, 0, 0)) >= 0.000001 ||
            fabs(cvmGet( H, 0, 1)) >= 0.000001 ||
            fabs(cvmGet( H, 1, 0)) >= 0.000001 ||
            fabs(cvmGet( H, 1, 1)) >= 0.000001 ||
            fabs(cvmGet( H, 2, 0)) >= 0.000001 ||
            fabs(cvmGet( H, 2, 1)) >= 0.000001
           )
          ) {
        // TROVATO
        printf("RANSAC OK\n");
        /*
        xformed = cvCreateImage( cvGetSize( img2 ), IPL_DEPTH_8U, 3 );
        cvWarpPerspective( img1, xformed, H, CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS, cvScalarAll( 0 ) );
        cvNamedWindow( "Xformed", 1 );
        cvShowImage( "Xformed", xformed );
        cvWaitKey( 0 );
        cvReleaseImage( &xformed );
        */
        printf("N. inliers: %d\n", n_inliers);
      }
      else {
        // Trovato ma probabilmente la matrice di trasformazione e' sbagliata.
        printf("RANSAC FAIL\n");
      }

      cvReleaseMat( &H );
    }
    else {
      printf("RANSAC FAIL\n");
    }
  }
#endif








    fprintf( stderr, "Found %d total matches\n", m );

    kdtree_release( kd_root );
    free( feat1 );
    free( feat2 );
    return 0;
}

void ShiftMapHierarchy::ComputeFastShiftMap(IplImage* input, IplImage* saliency, CvSize output)
{
	printf("Downsampling...");
	_imageList = new vector<IplImage*>(0);
	_labelMapList = new vector<CvMat*>(0);
	vector<IplImage*>* imageSList = new vector<IplImage*>(0);
	vector<CvSize>* outputSizeList = new vector<CvSize>(0);
	_imageList->push_back(input);
	imageSList->push_back(saliency);
	outputSizeList->push_back(output);

	IplImage* level = input;
	IplImage* levelS = saliency;	
	CvSize levelSize = output;

	int levelCount = 0;

	while(level->width >20 && level->height > 20)
	{
		IplImage* level_temp = cvCreateImage(cvSize(level->width/2, level->height/2), input->depth, input->nChannels);
		IplImage* levelS_temp = cvCreateImage(cvSize(level->width/2, level->height/2), input->depth, input->nChannels);
		
		DownSampling(level, level_temp);
		DownSampling(levelS, levelS_temp);
		// 
		//cvPyrDown(level, level_temp);
		//cvPyrDown(levelS, levelS_temp);		
		
		level = level_temp;
		levelS = levelS_temp; 
		levelCount++;

		CvSize output_temp;
		output_temp.width = levelSize.width / 2;
		output_temp.height = levelSize.height / 2;
		levelSize = output_temp;
		
		_imageList->push_back(level);
		imageSList->push_back(levelS);
		outputSizeList->push_back(levelSize);
	}  

	// dummy initialGuess - just to be released later
	 _initialGuess = cvCreateMat(10,10, CV_32SC2);
	
	 _level = levelCount;
	//int index = levelCount;
	for(int i = levelCount; i >= 0; i--)
	{ 
		if(i == levelCount)		
			// first level does not require an initialGuess
			ComputeShiftMap((*_imageList)[i], (*imageSList)[i], (*outputSizeList)[i], cvSize((*_imageList)[i]->width, (*_imageList)[i]->height));
		else
			ComputeShiftMapGuess((*_imageList)[i], (*imageSList)[i], _initialGuess, (*outputSizeList)[i], cvSize(3,3));
		//
		//if(i == levelCount - 3)
		//{
		//	IplImage* image = GetRetargetImage();
		//	//IplImage* image = (*imageSList)[i];
		//	cvNamedWindow("test");
		//	while(1)
		//	{
		//		cvShowImage("test", image);
		//		cvWaitKey(100);
		//	}
		//}
		// save result	

		

		CvMat* labelMap;
		if(i == levelCount)
			labelMap = CalculateLabelMap();
		else
			labelMap = CalculateLabelMapGuess();
	 


		_labelMapList->insert(_labelMapList->begin(), labelMap);
		if(i > 0)
		{	
			cvReleaseMat(&_initialGuess);	
			_initialGuess = cvCreateMat((*outputSizeList)[i-1].height, (*outputSizeList)[i-1].width , CV_32SC2);
			GetInterpolationMap(labelMap, _initialGuess);			
			//if(i == index)
			//{
			//	IplImage* image = GetImageFromLabelMap(_initialGuess, (*_imageList)[i-1]);
			//	//IplImage* image = (*imageList)[0];
			//	cvNamedWindow("test");
			//	while(1)
			//	{
			//		cvShowImage("test", image);
			//		int key = cvWaitKey(100);
			//		if(key == 32)
			//		{
			//			index--;
			//			break;
			//		}
			//	}
			//}			 
//.........这里部分代码省略.........

static void*
imdecode_( const Mat& buf, int flags, int hdrtype, Mat* mat=0 )
{
    CV_Assert(!buf.empty() && buf.isContinuous());
    IplImage* image = 0;
    CvMat *matrix = 0;
    Mat temp, *data = &temp;
    String filename;

    ImageDecoder decoder = findDecoder(buf);
    if( !decoder )
        return 0;

    if( !decoder->setSource(buf) )
    {
        filename = tempfile();
        FILE* f = fopen( filename.c_str(), "wb" );
        if( !f )
            return 0;
        size_t bufSize = buf.cols*buf.rows*buf.elemSize();
        fwrite( buf.ptr(), 1, bufSize, f );
        fclose(f);
        decoder->setSource(filename);
    }

    if( !decoder->readHeader() )
    {
        decoder.release();
        if ( !filename.empty() )
        {
            if ( remove(filename.c_str()) != 0 )
            {
                CV_Error( CV_StsError, "unable to remove temporary file" );
            }
        }
        return 0;
    }

    CvSize size;
    size.width = decoder->width();
    size.height = decoder->height();

    int type = decoder->type();
    if( (flags & IMREAD_LOAD_GDAL) != IMREAD_LOAD_GDAL && flags != IMREAD_UNCHANGED )
    {
        if( (flags & CV_LOAD_IMAGE_ANYDEPTH) == 0 )
            type = CV_MAKETYPE(CV_8U, CV_MAT_CN(type));

        if( (flags & CV_LOAD_IMAGE_COLOR) != 0 ||
           ((flags & CV_LOAD_IMAGE_ANYCOLOR) != 0 && CV_MAT_CN(type) > 1) )
            type = CV_MAKETYPE(CV_MAT_DEPTH(type), 3);
        else
            type = CV_MAKETYPE(CV_MAT_DEPTH(type), 1);
    }

    if( hdrtype == LOAD_CVMAT || hdrtype == LOAD_MAT )
    {
        if( hdrtype == LOAD_CVMAT )
        {
            matrix = cvCreateMat( size.height, size.width, type );
            temp = cvarrToMat(matrix);
        }
        else
        {
            mat->create( size.height, size.width, type );
            data = mat;
        }
    }
    else
    {
        image = cvCreateImage( size, cvIplDepth(type), CV_MAT_CN(type) );
        temp = cvarrToMat(image);
    }

    bool code = decoder->readData( *data );
    decoder.release();
    if ( !filename.empty() )
    {
        if ( remove(filename.c_str()) != 0 )
        {
            CV_Error( CV_StsError, "unable to remove temporary file" );
        }
    }

    if( !code )
    {
        cvReleaseImage( &image );
        cvReleaseMat( &matrix );
        if( mat )
            mat->release();
        return 0;
    }

    return hdrtype == LOAD_CVMAT ? (void*)matrix :
        hdrtype == LOAD_IMAGE ? (void*)image : (void*)mat;
}

//.........这里部分代码省略.........
		}
		AAtData.AOps( dx, Adx, AAtData.userdata );
		dup = du->data.db;
		sig1211p = sig1211->data.db;
		double* dxp = dx->data.db;
		for ( i = 0; i < X->rows; ++i, ++dup, ++sig1211p, ++dxp )
			*dup -= (*sig1211p) * (*dxp);

		/* minimum step size that stays in the interior */
		double aqe = cvDotProduct( Adx, Adx );
		double bqe = 2. * cvDotProduct( R, Adx );
		double cqe = cvDotProduct( R, R ) - epsilon2;
		double smax = MIN( 1, -bqe + sqrt( bqe * bqe - 4 * aqe * cqe ) / (2 * aqe) );
		dup = du->data.db;
		dxp = dx->data.db;
		fu1p = fu1->data.db;
		fu2p = fu2->data.db;
		for ( i = 0; i < X->rows; ++i, ++dup, ++dxp, ++fu1p, ++fu2p )
		{
			if ( (*dxp) - (*dup) > 0 )
				smax = MIN( smax, -(*fu1p) / ((*dxp) - (*dup)) );
			if ( (*dxp) + (*dup) < 0 )
				smax = MIN( smax, (*fu2p) / ((*dxp) + (*dup)) );
		}
		smax *= .99;

		/* backtracking line search */
		bool suffdec = 0;
		int backiter = 0;
		double fep = fe;
		double fp = f;
		double lambda2;
		while (!suffdec)
		{
			cvAddWeighted( X, 1, dx, smax, 0, pX );
			cvAddWeighted( U, 1, du, smax, 0, pU );
			cvAddWeighted( R, 1, Adx, smax, 0, pR );
			cvSub( pU, pX, lfu1 );
			cvAdd( pX, pU, lfu2 );
			cvSubRS( lfu1, cvScalar(0), pfu1 );
			cvSubRS( lfu2, cvScalar(0), pfu2 );
			fep = .5 * (cvDotProduct( pR, pR ) - epsilon2);
			cvLog( lfu1, lfu1 );
			cvLog( lfu2, lfu2 );
			CvScalar sumpU = cvSum( pU );
			CvScalar sumpfu1 = cvSum( pfu1 );
			CvScalar sumpfu2 = cvSum( pfu2 );
			fp = sumpU.val[0] - tau_inv * (sumpfu1.val[0] + sumpfu2.val[0] + log(-fep));
			lambda2 = cvDotProduct( ntgx, dx ) + cvDotProduct( ntgu, du );
			double flin = f + alpha * smax * lambda2;
			suffdec = (fp <= flin);
			smax = beta * smax;
			++backiter;
			if ( backiter > 32 )
			{
				result = t;
				goto __clean_up__;
			}
		}

		/* set up for next iteration */
		cvCopy( pX, X );
		cvCopy( pU, U );
		cvCopy( pR, R );
		cvCopy( pfu1, fu1 );
		cvCopy( pfu2, fu2 );
		fe = fep;
		fe_inv = 1. / fe;
		f = fp;
		lambda2 = -lambda2 * .5;
		if ( lambda2 < nt_term_crit.epsilon )
		{
			result = t + 1;
			break;
		}
	}

__clean_up__:

	cvReleaseMat( &pfu2 );
	cvReleaseMat( &pfu1 );
	cvReleaseMat( &pR );
	cvReleaseMat( &pU );
	cvReleaseMat( &pX );
	cvReleaseMat( &tX );
	cvReleaseMat( &dx );
	cvReleaseMat( &Adx );
	cvReleaseMat( &du );
	cvReleaseMat( &w1 );
	cvReleaseMat( &sigx );
	cvReleaseMat( &sig1211 );
	cvReleaseMat( &ntgu );
	cvReleaseMat( &ntgx );
	cvReleaseMat( &lfu2 );
	cvReleaseMat( &lfu1 );
	cvReleaseMat( &fu2 );
	cvReleaseMat( &fu1 );
	cvReleaseMat( &R );
	return result;
}

//.........这里部分代码省略.........
{
    IplImage* image = 0;
    CvMat *matrix = 0;
    Mat temp, *data = &temp;

    /// Search for the relevant decoder to handle the imagery
    ImageDecoder decoder;

#ifdef HAVE_GDAL
    if(flags != IMREAD_UNCHANGED && (flags & IMREAD_LOAD_GDAL) == IMREAD_LOAD_GDAL ){
        decoder = GdalDecoder().newDecoder();
    }else{
#endif
        decoder = findDecoder( filename );
#ifdef HAVE_GDAL
    }
#endif

    /// if no decoder was found, return nothing.
    if( !decoder ){
        return 0;
    }

    int scale_denom = 1;
    if( flags > IMREAD_LOAD_GDAL )
    {
    if( flags & IMREAD_REDUCED_GRAYSCALE_2 )
        scale_denom = 2;
    else if( flags & IMREAD_REDUCED_GRAYSCALE_4 )
        scale_denom = 4;
    else if( flags & IMREAD_REDUCED_GRAYSCALE_8 )
        scale_denom = 8;
    }

    /// set the scale_denom in the driver
    decoder->setScale( scale_denom );

    /// set the filename in the driver
    decoder->setSource( filename );

   // read the header to make sure it succeeds
   if( !decoder->readHeader() )
        return 0;

    // established the required input image size
    CvSize size;
    size.width = decoder->width();
    size.height = decoder->height();

    // grab the decoded type
    int type = decoder->type();
    if( (flags & IMREAD_LOAD_GDAL) != IMREAD_LOAD_GDAL && flags != IMREAD_UNCHANGED )
    {
        if( (flags & CV_LOAD_IMAGE_ANYDEPTH) == 0 )
            type = CV_MAKETYPE(CV_8U, CV_MAT_CN(type));

        if( (flags & CV_LOAD_IMAGE_COLOR) != 0 ||
           ((flags & CV_LOAD_IMAGE_ANYCOLOR) != 0 && CV_MAT_CN(type) > 1) )
            type = CV_MAKETYPE(CV_MAT_DEPTH(type), 3);
        else
            type = CV_MAKETYPE(CV_MAT_DEPTH(type), 1);
    }

    if( hdrtype == LOAD_CVMAT || hdrtype == LOAD_MAT )
    {
        if( hdrtype == LOAD_CVMAT )
        {
            matrix = cvCreateMat( size.height, size.width, type );
            temp = cvarrToMat( matrix );
        }
        else
        {
            mat->create( size.height, size.width, type );
            data = mat;
        }
    }
    else
    {
        image = cvCreateImage( size, cvIplDepth(type), CV_MAT_CN(type) );
        temp = cvarrToMat( image );
    }

    // read the image data
    if( !decoder->readData( *data ))
    {
        cvReleaseImage( &image );
        cvReleaseMat( &matrix );
        if( mat )
            mat->release();
        return 0;
    }

    if( decoder->setScale( scale_denom ) > 1 ) // if decoder is JpegDecoder then decoder->setScale always returns 1
    {
        resize( *mat, *mat, Size( size.width / scale_denom, size.height / scale_denom ) );
    }

    return hdrtype == LOAD_CVMAT ? (void*)matrix :
        hdrtype == LOAD_IMAGE ? (void*)image : (void*)mat;
}

//.........这里部分代码省略.........
        CvPoint2D32f* src;
        CvPoint2D32f* dst;
        src = (CvPoint2D32f*)calloc(4, sizeof(CvPoint2D32f));
        dst = (CvPoint2D32f*)calloc(4, sizeof(CvPoint2D32f));

        //populate the src array 
        if( pxsum1 >= pxsum2 ){
            for ( i=0; i<4; i++){
                src[i].x = corners[i].x;
                src[i].y = corners[i].y;
            }
        }else{
            for ( i=0; i<4; i++){
                int j = i-1;
                if ( j<0 ) j = 3;
                src[i].x = corners[j].x;
                src[i].y = corners[j].y;
            }
        }
        //populate the dst array
        dst[0].x = 0;
        dst[0].y = 0;
        dst[1].x = 0;
        dst[1].y = 100;
        dst[2].x = 100;
        dst[2].y = 100;
        dst[3].x = 100;
        dst[3].y = 0;

        //get the transformation matrix
        CvMat* tmatrix = cvCreateMat(3,3,CV_32FC1);
        tmatrix = cvGetPerspectiveTransform( src, dst, tmatrix);

        //transform the image
        CvSize warpedsize = {100,100};
        IplImage* warped = cvCreateImage(warpedsize, 8, 1);
        CvScalar fillcolor = {{0}};
        cvWarpPerspective(binary, warped, tmatrix,CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS , fillcolor);

    #ifdef VISUAL_DEBUG_X
        IplImage* compared = cvCreateImage(cvGetSize(xtemplate),8,3);
    #endif

    //XOR the template image with our warped image 
    int xor_sum = 0;
    for ( i=xtemplate->width*xtemplate->height -1; i>=0; i--){
        int p1 = xtemplate->imageData[i];
        int p2 = warped->imageData[i];
        if(( p1 != 0 &&  p2 != 0 ) || ( p1 == 0 && p2 == 0 )){
            xor_sum++;
        }
       
        #ifdef VISUAL_DEBUG_X
        if(p1 != 0 && p2 != 0){
            compared->imageData[i*3+0] = 0x00;
            compared->imageData[i*3+1] = 0xff;
            compared->imageData[i*3+2] = 0x00;
        }else if(p1 == 0 && p2 == 0){
            compared->imageData[i*3+0] = 0x00;
            compared->imageData[i*3+1] = 0xff;
            compared->imageData[i*3+2] = 0x00;
        }else if(p1 != 0){
            compared->imageData[i*3+0] = 0xff;
            compared->imageData[i*3+1] = 0x00;
            compared->imageData[i*3+2] = 0x00;
        }else if(p2 != 0){
            compared->imageData[i*3+0] = 0xff;
            compared->imageData[i*3+1] = 0x00;
            compared->imageData[i*3+2] = 0x00;
        }
        #endif
    }

    //compute confidence
    int confidence = xor_sum * 100 / (xtemplate->width*xtemplate->height);

    #ifdef VISUAL_DEBUG_X
        cvNamedWindow("Compared",CV_WINDOW_AUTOSIZE);
        cvShowImage("Compared", compared);

        cvNamedWindow("Warped", CV_WINDOW_AUTOSIZE);
        cvShowImage("Warped", warped);

        cvNamedWindow("Xtemplate", CV_WINDOW_AUTOSIZE);
        cvShowImage("Xtemplate",xtemplate);
    #endif

    //free memory
    cvReleaseImage(&xtemplate);
    cvReleaseImage(&warped);
    free(corners);
    free(src);
    free(dst);
    cvReleaseMat(&tmatrix);
    #ifdef VISUAL_DEBUG_X
        cvReleaseImage(&compared);
    #endif

    return confidence;
}

//.........这里部分代码省略.........
	H->data.db[3] = 0;
	H->data.db[4] = f.val[0];
	H->data.db[5] = ((-1) * f.val[0] * cvmGet(imsize, 1, 0)) / 2;
	H->data.db[6] = 0;
	H->data.db[7] = 0;
	H->data.db[8] = 1;

	for(mat_id = 0; mat_id < K ; mat_id++)
	{
		cvMatMul(H, P[mat_id], P[mat_id]);
	}
	/* u = H * u; */
	cvMatMul(H, u, u);
	/*
	//debug
	printf("....H\n");
	CvMat_printdb(stdout,"%7.3f ",H);
	//debug
	printf("....u\n");
	CvMat_printdb(stdout,"%7.3f ",u);
	*/

	/* Parametrize X such that X = T*[Y;1]; thus x = P*T*[Y;1] = Q*[Y;1] */
	/* Create the SVD matrices X = U*W*V'*/

	X_T = cvCreateMat(X->cols, X->rows, CV_64FC1); /* M * N */
	W = cvCreateMat(X_T->rows, X_T->cols, CV_64FC1); /* M * N */
	U = cvCreateMat(X_T->rows, X_T->rows, CV_64FC1); /* M * M */
	T = cvCreateMat(X_T->cols, X_T->cols, CV_64FC1); /* N * N */
	cvTranspose(X, X_T);
	cvSVD(X_T, W, U, T);


	cvReleaseMat(&W);
	
	/* T = T(:,[2:end 1]); */
	/* Initialize the temporary matrix by extracting the first two columns */
	/* Create temporary matrix for the linear function */
	T_2_cols    = cvCreateMat(T->rows, 2, CV_64FC1);
	T_rest_cols = cvCreateMat(T->rows, (T->cols - 2), CV_64FC1);

	/* Initialize the temporary matrix by extracting the first two columns */
	T_2_cols= sfmGetCols(T,0,0);
	T_rest_cols=sfmGetCols(T,1,T->cols-1);

	T=sfmAlignMatH(T_rest_cols,T_2_cols);

	for(mat_id = 0; mat_id < K ; mat_id++)
	{
	    /* Create temporary matrix for the linear function */
	    Q[mat_id] = cvCreateMat(P[mat_id]->rows, T->cols, CV_64FC1);
		cvMatMul(P[mat_id], T, Q[mat_id]);
	}

	/*
	//debug
	printf("....Q0\n");
	CvMat_printdb(stdout,"%7.3f ",Q[0]);

	//debug
	printf("....Q1\n");
	CvMat_printdb(stdout,"%7.3f ",Q[1]);
*/
	/* Newton estimation */
	/* Create the required Y matrix for the Newton process */
	Y = cvCreateMat(3, 1, CV_64FC1);

//.........这里部分代码省略.........
        CV_ERROR( CV_StsBadArg,
        "input training data should be a floating-point matrix with"
        "the number of rows equal to the number of training samples and "
        "the number of columns equal to the size of 0-th (input) layer" );

    if( !CV_IS_MAT(_outputs) || CV_MAT_TYPE(_outputs->type) != CV_32FC1 &&
        CV_MAT_TYPE(_outputs->type) != CV_64FC1 ||
        _outputs->cols != layer_sizes->data.i[layer_sizes->cols - 1] )
        CV_ERROR( CV_StsBadArg,
        "output training data should be a floating-point matrix with"
        "the number of rows equal to the number of training samples and "
        "the number of columns equal to the size of last (output) layer" );

    if( _inputs->rows != _outputs->rows )
        CV_ERROR( CV_StsUnmatchedSizes, "The numbers of input and output samples do not match" );

    if( _sample_idx )
    {
        CV_CALL( sample_idx = cvPreprocessIndexArray( _sample_idx, _inputs->rows ));
        sidx = sample_idx->data.i;
        count = sample_idx->cols + sample_idx->rows - 1;
    }
    else
        count = _inputs->rows;

    if( _sample_weights )
    {
        if( !CV_IS_MAT(_sample_weights) )
            CV_ERROR( CV_StsBadArg, "sample_weights (if passed) must be a valid matrix" );

        sw_type = CV_MAT_TYPE(_sample_weights->type);
        sw_count = _sample_weights->cols + _sample_weights->rows - 1;

        if( sw_type != CV_32FC1 && sw_type != CV_64FC1 ||
            _sample_weights->cols != 1 && _sample_weights->rows != 1 ||
            sw_count != count && sw_count != _inputs->rows )
            CV_ERROR( CV_StsBadArg,
            "sample_weights must be 1d floating-point vector containing weights "
            "of all or selected training samples" );

        sw_step = CV_IS_MAT_CONT(_sample_weights->type) ? 1 :
            _sample_weights->step/CV_ELEM_SIZE(sw_type);
        
        CV_CALL( sw = (double*)cvAlloc( count*sizeof(sw[0]) ));
    }

    CV_CALL( ivecs.data.ptr = (uchar**)cvAlloc( count*sizeof(ivecs.data.ptr[0]) ));
    CV_CALL( ovecs.data.ptr = (uchar**)cvAlloc( count*sizeof(ovecs.data.ptr[0]) ));
    
    ivecs.type = CV_MAT_TYPE(_inputs->type);
    ovecs.type = CV_MAT_TYPE(_outputs->type);
    ivecs.count = ovecs.count = count;

    for( i = 0; i < count; i++ )
    {
        int idx = sidx ? sidx[i] : i;
        ivecs.data.ptr[i] = _inputs->data.ptr + idx*_inputs->step;
        ovecs.data.ptr[i] = _outputs->data.ptr + idx*_outputs->step;
        if( sw )
        {
            int si = sw_count == count ? i : idx;
            double w = sw_type == CV_32FC1 ?
                (double)_sample_weights->data.fl[si*sw_step] :
                _sample_weights->data.db[si*sw_step];
            sw[i] = w;
            if( w < 0 )
                CV_ERROR( CV_StsOutOfRange, "some of sample weights are negative" );
            sw_sum += w;
        }
    }

    // normalize weights
    if( sw )
    {
        sw_sum = sw_sum > DBL_EPSILON ? 1./sw_sum : 0;
        for( i = 0; i < count; i++ )
            sw[i] *= sw_sum;
    }

    calc_input_scale( &ivecs, _flags );
    CV_CALL( calc_output_scale( &ovecs, _flags ));

    ok = true;

    __END__;

    if( !ok )
    {
        cvFree( &ivecs.data.ptr );
        cvFree( &ovecs.data.ptr );
        cvFree( &sw );
    }

    cvReleaseMat( &sample_idx );
    *_ivecs = ivecs;
    *_ovecs = ovecs;
    *_sw = sw;

    return ok;
}

void MainWindow::stereoVisionTest(QString image_path,int cornersX,int cornersY){
    trace("stereoVisionTest ... ");
    StereoVision* sv = new StereoVision(CAM_WIDTH,CAM_HEIGHT);

    IplImage* images[2];

    //perform calibration based on sets of 2 images (chessboard)
    sv->calibrationStart(cornersX,cornersY);
    // READ IN THE LIST OF CHESSBOARDS:
    QString file_name;
    for(int i=0;;i++){
        for(int lr=0;lr<2;lr++){
            file_name = tr("%1%2%3.jpg").arg(image_path).arg(i).arg(lr ? 'L' : 'R');
            trace(file_name);
            images[lr] = cvLoadImage( file_name.toLatin1(), 0 );
        }
        if(images[0] && images[1]){
            trace(file_name);
            sv->calibrationAddSample(images[0],images[1]);
            for(int lr=0;lr<2;lr++)
                cvReleaseImage(&images[lr]);
        }else{
            break;
        }
    }
    sv->calibrationEnd();
    sv->calibrationSave("stereovisiontest.dat");

    //Once saved calibartion data can be loaded later with:
    //sv->calibrationLoad("stereovisiontest.dat");

   //test our calibartion with first image set:
    CvSize imageSize = sv->getImageSize();
   for(int lr=0;lr<2;lr++){
        QString file_name = tr("%1%2%3.jpg").arg(image_path).arg(0).arg(lr ? 'L' : 'R');
        images[lr] = cvLoadImage( file_name.toLatin1(), 0 );
    }


    //this method will compute  sv->imagesRectified[0],sv->imagesRectified[1],sv->imageDepth,sv->imageDepthNormalized
    sv->stereoProcess(images[0],images[1]);


    //merge 2 rectified images in one image (pair) and display horizontal lines
    //to evaluate rectification.
    CvMat* pair = cvCreateMat( imageSize.height, imageSize.width*2,CV_8UC3 );
    CvMat part;
    cvGetCols( pair, &part, 0, imageSize.width );
    cvCvtColor( sv->imagesRectified[0], &part, CV_GRAY2BGR );
    cvGetCols( pair, &part, imageSize.width,imageSize.width*2 );
    cvCvtColor( sv->imagesRectified[1], &part, CV_GRAY2BGR );
    for(int j = 0; j < imageSize.height; j += 16 )
        cvLine( pair, cvPoint(0,j),cvPoint(imageSize.width*2,j),CV_RGB(0,255,0));


    //display the results
    cvNamedWindow( "rectified", 1 );
    cvShowImage( "rectified", pair );

    cvNamedWindow( "depth", 1 );
    cvShowImage( "depth", sv->imageDepthNormalized);

    //free up memory
    cvReleaseImage(&images[0]);
    cvReleaseImage(&images[1]);
    cvReleaseMat(&pair);

}

// This function is copied from http://mehrez.kristou.org/opencv-change-contrast-and-brightness-of-an-image/
boost::shared_ptr< Image > Image::ContrastBrightness( int contrast, int brightness ) const
{
	if(contrast > 100) contrast = 100;
	if(contrast < -100) contrast = -100;
	if(brightness > 100) brightness = 100;
	if(brightness < -100) brightness = -100;

	uchar lut[256];

	CvMat* lut_mat;
	int hist_size = 256;
	float range_0[]={0,256};
	float* ranges[] = { range_0 };
	int i;

	IplImage * dest = cvCloneImage(this);
	
	IplImage * GRAY;
	if (this->nChannels == 3)
	{
		GRAY = cvCreateImage(cvGetSize(this),this->depth,1);
		cvCvtColor(this,GRAY,CV_RGB2GRAY);
	}
	else
	{
		GRAY = cvCloneImage(this);
	}
    lut_mat = cvCreateMatHeader( 1, 256, CV_8UC1 );
    cvSetData( lut_mat, lut, 0 );
	/*
     * The algorithm is by Werner D. Streidt
     * (http://visca.com/ffactory/archives/5-99/msg00021.html)
     */
	if( contrast > 0 )
    {
        double delta = 127.* contrast/100;
        double a = 255./(255. - delta*2);
        double b = a*(brightness