Example #1
0
//通过引用,修改rotation和scale,使shape1和shape2差距最小
void SimilarityTransform(const Mat_<double>& shape1, const Mat_<double>& shape2, Mat_<double>& rotation, double& scale) {
	rotation = Mat::zeros(2, 2, CV_64FC1);
	scale = 0;

	// center the data
	double center_x_1 = 0;
	double center_y_1 = 0;
	double center_x_2 = 0;
	double center_y_2 = 0;
	for (int i = 0; i < shape1.rows; i++) {
		center_x_1 += shape1(i, 0);
		center_y_1 += shape1(i, 1);
		center_x_2 += shape2(i, 0);
		center_y_2 += shape2(i, 1);
	}
	center_x_1 /= shape1.rows;
	center_y_1 /= shape1.rows;
	center_x_2 /= shape2.rows;
	center_y_2 /= shape2.rows;

	Mat_<double> temp1 = shape1.clone();
	Mat_<double> temp2 = shape2.clone();
	for (int i = 0; i < shape1.rows; i++) {
		temp1(i, 0) -= center_x_1;
		temp1(i, 1) -= center_y_1;
		temp2(i, 0) -= center_x_2;
		temp2(i, 1) -= center_y_2;
	}
	//至此,temp1(与shape1形状相同)和temp2(与shape2形状相同)已经移到了以(0,0)为原点的坐标系

	Mat_<double> covariance1, covariance2;	//covariance = 协方差
	Mat_<double> mean1, mean2;
	// calculate covariance matrix
	calcCovarMatrix(temp1, covariance1, mean1, CV_COVAR_COLS);	//输出covariance1为temp1的协方差,mean1为temp1的均值
	calcCovarMatrix(temp2, covariance2, mean2, CV_COVAR_COLS);

	double s1 = sqrt(norm(covariance1));	//norm用来计算covariance1的L2范数
	double s2 = sqrt(norm(covariance2));
	scale = s1 / s2;
	temp1 = 1.0 / s1 * temp1;
	temp2 = 1.0 / s2 * temp2;
	// 至此,通过缩放,temp1和temp2的差距最小(周叔说,这是最小二乘法)

	double num = 0;
	double den = 0;
	for (int i = 0; i < shape1.rows; i++) {
		num = num + temp1(i, 1) * temp2(i, 0) - temp1(i, 0) * temp2(i, 1);
		den = den + temp1(i, 0) * temp2(i, 0) + temp1(i, 1) * temp2(i, 1);
	}

	double norm = sqrt(num * num + den * den);
	double sin_theta = num / norm;
	double cos_theta = den / norm;
	rotation(0, 0) = cos_theta;
	rotation(0, 1) = -sin_theta;
	rotation(1, 0) = sin_theta;
	rotation(1, 1) = cos_theta;
}
Example #2
0
void SimilarityTransform(const Mat_<double>& shape1, const Mat_<double>& shape2,
                         Mat_<double>& rotation,double& scale){
    rotation = Mat::zeros(2,2,CV_64FC1);
    scale = 0;

    // center the data
    double center_x_1 = 0;
    double center_y_1 = 0;
    double center_x_2 = 0;
    double center_y_2 = 0;
    for(int i = 0;i < shape1.rows;i++){
        center_x_1 += shape1(i,0);
        center_y_1 += shape1(i,1);
        center_x_2 += shape2(i,0);
        center_y_2 += shape2(i,1);
    }
    center_x_1 /= shape1.rows;
    center_y_1 /= shape1.rows;
    center_x_2 /= shape2.rows;
    center_y_2 /= shape2.rows;

    Mat_<double> temp1 = shape1.clone();
    Mat_<double> temp2 = shape2.clone();
    for(int i = 0;i < shape1.rows;i++){
        temp1(i,0) -= center_x_1;
        temp1(i,1) -= center_y_1;
        temp2(i,0) -= center_x_2;
        temp2(i,1) -= center_y_2;
    }


    Mat_<double> covariance1, covariance2;
    Mat_<double> mean1,mean2;
    // calculate covariance matrix
    calcCovarMatrix(temp1,covariance1,mean1,CV_COVAR_COLS);
    calcCovarMatrix(temp2,covariance2,mean2,CV_COVAR_COLS);

    double s1 = sqrt(norm(covariance1));
    double s2 = sqrt(norm(covariance2));
    scale = s1 / s2;
    temp1 = 1.0 / s1 * temp1;
    temp2 = 1.0 / s2 * temp2;

    double num = 0;
    double den = 0;
    for(int i = 0;i < shape1.rows;i++){
        num = num + temp1(i,1) * temp2(i,0) - temp1(i,0) * temp2(i,1);
        den = den + temp1(i,0) * temp2(i,0) + temp1(i,1) * temp2(i,1);
    }

    double norm = sqrt(num*num + den*den);
    double sin_theta = num / norm;
    double cos_theta = den / norm;
    rotation(0,0) = cos_theta;
    rotation(0,1) = -sin_theta;
    rotation(1,0) = sin_theta;
    rotation(1,1) = cos_theta;
}
void similarityTransform(const Mat_<double> &fromShape,
						 const Mat_<double> &toShape,
						 Mat_<double> &rotation, double &scale)
{
	rotation = Mat::zeros(2, 2, CV_64FC1);
	scale = 0;

	assert(fromShape.rows == toShape.rows);
	int rows = fromShape.rows;

	double fcx = 0, fcy = 0, tcx = 0, tcy = 0;
	for (int i = 0; i < rows; i++) {
		fcx += fromShape(i, 0);
		fcy += fromShape(i, 1);
		tcx += toShape(i, 0);
		tcy += toShape(i, 1);
	}
	fcx /= rows;
	fcy /= rows;
	tcx /= rows;
	tcy /= rows;

	Mat_<double> ftmp = fromShape.clone();
	Mat_<double> ttmp = toShape.clone();
	for (int i = 0; i < rows; i++) {
		ftmp(i, 0) -= fcx;
		ftmp(i, 1) -= fcy;
		ttmp(i, 0) -= tcx;
		ttmp(i, 1) -= tcy;
	}

	Mat_<double> fcov, tcov;
	Mat_<double> fmean, tmean;
	calcCovarMatrix(ftmp, fcov, fmean, CV_COVAR_COLS);
	calcCovarMatrix(ttmp, tcov, tmean, CV_COVAR_COLS);

	double fsize = sqrtf(norm(fcov));
	double tsize = sqrtf(norm(tcov));
	scale = tsize / fsize;
	ftmp /= fsize;
	ttmp /= tsize;

	double num = 0, den = 0;
	// an efficient way to calculate rotation, using cross and dot production
	for (int i = 0; i < rows; i++) {
		num += ftmp(i, 1)*ttmp(i, 0) - ftmp(i, 0)*ttmp(i, 1);
		den += ftmp(i, 0)*ttmp(i, 0) + ftmp(i, 1)*ttmp(i, 1);
	}
	double norm = sqrtf(num*num + den*den);
	// theta is clock-wise rotation angle(fromShape -> toShape)
	double sinTheta = num / norm;
	double cosTheta = den / norm;

	rotation(0, 0) = cosTheta;
	rotation(0, 1) = sinTheta;
	rotation(1, 0) = -sinTheta;
	rotation(1, 1) = cosTheta;
}
void SimilarityTransform(const Mat_<float>& shape1, const Mat_<float>& shape2, 
                         Mat_<float>& rotation,float& scale){
    rotation = Mat::zeros(2,2,CV_32FC1);
    scale = 0;
    
    // center the data
    float center_x_1 = 0;
    float center_y_1 = 0;
    float center_x_2 = 0;
    float center_y_2 = 0;
    for(int i = 0;i < shape1.rows;i++){
        center_x_1 += shape1(i,0);
        center_y_1 += shape1(i,1);
        center_x_2 += shape2(i,0);
        center_y_2 += shape2(i,1); 
    }
    center_x_1 /= shape1.rows;
    center_y_1 /= shape1.rows;
    center_x_2 /= shape2.rows;
    center_y_2 /= shape2.rows;
    
    Mat_<float> temp1 = shape1.clone();
    Mat_<float> temp2 = shape2.clone();
    for(int i = 0;i < shape1.rows;i++){
        temp1(i,0) -= center_x_1;
        temp1(i,1) -= center_y_1;
        temp2(i,0) -= center_x_2;
        temp2(i,1) -= center_y_2;
    }
    Mat_<float> covariance1, covariance2;
    Mat_<float> mean1,mean2;
    // calculate covariance matrix
    calcCovarMatrix(temp1,covariance1,mean1,CV_COVAR_SCALE|CV_COVAR_ROWS|CV_COVAR_NORMAL,CV_32F);
	calcCovarMatrix(temp2,covariance2,mean2,CV_COVAR_SCALE|CV_COVAR_ROWS|CV_COVAR_NORMAL,CV_32F);
	//cout<<covariance1<<endl;
	//cout<<covariance2<<endl;
    float s1 = sqrt(norm(covariance1));
    float s2 = sqrt(norm(covariance2));
    scale = s1 / s2; 
    temp1 = 1.0 / s1 * temp1;
    temp2 = 1.0 / s2 * temp2;
    float num = 0;
    float den = 0;
    for(int i = 0;i < shape1.rows;i++){
        num = num + temp1(i,1) * temp2(i,0) - temp1(i,0) * temp2(i,1);
        den = den + temp1(i,0) * temp2(i,0) + temp1(i,1) * temp2(i,1);      
    }
    float norm = sqrt(num*num + den*den);    
    float sin_theta = num / norm;
    float cos_theta = den / norm;
    rotation(0,0) = cos_theta;
    rotation(0,1) = -sin_theta;
    rotation(1,0) = sin_theta;
    rotation(1,1) = cos_theta;
}
Example #5
0
void Evm::amplify_spatial_lpyr_temporal_ideal(const cv::Mat &src, std::vector<cv::Mat_<cv::Vec3f> > &lapPyr, cv::Mat &dst, float alpha, float lambda_c, float fl, float fh, float samplingRate, float chromAttenuation)
{
    Mat_<Vec3f> s = src.clone();
    rgb2ntsc(src, s);
    
    buildLaplacianPyramid(s, lapPyr);
    
    if (is_not_first_frame) {
        //temporal ideal
        for (int i=0; i<lapPyr.size(); i++) {
            //TODO:implement temporal ideal filter
            //
        }
        //amplify
//        amplifyByAlpha(src, alpha, lambda_c);
    } else {
        lowpass1 = lapPyr;
        lowpass2 = lapPyr;
        filtered = lapPyr;
        is_not_first_frame = true;
    }
    
    dst = s + reconstructImgFromLapPyramid(filtered).mul(Vec3f(1,chromAttenuation,chromAttenuation));
    
    s = dst.clone();
    ntsc2rgb(s, s);
    dst = s.clone();
}
Example #6
0
void Evm::amplify_spatial_lpyr_temporal_iir(const Mat& src, vector<Mat_<Vec3f> >& lapPyr, Mat& dst,
                                            float alpha, float lambda_c, float r1, float r2, float chromAttenuation)
{
    Mat_<Vec3f> s = src.clone();
    rgb2ntsc(src, s);
    
    buildLaplacianPyramid(s, lapPyr);
    
    if (is_not_first_frame) {
        //temporal iir
        for (int i=0; i<lapPyr.size(); i++) {
            Mat temp1 = (1-r1)*lowpass1[i] + r1*lapPyr[i];
            Mat temp2 = (1-r2)*lowpass2[i] + r2*lapPyr[i];
            lowpass1[i] = temp1;
            lowpass2[i] = temp2;
            filtered[i] = temp1 - temp2;
        }
        //amplify
        amplifyByAlpha(src, alpha, lambda_c);
    } else { // first frame
        lowpass1 = lapPyr;
        lowpass2 = lapPyr;
        filtered = lapPyr;
        is_not_first_frame = true;
    }
    
    dst = s + reconstructImgFromLapPyramid(filtered).mul(Vec3f(1,chromAttenuation,chromAttenuation));

    s = dst.clone();
    ntsc2rgb(s, s);
    dst = s.clone();
}
Example #7
0
void fft2(Mat_<float> src)
{
	int x = getOptimalDFTSize(2* src.rows );
	int y = getOptimalDFTSize(2* src.cols );
	copyMakeBorder(src, src, 0, (x - src.rows), 0, (y - src.cols), BORDER_CONSTANT, Scalar::all(0));
	     // Get padded image size
	     const int wx = src.cols, wy = src.rows;
	     const int cx = wx/2, cy = wy/2;

	//--------------------------------//
	// DFT - performing		     //
	cv::Mat_<float> imgs[] = {src.clone(), Mat::zeros(src.size(), CV_32F)};
	cv::Mat_<cv::Vec2f> img_dft;
	merge(imgs,2,img_dft);
	dft(img_dft, img_dft);
	split(img_dft,imgs);
	cv::Mat_<float> magnitude, phase;
	cartToPolar(imgs[0],imgs[1],magnitude,phase);
	dftshift(magnitude);
	magnitude = magnitude + 1.0f;
	log(magnitude,magnitude);
	normalize(magnitude,magnitude,0,1,CV_MINMAX);
	namedWindow("img_dft",WINDOW_NORMAL);
	imshow("img_dft",magnitude);
	waitKey(0);
	cout << "out" << endl;
}
// Pick only the more stable/rigid points under changes of expression
void extract_rigid_points(Mat_<double>& source_points, Mat_<double>& destination_points)
{
	if(source_points.rows == 68)
	{
		Mat_<double> tmp_source = source_points.clone();
		source_points = Mat_<double>();

		// Push back the rigid points (some face outline, eyes, and nose)
		source_points.push_back(tmp_source.row(0));
		source_points.push_back(tmp_source.row(2));
		source_points.push_back(tmp_source.row(14));
		source_points.push_back(tmp_source.row(16));
		source_points.push_back(tmp_source.row(36));
		source_points.push_back(tmp_source.row(39));
		source_points.push_back(tmp_source.row(43));
		source_points.push_back(tmp_source.row(38));
		source_points.push_back(tmp_source.row(42));
		source_points.push_back(tmp_source.row(45));
		source_points.push_back(tmp_source.row(31));
		source_points.push_back(tmp_source.row(33));
		source_points.push_back(tmp_source.row(35));

		Mat_<double> tmp_dest = destination_points.clone();
		destination_points = Mat_<double>();

		// Push back the rigid points
		destination_points.push_back(tmp_dest.row(0));
		destination_points.push_back(tmp_dest.row(2));
		destination_points.push_back(tmp_dest.row(14));
		destination_points.push_back(tmp_dest.row(16));
		destination_points.push_back(tmp_dest.row(36));
		destination_points.push_back(tmp_dest.row(39));
		destination_points.push_back(tmp_dest.row(43));
		destination_points.push_back(tmp_dest.row(38));
		destination_points.push_back(tmp_dest.row(42));
		destination_points.push_back(tmp_dest.row(45));
		destination_points.push_back(tmp_dest.row(31));
		destination_points.push_back(tmp_dest.row(33));
		destination_points.push_back(tmp_dest.row(35));
	}
}
Example #9
0
void calculate_tangent(const Mat& input_image, const Mat_<double>& tangent,
        const Mat_<int>& boundary, const Mat_<int>& region_id,int current_region_id,
        Mat_<double>& tangent){

    int width = input_image.cols;
    int height = input_image.rows;

    Mat_<int> visited(height,width);
    Mat_<int> current_boundary(width*height,2);

    current_boundary = boundary.clone();
    int current_boundary_size = current_boundary.rows();


    while(true){
        // find new boundary after thinning
        for(int i = 0;i < current_boundary_size;i++){
            int x = current_boundary(i,0);
            int y = current_boundary(i,1);
            Mat_<int> new_boundary(width*height,2);
            int new_boundary_size = 0;	

            for(int j = -1;j < 2;j++){
                for(int k = -1;k < 2;k++){
                    int new_x = x + j;
                    int new_y = y + k;
                    if(new_x < 0 || new_y < 0 || new_x >= width || new_y >= height){
                        continue;
                    }

                    if(region_id(new_y,new_x) != current_region_id){
                        continue;
                    }

                    if(visited(new_y,new_x) == 1){
                        continue;
                    }

                    new_boundary(new_boundary_size,0) = new_x;
                    new_boundary(new_boundary_size,1) = new_y;
                    new_boundary_size = new_boundary_size + 1;
                    visited(new_y,new_x) = 1;
                    tangent(new_y,new_x) = tangent(y,x);
                }
            }
        }

        // if no boundary points found, exit
        if(new_boundary_size == 0){
            break;
        }
    }
}
Mat_<float> MotionDetection::SubstractAllPixels(float value, Mat_<float> frame)
{
  Mat_<float> temp = frame.clone();
  for(int i = 0; i < frame.cols; i++)
  {
    for(int j = 0; j < frame.rows; j++)
    {
      temp.at<float>(Point(i,j)) = value - frame.at<float>(Point(i,j));
    }
  }
  return temp;
}
Mat_<float> MotionDetection::Mat_MultiplyFloatInRect(Mat_<float> frame, float value, Rect rect) 
{
  Mat_<float> mat = frame.clone();
  for(int i = rect.x; i < rect.x + rect.width; i++)
  {
    for (int j = rect.y; j < rect.y + rect.height; j++)
    {
      mat.at<float>(Point(i,j)) = frame.at<float>(Point(i,j)) * value;
    }
  }
  return mat;
}
Example #12
0
//=============================================================================
// cropping from the source image to the destination image using the shape in s, used to determine if shape fitting converged successfully
void PAW::Warp(const Mat& image_to_warp, Mat& destination_image, const Mat_<double>& landmarks_to_warp)
{
  
	// set the current shape
	source_landmarks = landmarks_to_warp.clone();

	// prepare the mapping coefficients using the current shape
	this->CalcCoeff();

	// Do the actual mapping computation (where to warp from)
	this->WarpRegion(map_x, map_y);
  	
	// Do the actual warp (with bi-linear interpolation)
	remap(image_to_warp, destination_image, map_x, map_y, CV_INTER_LINEAR);
  
}
Example #13
0
void applyFilter(Mat_<float> src, Mat_<float> output)
{
	int wxOrig = src.cols;
  int wyOrig = src.rows;

	int m = cv::getOptimalDFTSize(2*wyOrig);
	int n = cv::getOptimalDFTSize(2*wxOrig);

  copyMakeBorder(src, src, 0, m - wyOrig, 0, n - wxOrig, cv::BORDER_CONSTANT, cv::Scalar::all(0));

	const int wx = src.cols, wy = src.rows;
  const int cx = wx/2, cy = wy/2;

	std::cout << wxOrig << " " << wyOrig << std::endl;
	std::cout << wx << " " << wy << std::endl;
	std::cout << cx << " " << cy << std::endl;

	cv::Mat_<float> imgs[] = {src.clone(), cv::Mat_<float>::zeros(wy, wx)};
  cv::Mat_<cv::Vec2f> img_dft;
  cv::merge(imgs, 2, img_dft);
  cv::dft(img_dft, img_dft);

	dftshift(img_dft);
	cout << "helo " << endl;

  cv::Mat hpf = BHPF(3000, 4, wy, wx, cx, cy);
  cv::mulSpectrums(hpf, img_dft, img_dft, cv::DFT_ROWS);

	dftshift(img_dft);

  cv::idft(img_dft, img_dft); //the result is a 2 channel image
	split(img_dft, imgs);
	normalize(imgs[0], output, 0, 1, CV_MINMAX);

	cv::Mat_<float> croppedOutput(output,cv::Rect(0,0,wxOrig,wyOrig));

	output = croppedOutput;

	namedWindow("High-pass filtered input",WINDOW_NORMAL);
	namedWindow("Input",WINDOW_NORMAL);
	cv::imshow("Input", src);
	cv::imshow("High-pass filtered input", croppedOutput);
	imwrite("/home/student/ROVI/VisMand1/build-vis_man_1-Desktop-Debug/output/out.jpg",croppedOutput);
	cout << "lol" << endl;
	cv::waitKey(0);

}
Example #14
0
/**
 * @author     	JIA Pei
 * @version    	2010-05-20
 * @brief      	Basic AAM Fitting, for dynamic image sequence
 * @param      	iImg			Input - image to be fitted
 * @param      	ioShape         Input and Output - the fitted shape
 * @param      	oImg            Output - the fitted image
 * @param		epoch			Input - the iteration epoch
*/
float VO_FittingAAMBasic::VO_BasicAAMFitting(const Mat& iImg,
											VO_Shape& ioShape,
											Mat& oImg,
											unsigned int epoch)
{
	this->m_VOFittingShape.clone(ioShape);
double t = (double)cvGetTickCount();

    this->SetProcessingImage(iImg, this->m_VOAAMBasic);
    this->m_iIteration = 0;

    // Get m_MatModelAlignedShapeParam and m_fScale, m_vRotateAngles, m_MatCenterOfGravity
    this->m_VOAAMBasic->VO_CalcAllParams4AnyShapeWithConstrain(	this->m_VOFittingShape,
																this->m_MatModelAlignedShapeParam,
																this->m_fScale,
																this->m_vRotateAngles,
																this->m_MatCenterOfGravity);
	this->m_VOFittingShape.ConstrainShapeInImage(this->m_ImageProcessing);

	// Get m_MatModelNormalizedTextureParam
    VO_TextureModel::VO_LoadOneTextureFromShape(this->m_VOFittingShape,
												this->m_ImageProcessing,
												this->m_vTriangle2D,
												this->m_vPointWarpInfo,
												this->m_VOFittingTexture );
	// estimate the texture model parameters
    this->m_VOAAMBasic->VO_CalcAllParams4AnyTexture(this->m_VOFittingTexture, this->m_MatModelNormalizedTextureParam);

    // Calculate m_MatCurrentC
    this->m_VOAAMBasic->VO_SParamTParamProjectToCParam(	this->m_MatModelAlignedShapeParam,
														this->m_MatModelNormalizedTextureParam,
														this->m_MatCurrentC );
    // Set m_MatCurrentT, m_MatDeltaT, m_MatEstimatedT, m_MatDeltaC, m_MatEstimatedC, etc.
	this->m_MatCurrentT 	= Mat_<float>::zeros(this->m_MatCurrentT.size());
	this->m_MatDeltaT 		= Mat_<float>::zeros(this->m_MatDeltaT.size());
	this->m_MatEstimatedT 	= Mat_<float>::zeros(this->m_MatEstimatedT.size());
	this->m_MatDeltaC 		= Mat_<float>::zeros(this->m_MatDeltaC.size());
	this->m_MatEstimatedC 	= Mat_<float>::zeros(this->m_MatEstimatedC.size());
	
	//////////////////////////////////////////////////////////////////////////////////////////////////////
	// explained by JIA Pei. 2010-05-20
	// For the first round, this->m_VOFittingShape should not change after calling "VO_CParamTParam2FittingShape"
	// But this is not the case. why?
	// Before calling VO_CParamTParam2FittingShape, this->m_VOFittingShape is calculated by 
	// a) assigning m_VOTemplateAlignedShape
	// b) align to the real-size face using detected eyes and mouth
	// c) constrain the shape within the image
	// d) constrain the shape parameters and calculate those rigid transform parameters
	// cout << this->m_VOFittingShape << endl;
	//////////////////////////////////////////////////////////////////////////////////////////////////////
	// Estimate m_VOFittingShape and m_VOFittingTexture
	this->VO_CParamTParam2FittingShape(	this->m_MatCurrentC,
										this->m_MatCurrentT,
										this->m_VOModelNormalizedTexture,
										this->m_VOFittingShape,
										this->m_fScale,
										this->m_vRotateAngles,
										this->m_MatCenterOfGravity );
	this->m_VOFittingShape.ConstrainShapeInImage(this->m_ImageProcessing);		// Remember to call ConstrainShapeInImage() whenever you update m_VOFittingShape
	//////////////////////////////////////////////////////////////////////////////////////////////////////
	// When calling VO_CParamTParam2FittingShape, this->m_VOFittingShape is calculated by
	// a) c parameters to reconstruct shape parameters
	// b) shape parameters to reconstruct shape
	// c) align to the real-size face by global shape normalization
	// cout << this->m_VOFittingShape << endl;
	//////////////////////////////////////////////////////////////////////////////////////////////////////
	
	this->m_E_previous = this->m_E = this->VO_CalcErrorImage(this->m_ImageProcessing,
															this->m_VOFittingShape,
															this->m_VOModelNormalizedTexture,
															this->m_VOTextureError);

    do
    {
		float estScale = this->m_fScale;
		vector<float> estRotateAngles = this->m_vRotateAngles;
		Mat_<float> estCOG = this->m_MatCenterOfGravity.clone();
		bool cBetter 	= false;
		bool poseBetter = false;

        /**First shape parameters, c parameters. refer to equation (9.3)
		* Cootes "Statistical Model of Appearance for Computer Vision" */
        cv::gemm(this->m_VOTextureError.GetTheTextureInARow(), this->m_VOAAMBasic->m_MatRc, -1, Mat(), 0.0, this->m_MatDeltaC, GEMM_2_T);

        // damp -- C
        for(unsigned int i = 0; i < k_values.size(); i++)
        {
            // make damped c prediction
            cv::scaleAdd(this->m_MatDeltaC, k_values[i], this->m_MatCurrentC, this->m_MatEstimatedC);

            // make sure m_MatEstimatedC are constrained
			this->m_VOAAMBasic->VO_AppearanceParameterConstraint(this->m_MatEstimatedC);
			this->VO_CParamTParam2FittingShape(	this->m_MatEstimatedC,
												this->m_MatCurrentT,
												this->m_VOModelNormalizedTexture,
												this->m_VOEstimatedShape,
												estScale,
												estRotateAngles,
												estCOG);
			if ( !VO_ShapeModel::VO_IsShapeInsideImage(this->m_VOEstimatedShape, this->m_ImageProcessing) )
				continue;
			else
				this->m_E = this->VO_CalcErrorImage(this->m_ImageProcessing,
													this->m_VOEstimatedShape,
													this->m_VOModelNormalizedTexture,
													this->m_VOEstimatedTextureError);

            if (this->m_E < this->m_E_previous)
            {
                this->m_MatEstimatedC.copyTo(this->m_MatCurrentC);
				this->m_VOFittingShape.clone(this->m_VOEstimatedShape);
                this->m_VOTextureError.clone(this->m_VOEstimatedTextureError);
                this->m_E_previous = this->m_E;
                cBetter = true;
				this->m_fScale = estScale;
				this->m_vRotateAngles = estRotateAngles;
				this->m_MatCenterOfGravity = estCOG.clone();
                break;
            }
        }

		/** Second pose, t parameters. refer to equation (9.3)
		* Cootes "Statistical Model of Appearance for Computer Vision" */
        cv::gemm(this->m_VOTextureError.GetTheTextureInARow(), this->m_VOAAMBasic->m_MatRt, -1, Mat(), 0, this->m_MatDeltaT, GEMM_2_T);

        // damp -- T
        for(unsigned int i = 0; i < k_values.size(); i++)
        {
            // make damped c/pose prediction
            cv::scaleAdd(this->m_MatDeltaT, k_values[i], this->m_MatCurrentT, this->m_MatEstimatedT);
			this->VO_CParamTParam2FittingShape(	this->m_MatCurrentC,
												this->m_MatEstimatedT,
												this->m_VOModelNormalizedTexture,
												this->m_VOEstimatedShape,
												estScale,
												estRotateAngles,
												estCOG);
			if ( !VO_ShapeModel::VO_IsShapeInsideImage(this->m_VOEstimatedShape, this->m_ImageProcessing) )
				continue;
			else
				this->m_E = this->VO_CalcErrorImage(this->m_ImageProcessing,
													this->m_VOEstimatedShape,
													this->m_VOModelNormalizedTexture,
													this->m_VOEstimatedTextureError);

            if (this->m_E < this->m_E_previous)
            {
				// Since m_fScale, m_vRotateAngles and m_MatCenterOfGravity have been updated,
				// m_MatCurrentT should be assigned to 0 now!
				this->m_MatCurrentT = Mat_<float>::zeros(this->m_MatCurrentT.size());
				//                this->m_MatEstimatedT.copyTo(this->m_MatCurrentT);
				this->m_VOFittingShape.clone(this->m_VOEstimatedShape);
				this->m_VOTextureError.clone(this->m_VOEstimatedTextureError);
                this->m_E_previous = this->m_E;
                poseBetter = true;
				this->m_fScale = estScale;
				this->m_vRotateAngles = estRotateAngles;
				this->m_MatCenterOfGravity = estCOG.clone();
                break;
            }
        }

        if( cBetter || poseBetter)
        {
			ioShape.clone(this->m_VOFittingShape);
        }
        else
        break;

        ++this->m_iIteration;

    }while( ( fabs(this->m_E) > FLT_EPSILON ) && (this->m_iIteration < epoch)/* && (cv::norm(this->m_MatDeltaC) > FLT_EPSILON) */ );

	t = ((double)cvGetTickCount() -  t )/  (cvGetTickFrequency()*1000.);
	cout << "Basic fitting time cost: " << t << " millisec" << endl;
this->m_fFittingTime = t;

	VO_Fitting2DSM::VO_DrawMesh(ioShape, this->m_VOAAMBasic, oImg);

	return t;
}
Example #15
0
static int initialize_simplex(Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,vector<unsigned int>& indexToRow){
    N.resize(c.cols);
    N[0]=0;
    for (std::vector<int>::iterator it = N.begin()+1 ; it != N.end(); ++it){
        *it=it[-1]+1;
    }
    B.resize(b.rows);
    B[0]=(int)N.size();
    for (std::vector<int>::iterator it = B.begin()+1 ; it != B.end(); ++it){
        *it=it[-1]+1;
    }
    indexToRow.resize(c.cols+b.rows);
    indexToRow[0]=0;
    for (std::vector<unsigned int>::iterator it = indexToRow.begin()+1 ; it != indexToRow.end(); ++it){
        *it=it[-1]+1;
    }
    v=0;

    int k=0;
    {
        double min=DBL_MAX;
        for(int i=0;i<b.rows;i++){
            if(b(i,b.cols-1)<min){
                min=b(i,b.cols-1);
                k=i;
            }
        }
    }

    if(b(k,b.cols-1)>=0){
        N.erase(N.begin());
        for (std::vector<unsigned int>::iterator it = indexToRow.begin()+1 ; it != indexToRow.end(); ++it){
            --(*it);
        }
        return 0;
    }

    Mat_<double> old_c=c.clone();
    c=0;
    c(0,0)=-1;
    for(int i=0;i<b.rows;i++){
        b(i,0)=-1;
    }

    print_simplex_state(c,b,v,N,B);

    dprintf(("\tWE MAKE PIVOT\n"));
    pivot(c,b,v,N,B,k,0,indexToRow);

    print_simplex_state(c,b,v,N,B);

    inner_simplex(c,b,v,N,B,indexToRow);

    dprintf(("\tAFTER INNER_SIMPLEX\n"));
    print_simplex_state(c,b,v,N,B);

    unsigned int nsize = (unsigned int)N.size();
    if(indexToRow[0]>=nsize){
        int iterator_offset=indexToRow[0]-nsize;
        if(b(iterator_offset,b.cols-1)>0){
            return SOLVELP_UNFEASIBLE;
        }
        pivot(c,b,v,N,B,iterator_offset,0,indexToRow);
    }

    vector<int>::iterator iterator;
    {
        int iterator_offset=indexToRow[0];
        iterator=N.begin()+iterator_offset;
        std::iter_swap(iterator,N.begin());
        SWAP(int,indexToRow[*iterator],indexToRow[0]);
        swap_columns(c,iterator_offset,0);
        swap_columns(b,iterator_offset,0);
    }

    dprintf(("after swaps\n"));
    print_simplex_state(c,b,v,N,B);

    //start from 1, because we ignore x_0
    c=0;
    v=0;
    for(int I=1;I<old_c.cols;I++){
        if(indexToRow[I]<nsize){
            dprintf(("I=%d from nonbasic\n",I));
            int iterator_offset=indexToRow[I];
            c(0,iterator_offset)+=old_c(0,I);
            print_matrix(c);
        }else{
            dprintf(("I=%d from basic\n",I));
            int iterator_offset=indexToRow[I]-nsize;
            c-=old_c(0,I)*b.row(iterator_offset).colRange(0,b.cols-1);
            v+=old_c(0,I)*b(iterator_offset,b.cols-1);
            print_matrix(c);
        }
    }

    dprintf(("after restore\n"));
    print_simplex_state(c,b,v,N,B);

    N.erase(N.begin());
    for (std::vector<unsigned int>::iterator it = indexToRow.begin()+1 ; it != indexToRow.end(); ++it){
        --(*it);
    }
    return 0;
}
Example #16
0
Mat_<double> estimateRotTransl(
    Mat_<double> const worldPts,
    Mat_<double> const imagePts)
{
    assert(imagePts.cols == 2);
    assert(worldPts.cols == 3);
    assert(imagePts.rows == worldPts.rows);
    // TODO verify all worldPts have z=0

    // See "pose estimation" section in the paper.

    // Set up linear system of equations.
    int const n = imagePts.rows;
    Mat_<double> F(2 * n, 9);
    for(int i = 0; i < n; i++)
    {
        F(2 * i, 0) = worldPts(i, 0);
        F(2 * i, 1) = 0;
        F(2 * i, 2) = -worldPts(i, 0) * imagePts(i, 0);
        F(2 * i, 3) = worldPts(i, 1);
        F(2 * i, 4) = 0;
        F(2 * i, 5) = -worldPts(i, 1) * imagePts(i, 0);
        F(2 * i, 6) = 1;
        F(2 * i, 7) = 0;
        F(2 * i, 8) = -imagePts(i, 0);

        F(2 * i + 1, 0) = 0;
        F(2 * i + 1, 1) = worldPts(i, 0);
        F(2 * i + 1, 2) = -worldPts(i, 0) * imagePts(i, 1);
        F(2 * i + 1, 3) = 0;
        F(2 * i + 1, 4) = worldPts(i, 1);
        F(2 * i + 1, 5) = -worldPts(i, 1) * imagePts(i, 1);
        F(2 * i + 1, 6) = 0;
        F(2 * i + 1, 7) = 1;
        F(2 * i + 1, 8) = -imagePts(i, 1);
    }

    // Find least-squares estimate of rotation + translation.
    SVD svd(F);

    Mat_<double> rrp = svd.vt.row(8);
    rrp = rrp.clone().reshape(0, 3).t();
    if(rrp(2, 2) < 0) {
        rrp *= -1;  // make sure depth is positive
    }
    // cout << "rrp: " << rrp << endl;

    Mat_<double> transl = \
        2 * rrp.col(2) / (norm(rrp.col(0)) + norm(rrp.col(1)));
    // cout << "transl: " << transl << endl;

    Mat_<double> rot = Mat_<double>::zeros(3, 3);
    rrp.col(0).copyTo(rot.col(0));
    rrp.col(1).copyTo(rot.col(1));
    SVD svd2(rot);
    rot = svd2.u * svd2.vt;
    if(determinant(rot) < 0) {
        rot.col(2) *= -1; // make sure it's a valid rotation matrix
    }
    if(abs(determinant(rot) - 1) > 1e-10) {
        cerr << "Warning: rotation matrix has determinant " \
             << determinant(rot) << " where expected 1." << endl;
    }
    // cout << "rot: " << rot << endl;

    Mat_<double> rotTransl(3, 4);
    rot.col(0).copyTo(rotTransl.col(0));
    rot.col(1).copyTo(rotTransl.col(1));
    rot.col(2).copyTo(rotTransl.col(2));
    transl.copyTo(rotTransl.col(3));
    return rotTransl;
}
int main (int argc, char **argv)
{

	//Convert arguments to more convenient vector form
	vector<string> arguments = get_arguments(argc, argv);

	// Some initial parameters that can be overriden from command line
	vector<string> files, dFiles, outimages, outfeaturess;

	// these can be overriden using -cx, -cy, -fx, -fy, -dimx, -dimy flags
	float fx = 500, fy = 500, cx = 0, cy = 0;
	int dimx = 0, dimy = 0;
	
	// initial translation, rotation and scale (can be specified by the user)
	vector<Vec6d> initial_poses;

	bool useCLMTracker = true;

	// Get camera parameters
	CLMWrapper::get_camera_params(fx, fy, cx, cy, dimx, dimy, arguments);
	CLMWrapper::get_image_input_output_params(files, dFiles, outfeaturess, outimages, initial_poses, arguments);	
	CLMWrapper::CLMParameters clmParams(arguments);	

	// The modules that are being used for tracking
	CLMTracker::TrackerCLM clmModel;

	cout << "Loading the model" << endl;
	clmModel.Read(clmParams.defaultModelLoc, clmParams.override_pdm_loc);
	cout << "Model loaded" << endl;

	PoseDetectorHaar::PoseDetectorHaarParameters haarParams;

	haarParams.ClassifierLocation = "classifiers/haarcascade_frontalface_alt2.xml";

	CascadeClassifier classifier(haarParams.ClassifierLocation);

	bool visualise = true;

	//clmParams.multi_view = true;

	// Do some image loading
	for(size_t i = 0; i < files.size(); i++)
	{
		string file = files.at(i);

		// Loading image
		Mat img = imread(file, -1);
		
		// Loading depth file if exists
		Mat dTemp;		
		Mat_<float> dImg;

		if(dFiles.size() > 0)
		{
			string dFile = dFiles.at(i);
			dTemp = imread(dFile, -1);
			dTemp.convertTo(dImg, CV_32F);
		}

		if(dimx != 0)
		{
			cv::resize(img.clone(), img, cv::Size(dimx, dimy));
			if(!dImg.empty())
			{
				cv::resize(dImg.clone(), dImg, cv::Size(dimx, dimy));
			}
		}

		bool trackingInitialised = false;
	
		// Making sure the image is in uchar grayscale
		Mat_<uchar> gray;		
		convert_to_grayscale(img, gray);
		
			
		// Face detection initialisation
		Vec6d poseEstimateHaar;
		Matx66d poseEstimateHaarUncertainty;

		vector<Vec6d> poseEstimatesInitialiser;
		vector<Matx66d> covariancesInitialiser;	
		vector<Rect> faceRegions;
	
		bool initSuccess = false;

		// if no pose defined we just use OpenCV
		if(initial_poses.empty())
		{
			initSuccess = PoseDetectorHaar::InitialisePosesHaar(gray, dImg, poseEstimatesInitialiser, covariancesInitialiser, faceRegions, classifier, fx, fy, cx, cy, haarParams);

			if(initSuccess)
			{
				if(poseEstimatesInitialiser.size() > 1)
				{
					cout << "Multiple faces detected" << endl;
				}
			}
		
			if(initSuccess)
			{
				// perform landmark detection for every face detected
				for(int r=0; r < faceRegions.size(); ++r)
				{
					if(!clmParams.multi_view)
					{
						Vec6d pose(0.0);
						bool success = CLMWrapper::InitialiseCLM(gray, dImg, clmModel, pose, faceRegions[r], fx, fy, cx, cy, clmParams);
					}
					else
					{
						vector<Vec3d> orientations;

						// Have multiple hypotheses, check which one is best and keep it
						orientations.push_back(Vec3d(0,0,0));
						orientations.push_back(Vec3d(0,0.5236,0));
						orientations.push_back(Vec3d(0,-0.5236,0));
						orientations.push_back(Vec3d(0.5236,0,0));
						orientations.push_back(Vec3d(-0.5236,0,0));
				
						double best_lhood;
						Mat best_shape;

						for (size_t p = 0; p<orientations.size(); ++p)
						{

							Vec6d pose;		

							pose(1) = orientations[p][0];
							pose(2) = orientations[p][1];
							pose(3) = orientations[p][2];
					
							CLMWrapper::InitialiseCLM(gray, dImg, clmModel, pose, faceRegions[r], fx, fy, cx, cy, clmParams);

							double curr_lhood = clmModel._clm._likelihood;

							if(p==0 || curr_lhood > best_lhood)
							{
								best_lhood = curr_lhood;
								best_shape = clmModel._shape.clone();
							}
						}

						clmModel._shape = best_shape;
						clmModel._clm._likelihood = best_lhood;
					}

					// Writing out the detected landmarks
					if(!outfeaturess.empty())
					{
						char name[100];
						sprintf(name, "_det_%d", r);

						boost::filesystem::path slash("/");
						std::string preferredSlash = slash.make_preferred().string();

						// append detection number
						boost::filesystem::path out_feat_path(outfeaturess.at(i));
						boost::filesystem::path dir = out_feat_path.parent_path();
						boost::filesystem::path fname = out_feat_path.filename().replace_extension("");
						boost::filesystem::path ext = out_feat_path.extension();
						string outfeatures = dir.string() + preferredSlash + fname.string() + string(name) + ext.string();
						write_out_landmarks(outfeatures, clmModel);
					}

					// displaying detected stuff
					Mat disp;
					create_display_image(img, disp, clmModel);

					if(visualise)
					{
						// For debug purposes show final likelihood
						//std::ostringstream s;
						//s << clmModel._clm._likelihood;
						//string lhoodSt("Lhood:");
						//lhoodSt += s.str();
						//cv::putText(disp, lhoodSt, cv::Point(10,20), CV_FONT_HERSHEY_SIMPLEX, 0.5, CV_RGB(255,0,0));
						imshow("colour", disp);
						cv::waitKey(10);
					}

					if(!outimages.empty())
					{
						string outimage = outimages.at(i);
						if(!outimage.empty())
						{
							char name[100];
							sprintf(name, "_det_%d", r);

							boost::filesystem::path slash("/");
							std::string preferredSlash = slash.make_preferred().string();

							// append detection number
							boost::filesystem::path out_feat_path(outimage);
							boost::filesystem::path dir = out_feat_path.parent_path();
							boost::filesystem::path fname = out_feat_path.filename().replace_extension("");
							boost::filesystem::path ext = out_feat_path.extension();
							outimage = dir.string() + preferredSlash + fname.string() + string(name) + ext.string();

							imwrite(outimage, disp);	
						}
					}


				}
			}
			else
			{
				cout << "No faces detected" << endl;
				continue;
			}
		}
		else
		{
			initSuccess = true;

			// if pose defined we don't need the conventional initialisation			
			if(!clmParams.multi_view)
			{
				clmModel._clm._pglobl = Mat(initial_poses[i]);			

				Vec6d pose = CLMWrapper::GetPoseCLM(clmModel, dImg, fx, fy, cx, cy, clmParams);

				bool success = CLMWrapper::InitialiseCLM(gray, dImg, clmModel, pose, Rect(), fx, fy, cx, cy, clmParams);
			}
			else
			{
				vector<Vec3d> orientations;

				// Have multiple hypotheses, check which one is best and keep it
				orientations.push_back(Vec3d(0,0,0));
				orientations.push_back(Vec3d(0,0.5236,0));
				orientations.push_back(Vec3d(0,-0.5236,0));
				orientations.push_back(Vec3d(0.5236,0,0));
				orientations.push_back(Vec3d(-0.5236,0,0));
				
				double best_lhood;
				Mat best_shape;

				for (size_t p = 0; p<orientations.size(); ++p)
				{

					clmModel._clm._pglobl = Mat(initial_poses[i]);		

					clmModel._clm._pglobl.at<double>(1) = orientations[p][0];
					clmModel._clm._pglobl.at<double>(2) = orientations[p][1];
					clmModel._clm._pglobl.at<double>(3) = orientations[p][2];
					
					Vec6d pose = CLMWrapper::GetPoseCLM(clmModel, dImg, fx, fy, cx, cy, clmParams);

					CLMWrapper::InitialiseCLM(gray, dImg, clmModel, pose, Rect(), fx, fy, cx, cy, clmParams);

					double curr_lhood = clmModel._clm._likelihood;

					if(p==0 || curr_lhood > best_lhood)
					{
						best_lhood = curr_lhood;
						best_shape = clmModel._shape.clone();
					}
				}

				clmModel._shape = best_shape;

			}

			// Writing out the detected landmarks
			if(!outfeaturess.empty())
			{
				string outfeatures = outfeaturess.at(i);
				write_out_landmarks(outfeatures, clmModel);
			}

			// displaying detected stuff
			Mat disp;
			create_display_image(img, disp, clmModel);

			if(visualise)
			{
				imshow("colour", disp);
				cv::waitKey(5);
			}

			if(!outimages.empty())
			{
				string outimage = outimages.at(i);
				if(!outimage.empty())
				{
					imwrite(outimage, disp);	
				}
			}


		}				

		// Reset the parameters if more images are coming
		if(files.size() > 0)
		{
			clmModel._clm._plocal.setTo(0);
		}

	}
	return 0;
}
    void run(int /* start_from */ )
    {
        CvMat zeros;
        memset(&zeros, 0, sizeof(zeros));

        C_Caller caller, bad_caller;
        CvMat objectPoints_c, r_vec_c, t_vec_c, A_c, distCoeffs_c, imagePoints_c,
            dpdr_c, dpdt_c, dpdf_c, dpdc_c, dpdk_c;

        const int n = 10;

        Mat imagePoints_cpp(1, n, CV_32FC2); imagePoints_c = imagePoints_cpp;

        Mat objectPoints_cpp(1, n, CV_32FC3);
        randu(objectPoints_cpp, Scalar::all(1), Scalar::all(10));
        objectPoints_c = objectPoints_cpp;

        Mat t_vec_cpp(Mat::zeros(1, 3, CV_32F)); t_vec_c = t_vec_cpp;
        Mat r_vec_cpp;
        Rodrigues(Mat::eye(3, 3, CV_32F), r_vec_cpp); r_vec_c = r_vec_cpp;

        Mat A_cpp = camMat.clone(); A_c = A_cpp;
        Mat distCoeffs_cpp = distCoeffs.clone(); distCoeffs_c = distCoeffs_cpp;

        Mat dpdr_cpp(2*n, 3, CV_32F); dpdr_c = dpdr_cpp;
        Mat dpdt_cpp(2*n, 3, CV_32F); dpdt_c = dpdt_cpp;
        Mat dpdf_cpp(2*n, 2, CV_32F); dpdf_c = dpdf_cpp;
        Mat dpdc_cpp(2*n, 2, CV_32F); dpdc_c = dpdc_cpp;
        Mat dpdk_cpp(2*n, 4, CV_32F); dpdk_c = dpdk_cpp;

        caller.aspectRatio = 1.0;
        caller.objectPoints = &objectPoints_c;
        caller.r_vec = &r_vec_c;
        caller.t_vec = &t_vec_c;
        caller.A = &A_c;
        caller.distCoeffs = &distCoeffs_c;
        caller.imagePoints = &imagePoints_c;
        caller.dpdr = &dpdr_c;
        caller.dpdt = &dpdt_c;
        caller.dpdf = &dpdf_c;
        caller.dpdc = &dpdc_c;
        caller.dpdk = &dpdk_c;

        /********************/
        int errors = 0;


        bad_caller = caller;
        bad_caller.objectPoints = 0;
        errors += run_test_case( CV_StsBadArg, "Zero objectPoints", bad_caller );

        bad_caller = caller;
        bad_caller.r_vec = 0;
        errors += run_test_case( CV_StsBadArg, "Zero r_vec", bad_caller );

        bad_caller = caller;
        bad_caller.t_vec = 0;
        errors += run_test_case( CV_StsBadArg, "Zero t_vec", bad_caller );

        bad_caller = caller;
        bad_caller.A = 0;
        errors += run_test_case( CV_StsBadArg, "Zero camMat", bad_caller );

        bad_caller = caller;
        bad_caller.imagePoints = 0;
        errors += run_test_case( CV_StsBadArg, "Zero imagePoints", bad_caller );

        /****************************/
        Mat bad_r_vec_cpp1(r_vec_cpp.size(), CV_32S); CvMat bad_r_vec_c1 = bad_r_vec_cpp1;
        Mat bad_r_vec_cpp2(2, 2, CV_32F); CvMat bad_r_vec_c2 = bad_r_vec_cpp2;
        Mat bad_r_vec_cpp3(r_vec_cpp.size(), CV_32FC2); CvMat bad_r_vec_c3 = bad_r_vec_cpp3;

        bad_caller = caller;
        bad_caller.r_vec = &bad_r_vec_c1;
        errors += run_test_case( CV_StsBadArg, "Bad rvec format", bad_caller );

        bad_caller = caller;
        bad_caller.r_vec = &bad_r_vec_c2;
        errors += run_test_case( CV_StsBadArg, "Bad rvec format", bad_caller );

        bad_caller = caller;
        bad_caller.r_vec = &bad_r_vec_c3;
        errors += run_test_case( CV_StsBadArg, "Bad rvec format", bad_caller );

        /****************************/
        Mat bad_t_vec_cpp1(t_vec_cpp.size(), CV_32S); CvMat bad_t_vec_c1 = bad_t_vec_cpp1;
        Mat bad_t_vec_cpp2(2, 2, CV_32F); CvMat bad_t_vec_c2 = bad_t_vec_cpp2;
        Mat bad_t_vec_cpp3(1, 1, CV_32FC2); CvMat bad_t_vec_c3 = bad_t_vec_cpp3;

        bad_caller = caller;
        bad_caller.t_vec = &bad_t_vec_c1;
        errors += run_test_case( CV_StsBadArg, "Bad tvec format", bad_caller );

        bad_caller = caller;
        bad_caller.t_vec = &bad_t_vec_c2;
        errors += run_test_case( CV_StsBadArg, "Bad tvec format", bad_caller );

        bad_caller = caller;
        bad_caller.t_vec = &bad_t_vec_c3;
        errors += run_test_case( CV_StsBadArg, "Bad tvec format", bad_caller );

        /****************************/
        Mat bad_A_cpp1(A_cpp.size(), CV_32S); CvMat bad_A_c1 = bad_A_cpp1;
        Mat bad_A_cpp2(2, 2, CV_32F); CvMat bad_A_c2 = bad_A_cpp2;

        bad_caller = caller;
        bad_caller.A = &bad_A_c1;
        errors += run_test_case( CV_StsBadArg, "Bad A format", bad_caller );

        bad_caller = caller;
        bad_caller.A = &bad_A_c2;
        errors += run_test_case( CV_StsBadArg, "Bad A format", bad_caller );

        /****************************/
        Mat bad_distCoeffs_cpp1(distCoeffs_cpp.size(), CV_32S); CvMat bad_distCoeffs_c1 = bad_distCoeffs_cpp1;
        Mat bad_distCoeffs_cpp2(2, 2, CV_32F); CvMat bad_distCoeffs_c2 = bad_distCoeffs_cpp2;
        Mat bad_distCoeffs_cpp3(1, 7, CV_32F); CvMat bad_distCoeffs_c3 = bad_distCoeffs_cpp3;

        bad_caller = caller;
        bad_caller.distCoeffs = &zeros;
        errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller );

        bad_caller = caller;
        bad_caller.distCoeffs = &bad_distCoeffs_c1;
        errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller );

        bad_caller = caller;
        bad_caller.distCoeffs = &bad_distCoeffs_c2;
        errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller );

        bad_caller = caller;
        bad_caller.distCoeffs = &bad_distCoeffs_c3;
        errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller );


        /****************************/
        Mat bad_dpdr_cpp1(dpdr_cpp.size(), CV_32S); CvMat bad_dpdr_c1 = bad_dpdr_cpp1;
        Mat bad_dpdr_cpp2(dpdr_cpp.cols+1, 3, CV_32F); CvMat bad_dpdr_c2 = bad_dpdr_cpp2;
        Mat bad_dpdr_cpp3(dpdr_cpp.cols, 7, CV_32F); CvMat bad_dpdr_c3 = bad_dpdr_cpp3;

        bad_caller = caller;
        bad_caller.dpdr = &zeros;
        errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdr = &bad_dpdr_c1;
        errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdr = &bad_dpdr_c2;
        errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdr = &bad_dpdr_c3;
        errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller );

        /****************************/

        bad_caller = caller;
        bad_caller.dpdt = &zeros;
        errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdt = &bad_dpdr_c1;
        errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdt = &bad_dpdr_c2;
        errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdt = &bad_dpdr_c3;
        errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller );

        /****************************/

        Mat bad_dpdf_cpp2(dpdr_cpp.cols+1, 2, CV_32F); CvMat bad_dpdf_c2 = bad_dpdf_cpp2;

        bad_caller = caller;
        bad_caller.dpdf = &zeros;
        errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdf = &bad_dpdr_c1;
        errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdf = &bad_dpdf_c2;
        errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdf = &bad_dpdr_c3;
        errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller );

        /****************************/

        bad_caller = caller;
        bad_caller.dpdc = &zeros;
        errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdc = &bad_dpdr_c1;
        errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdc = &bad_dpdf_c2;
        errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdc = &bad_dpdr_c3;
        errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller );

        /****************************/

        bad_caller = caller;
        bad_caller.dpdk = &zeros;
        errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdk = &bad_dpdr_c1;
        errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdk = &bad_dpdf_c2;
        errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller );

        bad_caller = caller;
        bad_caller.dpdk = &bad_dpdr_c3;
        errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller );

        bad_caller = caller;
        bad_caller.distCoeffs = 0;
        errors += run_test_case( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not", bad_caller );


        if (errors)
            ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
        else
            ts->set_failed_test_info(cvtest::TS::OK);
    }
int main(int argc, char **argv) {
    try {
        ParseArgs(argc, argv);

        int num_cameras = static_cast<int>(imgs.size());
        if (num_cameras < 1)
            throw runtime_error("Need at least one camera");

        // Find features

        cout << "Finding features...\n";
        Ptr<detail::FeaturesFinder> features_finder = features_finder_creator->Create();

        for (int i = 0; i < num_cameras; ++i) {
            int64 t = getTickCount();
            cout << "Finding features in '" << img_names[i] << "'... ";

            Ptr<detail::ImageFeatures> features = new detail::ImageFeatures();
            (*features_finder)(imgs[i], *features);
            features_collection[i] = features;

            cout << "#features = " << features_collection.find(i)->second->keypoints.size()
                 << ", time = " << (getTickCount() - t) / getTickFrequency() << " sec\n";
        }

        // Match all pairs

        cout << "Matching pairs... ";
        MatchesCollection matches_collection;
        Ptr<detail::FeaturesMatcher> matcher = features_matcher_creator.Create();

        FeaturesCollection::iterator from_iter = features_collection.begin();
        FeaturesCollection::iterator from_next_iter = from_iter; ++from_next_iter;
        FeaturesCollection::iterator to_iter;

        for (; from_next_iter != features_collection.end(); from_iter = from_next_iter++) {
            for (to_iter = from_next_iter; to_iter != features_collection.end(); ++to_iter) {
                cout << "(" << from_iter->first << "->" << to_iter->first << ") ";
                detail::MatchesInfo mi;
                (*matcher)(*(from_iter->second), *(to_iter->second), mi);
                matches_collection[make_pair(from_iter->first, to_iter->first)]
                        = new vector<DMatch>(mi.matches);
            }
        }
        cout << endl;

        // Estimate homographies

        HomographiesP2 Hs;
        HomographiesP2 good_Hs;
        vector<Mat> Hs_from_0;
        RelativeConfidences rel_confs;
        Mat keypoints1, keypoints2;

        cout << "Estimating Hs...\n";
        for (int from = 0; from < num_cameras - 1; ++from) {
            for (int to = from + 1; to < num_cameras; ++to) {
                const vector<DMatch> &matches = *(matches_collection.find(make_pair(from, to))->second);

                cout << "Estimating H between '" << img_names[from] << "' and '" << img_names[to]
                     << "'... #matches = " << matches.size();

                if (static_cast<int>(matches.size()) < min_num_matches) {
                    cout << ", not enough matches\n";
                    continue;
                }

                ExtractMatchedKeypoints(*(features_collection.find(from)->second),
                                        *(features_collection.find(to)->second),
                                        matches, keypoints1, keypoints2);
                vector<uchar> inliers_mask;
                Mat_<double> H = findHomography(keypoints1.reshape(2), keypoints2.reshape(2),
                                                inliers_mask, RANSAC, H_est_thresh);

                if (H.empty()) {
                    cout << ", can't estimate H\n";
                    continue;
                }

                Ptr<vector<DMatch> > inliers = new vector<DMatch>();
                for (size_t i = 0; i < matches.size(); ++i)
                    if (inliers_mask[i])
                        inliers->push_back(matches[i]);
                cout << ", #inliers = " << inliers->size();

                double rms_err = 0;
                for (size_t i = 0; i < matches.size(); ++i) {
                    const Point2d &kp1 = keypoints1.at<Point2d>(0, i);
                    const Point2d &kp2 = keypoints2.at<Point2d>(0, i);
                    double x = H(0, 0) * kp1.x + H(0, 1) * kp1.y + H(0, 2);
                    double y = H(1, 0) * kp1.x + H(1, 1) * kp1.y + H(1, 2);
                    double z = H(2, 0) * kp1.x + H(2, 1) * kp1.y + H(2, 2);
                    x /= z; y /= z;
                    rms_err += (kp2.x - x) * (kp2.x - x) + (kp2.y - y) * (kp2.y - y);
                }
                rms_err = sqrt(rms_err / matches.size());
                cout << ", RMS err = " << rms_err;

                // See "Automatic Panoramic Image Stitching using Invariant Features"
                // by Matthew Brown and David G. Lowe, IJCV 2007 for the explanation
                double confidence = inliers->size() / (8 + 0.3 * matches.size()) - 1;

                rel_confs[make_pair(from, to)] = confidence;
                cout << ", conf = " << confidence;                

                cout << endl;                                

                Hs[make_pair(from, to)] = H;
                matches_collection[make_pair(from, to)] = inliers;

                if (confidence > 0)
                    good_Hs[make_pair(from, to)] = H;
                if (from == 0)
                    Hs_from_0.push_back(H);
            }
        }

        // Linear calibration

        if (K_init.empty()) {
            cout << "Linear calibrating...\n";
            if (lin_est_skew)
                K_init = CalibRotationalCameraLinear(good_Hs);
            else
                K_init = CalibRotationalCameraLinearNoSkew(good_Hs);
            cout << "K_init =\n" << K_init << endl;
        }

        // Non-linear refinement

        cout << "Refining camera...\n";

        if (Hs_from_0.size() != num_cameras - 1) {
            stringstream msg;
            msg << "Refinement requires Hs between first and all other images, "
                << "but only " << Hs_from_0.size() << " were/was found";
            throw runtime_error(msg.str());
        }

        map<int, Mat> Rs;
        Rs[0] = Mat::eye(3, 3, CV_64F);
        for (int i = 1; i < num_cameras; ++i)
            Rs[i] = K_init.inv() * Hs_from_0[i - 1] * K_init;

        Mat_<double> K_refined = K_init.clone();
        if (refine_skew)
            RefineRigidCamera(K_refined, Rs, features_collection, matches_collection);
        else {
            K_refined(0, 1) = 0;
            RefineRigidCamera(K_refined, Rs, features_collection, matches_collection,
                              REFINE_FLAG_K_ALL & ~REFINE_FLAG_K_SKEW);
        }
        cout << "K_refined =\n" << K_refined << endl;

        cout << "SUMMARY\n";
        cout << "K_init =\n" << K_init << endl;
        cout << "K_refined =\n" << K_refined << endl;
    }
    catch (const exception &e) {
        cout << "Error: " << e.what() << endl;
    }
    return 0;
}