예제 #1
0
void findEssentialMatrix(MFramePair& pair, Mat_<double> K) {
    vector<Point2f> k1, k2,n1,n2;
    vector<int> usedIndex1,usedIndex2;
    vector<uchar> status(pair.imgpts1.size());
    Mat F = findFundamentalMat(pair.matchPts1, pair.matchPts2, CV_FM_RANSAC, 0.2, 0.9,
                               status);
    Mat_<double> E;
    E = K.t() * F * K; //according to HZ (9.12)
    for (unsigned int i = 0; i < status.size(); i++) { // queryIdx is the "left" image
        if (status[i]) {
            usedIndex1.push_back(pair.matchedIndex1[i]);
            k1.push_back(pair.matchPts1[i]);
            usedIndex2.push_back(pair.matchedIndex2[i]);
            k2.push_back(pair.matchPts2[i]);
        }
    }
    correctMatches(F,k1,k2,n1,n2);
    pair.matchPts1 = n1;
    pair.matchPts2 = n2;
    pair.matchedIndex1=usedIndex1;
    pair.matchedIndex2=usedIndex2;
    pair.F = F;
    pair.E = E;
}
float TrainableStatModel::leaveOneOutCrossValidation(const Mat_<float> &samples, const Mat_<int> &classes) {
	int correctResults = 0;
	Mat_<int> sampleIdx(samples.rows - 1, 1);

	for (int i = 1; i < samples.rows; i++) {
		sampleIdx(i - 1, 0) = i;
	}

	for (int i = 0; i < samples.rows; i++) {
		this->clear();
		this->train(samples, classes, sampleIdx);
		int actual = (float)this->predict(samples.row(i));

		if (actual == classes(i,0)) {
			correctResults++;
		}

		cout<<"actual = "<<actual<<", expected = "<<classes(i,0)<<endl;

		sampleIdx(i, 0) = i;
	}

	return (float)correctResults/(float)samples.rows;
}
예제 #3
0
	static void drawOpticalFlow(const Mat_<Point2f>& flow, Mat& dst, float maxmotion = -1)
	{
	    dst.create(flow.size(), CV_8UC3);
	    dst.setTo(Scalar::all(0));
	
	    // determine motion range:
	    float maxrad = maxmotion;
	
	    if (maxmotion <= 0)
	    {
	        maxrad = 1;
	        for (int y = 0; y < flow.rows; ++y)
	        {
	            for (int x = 0; x < flow.cols; ++x)
	            {
	                Point2f u = flow(y, x);
	
	                if (!isFlowCorrect(u))
	                    continue;
	
	                maxrad = max(maxrad, sqrt(u.x * u.x + u.y * u.y));
	            }
	        }
	    }
	
	    for (int y = 0; y < flow.rows; ++y)
	    {
	        for (int x = 0; x < flow.cols; ++x)
	        {
	            Point2f u = flow(y, x);
	
	            if (isFlowCorrect(u))
	                dst.at<Vec3b>(y, x) = computeColor(u.x / maxrad, u.y / maxrad);
	        }
	    }
	}
예제 #4
0
bool SimpleNN::predict(const Mat_<double> &test_X, Mat_<double> &result, string &err_msg){
    
    Mat_<double> input_data = test_X.reshape(0, test_X.rows*test_X.cols); // make it column vector
    
    if (input_data.rows != this->structure[0]){
        err_msg = "wrong input size";
        return false;
    }
    
    for (int row_index = 1; row_index < this->layers[0].rows; ++row_index){
        this->layers[0](row_index, 0) = input_data(row_index-1, 0);
    }

    int num_layers = (int) this->layers.size();
    
    for (int layer_id = 0; layer_id < num_layers - 2; ++layer_id){
        Mat_<double> product = tanh(this->weights[layer_id]*this->layers[layer_id]);
        
        for (int row_index = 1; row_index < this->layers[layer_id+1].rows; ++row_index){
            this->layers[layer_id+1](row_index, 0) = product(row_index-1, 0);
        }
    }
    
    // compute the output layer
    {
        int layer_id = num_layers - 2;
        this->layers[layer_id + 1] = tanh(this->weights[layer_id] * this->layers[layer_id]);
    }
    
    result = this->layers[num_layers - 1]; // return last layers (output layer).
    cout << "result:\n" << result << endl;
    
    err_msg = "";
    
    return true;
}
예제 #5
0
Mat optical_flow(const Mat_<float>& ImgA, const Mat_<float>& ImgB, 
		 int num_it, float threshold) {

	// Compute Gaussin Pyramid 
	int nl = 5;
	float ds = 0.5;
	stack<pair<Mat, Mat> > gp = compute_gaussian_pyramids(ImgA, ImgB, nl, ds);
	
	Mat_<float> u = Mat::zeros(ImgA.size(), CV_32F);
	Mat_<float> v = Mat::zeros(ImgA.size(), CV_32F);
	while (!gp.empty()) {
	    Mat imgA = (gp.top()).first;
	    Mat imgB = (gp.top()).second;
	    
	    // Warp the first image. 
	    Mat_<float> imgBw = imgB.clone();
	   
	    /* Compute warping here from u and v. */
	    imgBw = compute_warp(imgB, u, v, ds);

	    /* Compute the derivatives. */
	    Mat_<float> Ix, Iy, It;
	    compute_derivatives(imgBw, imgA, Ix, Iy, It); // papers       
	    Mat_<float> du = Mat::zeros(imgA.size(), CV_32F);
	    Mat_<float> dv = Mat::zeros(imgA.size(), CV_32F);
	    for (int i = 0; i < 500; i ++) 
		  iterative_computation(du, dv, Ix, Iy, It);

	    u = u - du;
	    v = v - dv;
	    gp.pop();
	}
	
	Mat Mflow = color_map(u, v);
	return Mflow;
}
예제 #6
0
void NNLSOptimizer::scannls(const Mat& A, const Mat& b,Mat &x)
{
    int iter = 0;
    int m = A.size().height;
    int n = A.size().width;
    Mat_<double> AT = A.t();
    double error = 1e-8;
    Mat_<double> H = AT*A;
    Mat_<double> f = -AT*b;

    Mat_<double> x_old = Mat_<double>::zeros(n,1);
    Mat_<double> x_new = Mat_<double>::zeros(n,1);

    Mat_<double> mu_old = Mat_<double>::zeros(n,1);
    Mat_<double> mu_new = Mat_<double>::zeros(n,1);
    Mat_<double> r = Mat_<double>::zeros(n,1);
    f.copyTo(mu_old);

    while(iter < NNLS_MAX_ITER)
    {
        iter++;
        for(int k=0;k<n;k++)
        {
            x_old.copyTo(x_new);
            x_new(k,0) = std::max(0.0, x_old(k,0) - (mu_old(k,0)/H(k,k)) );

            if(x_new(k,0) != x_old(k,0))
            {
                r = mu_old + (x_new(k,0) - x_old(k,0))*H.col(k);
                r.copyTo(mu_new);
            }
            x_new.copyTo(x_old);
            mu_new.copyTo(mu_old);
        }

        if(eKKT(H,f,x_new,error) == true)
        {            
            break;
        }
    }
    x_new.copyTo(x);
}
void SegmenterHumanSimple::segment(const cv::Mat& img, Mat_<uchar>& mask)
{
	Mat imgBGR;
	Mat imgLAB;
	Mat imgBGRo;

	float rate = 500.0f/img.cols;

	GaussianBlur(img,imgBGRo,Size(),0.8,0.8);

	vector<Rect> faces;

	resize(imgBGRo,imgBGRo,Size(),rate,rate);
	cv::CascadeClassifier faceModel(this->_m_filenameFaceModel);
	faceModel.detectMultiScale(imgBGRo,faces);

	imgBGRo.convertTo( imgBGR, CV_32F, 1.0/255. );

	cvtColor( imgBGR, imgLAB, CV_BGR2Lab );

	Superpixel sp(1000,1,5);

	Mat_<int> segmentation = sp.segment(imgLAB);
	vector<SuperpixelStatistic> stat = sp.stat(imgLAB,imgBGR,segmentation);

	Mat_<float> prob;
	this->getPixelProbability(imgBGRo,prob,faces);
	Mat_<float> sprob;
	UtilsSuperpixel::Stat(segmentation,prob,stat,sprob);

	Mat_<int> initial(int(stat.size()),1);
	initial.setTo(1,sprob>0.5);
	initial.setTo(0,sprob<=0.5);
	Mat_<float> probaColor;
	int myx = cv::countNonZero(initial);
	this->_getColorProba(stat,initial,probaColor);

	Mat_<float> fgdInit,bgdInit,fgdColor,bgdColor;
	this->_prob2energy(sprob,fgdInit,bgdInit);
	this->_prob2energy(probaColor,fgdColor,bgdColor);
	Mat_<float> fgdEnergy, bgdEnergy;
	
	fgdEnergy = fgdInit + fgdColor;
	bgdEnergy = bgdInit + bgdColor;

	Mat_<int> label;
	mask.create(imgBGRo.rows,imgBGRo.cols);

	UtilsSegmentation::MaxFlowSuperpixel(stat,fgdEnergy,bgdEnergy,50.0,label);

	for( int i=0;i<mask.rows;i++)
	{
		for(int j=0;j<mask.cols;j++)
		{
			if ( label(segmentation(i,j)) > 0.5)
			{
				mask(i,j) = 255;
			}
			else
			{
				mask(i,j) = 0;
			}
		}
	}

	cv::resize(mask,mask,Size(img.cols,img.rows));
	mask.setTo(255,mask>128);
	mask.setTo(0,mask<=128);
}
    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);
    }
예제 #9
0
//===========================================================================
void SVR_patch_expert::Response(const Mat_<float>& area_of_interest, Mat_<double>& response)
{

	int response_height = area_of_interest.rows - weights.rows + 1;
	int response_width = area_of_interest.cols - weights.cols + 1;
	
	// the patch area on which we will calculate reponses
	cv::Mat_<float> normalised_area_of_interest;
  
	if(response.rows != response_height || response.cols != response_width)
	{
		response.create(response_height, response_width);
	}

	// If type is raw just normalise mean and standard deviation
	if(type == 0)
	{
		// Perform normalisation across whole patch
		cv::Scalar mean;
		cv::Scalar std;

		cv::meanStdDev(area_of_interest, mean, std);
		// Avoid division by zero
		if(std[0] == 0)
		{
			std[0] = 1;
		}
		normalised_area_of_interest = (area_of_interest - mean[0]) / std[0];
	}
	// If type is gradient, perform the image gradient computation
	else if(type == 1)
	{
		Grad(area_of_interest, normalised_area_of_interest);
	}
  	else
	{
		printf("ERROR(%s,%d): Unsupported patch type %d!\n", __FILE__,__LINE__, type);
		abort();
	}
	
	Mat_<float> svr_response;

	// The empty matrix as we don't pass precomputed dft's of image
	Mat_<double> empty_matrix_0(0,0,0.0);
	Mat_<float> empty_matrix_1(0,0,0.0);
	Mat_<float> empty_matrix_2(0,0,0.0);

	// Efficient calc of patch expert SVR response across the area of interest
	matchTemplate_m(normalised_area_of_interest, empty_matrix_0, empty_matrix_1, empty_matrix_2, weights, weights_dfts, svr_response, CV_TM_CCOEFF_NORMED); 

	response.create(svr_response.size());
	MatIterator_<double> p = response.begin();

	cv::MatIterator_<float> q1 = svr_response.begin(); // respone for each pixel
	cv::MatIterator_<float> q2 = svr_response.end();

	while(q1 != q2)
	{
		// the SVR response passed into logistic regressor
		*p++ = 1.0/(1.0 + exp( -(*q1++ * scaling + bias )));
	}

}
예제 #10
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;
}
예제 #11
0
파일: tree.cpp 프로젝트: xs2maverick/GBRT
int BaseDecisionTree::fit(Mat_<double> _X,
                          Mat_<double> _y,
                          Mat_<double> sample_weight)
{
    // Validation
    if (_X.rows == 0 || _X.cols == 0)
        return 1;

    // Determine output setting
    _n_samples = _X.rows;
    _n_features = _X.cols;

    // Reshape y to shape[n_samples, 1]
    _y = _y.reshape(1, _y.total());

    // Validation
    if (_y.rows != _n_samples)
        return 2;

    // Calculate class_weight
    Mat expended_class_weight(0, 0, CV_32F);
    // Get class_weight
    if (_class_weight.total() != 0)
        expended_class_weight = compute_sample_weight(_class_weight, _y);

    // Validation
    if (_max_depth <= 0)
        _max_depth = static_cast<int>(pow(2, 31) - 1);
    if (_max_leaf_nodes <= 0)
        _max_leaf_nodes = -1;
    if (_max_features <= 0)
        _max_features = _n_features;
    if (_max_leaf_nodes > -1 && _max_leaf_nodes < 2)
        return 3;
    if (_min_samples_split <= 0)
        return 4;
    if (_min_samples_leaf <= 0)
        return 5;
    if (_min_weight_fraction_leaf >= 0 && _min_weight_fraction_leaf <= 0.5)
        return 6;

    // Set samples' weight
    if (expended_class_weight.total())
    {
        for (int i = 0; i < sample_weight.total(); i++)
        {
            sample_weight.at<double>(i, 0) = sample_weight.at<double>(i, 0) * \
                                             expended_class_weight.at<double>(i, 0);
        }
    }
    else
    {
        sample_weight = expended_class_weight;
    }

    // Set min_weight_fraction_leaf
    if (_min_weight_fraction_leaf != 0.)
        _min_weight_fraction_leaf = _min_weight_fraction_leaf * cv::sum(sample_weight);
    else
        _min_weight_fraction_leaf = 0.;

    // Set min_samples_split
    _min_samples_split = max(_min_samples_split, 2 * _min_samples_leaf);




}
예제 #12
0
/**
* @param    avgSParam       - input        mean shape parameters
* @param    icovSParam      - input        covariance matrix of shape parameters
* @param    avgTParam       - input        mean texture parameters
* @param    icovTParam      - input        covariance matrix of texture parameters
* @param    iSParams        - input        the vector of multiple input shape parameters
* @param    iTParams        - input        the vector of multiple input texture parameter
* @param    ShapeDistMean   - input        mean texture parameters
* @param    ShapeDistStddev - input        covariance matrix of texture parameters
* @param    TextureDistMean - input        the input shape parameter
* @param    TextureDistStddev   - input    the input texture parameter
* @param    WeakFitting     - input        only shape parameter is used?
* @return   whether the fitting is acceptable
*/
bool CRecognitionAlgs::CalcFittingEffect4ImageSequence( 
    const Mat_<float>& avgSParam,
    const Mat_<float>& icovSParam,
    const Mat_<float>& avgTParam,
    const Mat_<float>& icovTParam,
    const Mat_<float>& iSParams,
    const Mat_<float>& iTParams,
    const Scalar& ShapeDistMean,
    const Scalar& ShapeDistStddev,
    const Scalar& TextureDistMean,
    const Scalar& TextureDistStddev,
    bool WeakFitting )
{
    assert(iSParams.rows == iTParams.rows);
    unsigned int NbOfSamples = iSParams.rows;
    vector<float> sDists, tDists;
    sDists.resize(NbOfSamples);
    tDists.resize(NbOfSamples);

    for(unsigned int i = 0; i < NbOfSamples; ++i)
    {
        CRecognitionAlgs::CalcFittingEffect4StaticImage(
            avgSParam,
            icovSParam,
            avgTParam,
            icovTParam,
            iSParams.row(i),
            iTParams.row(i),
            ShapeDistMean,
            ShapeDistStddev,
            TextureDistMean,
            TextureDistStddev,
            sDists[i],
            tDists[i],
            WeakFitting );
    }

    unsigned int NbOfGood1 = 0;
    unsigned int NbOfGood2 = 0;

    for(unsigned int i = 0; i < NbOfSamples; ++i)
    {
        if( ( fabs( sDists[i] - ShapeDistMean.val[0] )
            < 1.5f * ShapeDistStddev.val[0] ) )
        {
            NbOfGood1++;
            if( ( fabs( tDists[i] - TextureDistMean.val[0] )
                < 3.0f*TextureDistStddev.val[0] ) )
            {
                NbOfGood2++;
            }
        }
    }

    if(WeakFitting)
    {
        if(NbOfGood1 >= (unsigned int )(0.75*NbOfSamples) )
            return true;
        else
            return false;
    }
    else
    {
        if(NbOfGood2 >= (unsigned int )(0.75*NbOfGood1) )
            return true;
        else
            return false;
    }
}
예제 #13
0
void UtilsSegmentation::MaxFlowSuperpixel(std::vector<SuperpixelStatistic>& spstat, const Mat_<float>& fgdEnergy,
		const Mat_<float>& bgdEnergy, float gamma, Mat_<int>& label)
{
	//::Graph<float,float,float> graph(nNode,nEdge,errfunc);
	//graph
	int nEdge = UtilsSuperpixel::CountEdge(spstat);
	Mat_<int> edgeVertex(nEdge,2);
	Mat_<float> edgeWeight(nEdge,1);
	Mat_<float> edgeLen(nEdge,1);

	int idx = 0;
	for(int i=0;i<spstat.size();i++)
	{
		SuperpixelStatistic& sp = spstat[i];
		for( set<int>::iterator j=sp.conn.begin();
			j!= sp.conn.end();
			j++)
		{
			int d = (*j);
			SuperpixelStatistic& dsp = spstat[d];
			if ( i != d)
			{
				edgeVertex(idx,0) = min(i,d);
				edgeVertex(idx,1) = max(i,d);
				float diff = (float) norm(sp.mean_color_ - dsp.mean_color_);
				edgeWeight(idx) = diff*diff;
				edgeLen(idx) = (float) cv::norm(sp.mean_position_-dsp.mean_position_);
				idx++;
			}
		}
	}

	float beta = (float) cv::mean(edgeWeight)[0];

	Graph<float,float,float> graph((int)spstat.size(), nEdge, errfunc);

	graph.add_node((int)spstat.size());

	for(int i=0;i<fgdEnergy.total();i++)
	{
		graph.add_tweights(i,bgdEnergy(i),fgdEnergy(i));
	}

	edgeWeight = - edgeWeight / beta;
	cv::exp(edgeWeight,edgeWeight);
	edgeWeight *= gamma;
	cv::divide(edgeWeight, edgeLen,edgeWeight);

	for(int i=0;i<nEdge;i++)
	{
		float w = edgeWeight(i);
		graph.add_edge(edgeVertex(i,0),edgeVertex(i,1),w,w);
	}

	graph.maxflow();

	label.create((int)spstat.size(),1);
	for(int i=0;i<spstat.size();i++)
	{
		if ( graph.what_segment(i) == Graph<float,float,float>::SOURCE)
		{
			label(i) = 1;
		}
		else
		{
			label(i) = 0;
		}
	}
}
bool c_FourierTransfrom::ifftw_complex_3d(const Mat_<Vec6d> &_input,
                                         Mat_<Vec6d> &_output)
{
    size_t height = _input.rows;
    size_t width = _input.cols;
    size_t n_channels = _input.channels() / 2;
    size_t n_pixels = height * width;
    size_t n_data = n_pixels * n_channels;

    fftw_complex *in, *out;
    fftw_plan p;

    in = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * n_data);
    out = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * n_data);

    p = fftw_plan_dft_3d(height, width, n_channels, in, out, FFTW_BACKWARD,
                         FFTW_ESTIMATE);

    /*!< prepare the data */
    for (size_t i_row = 0; i_row < height; ++i_row)
    {
        const Vec3d *p = _input.ptr<Vec3d>(i_row);
        for (size_t i_col = 0; i_col < width; ++i_col)
        {
            size_t index = i_row * width + i_col;
            for (size_t k = 0; k < n_channels; ++k)
            {
                in[n_pixels * k + index][0] = p[i_col][k];
                in[n_pixels * k + index][1] = p[i_col][k + n_channels];
            }
#if 0
            in[index][0] = p[i_col][4];
            in[index][1] = p[i_col][5];
            in[n_pixels + index][0] = p[i_col][2];
            in[n_pixels + index][1] = p[i_col][3];
            in[n_pixels * 2 + index][0] = p[i_col][0];
            in[n_pixels * 2 + index][1] = p[i_col][1];
#endif
        }
    }

    fftw_execute(p);

    /*!< write back data */
    _output = Mat_<Vec6d>::zeros(_input.size());
    for (size_t i_row = 0; i_row < height; ++i_row)
    {
        Vec6d *p = _output.ptr<Vec6d>(i_row);
        for (size_t i_col = 0; i_col < width; ++i_col)
        {
            size_t index = i_row * width + i_col;
            for (size_t k = 0; k < n_channels; ++k)
            {
                p[i_col][k] = out[n_pixels * k + index][0];
                p[i_col][k + n_channels] = out[n_pixels * k + index][1];
            }
#if 0
            p[i_col][0] = out[n_pixels * 2 + index][0];
            p[i_col][1] = out[n_pixels + index][0];
            p[i_col][2] = out[index][0];
            p[i_col][3] = out[n_pixels * 2 + index][1];
            p[i_col][4] = out[n_pixels + index][1];
            p[i_col][5] = out[index][1];
#endif
        }
    }

    _output /= n_data;

    fftw_destroy_plan(p);
    fftw_free(in);
    fftw_free(out);

    return true;
}
예제 #15
0
void collectData(int subjId,
				 CascadeClassifier &classifier,
				 ShapePredictor &predictor,
				 Mat_<float> &labels,
				 Mat_<float> &multihog,
				 Mat_<float> &landmarks)
{
	int H[] = { -15, -10, -5, 0, 5, 10, 15 };
	int V[] = { -10, 0, 10 };

	string path = to_string(subjId) + "/";
	if (subjId < 10) path = "columbia/000" + path;
	else path = "columbia/00" + path;
	ifstream fin(path + "annotation.txt");

	for (int imgId = 0; imgId < 105; imgId++) {
		int p, v, h;
		fin >> p >> v >> h;
		if (abs(p) > 15) continue;
		string imgpath = path + to_string(imgId) + ".jpg";
		Mat_<uchar> img = imread(imgpath, 0);
		BBox bbox = getTestBBox(img, classifier);
		if (EmptyBox(bbox)) continue;

		int l = 0;
		// EYE, MOUTH, NOF
		if (abs(h) <= 5 && v == 0) l = 0;
		else if (abs(h) <= 5 && v == -10) l = 1;
		else l = 2;

		if (l == 2) {
			RNG rng(getTickCount());
			double num = rng.uniform(0.0, 1.0);
			if (num > 0.5) continue;
		}

		// 上中下
		/*if (v < 0) l = 0;
		else if (v == 0) l = 1;
		else l = 2;*/

		// 9分类
		/*if (h < -5) l += 0;
		else if (h > 5) l += 2;
		else l += 1;
		if (v < 0) l += 0;
		else if (v > 0) l += 2 * 3;
		else l += 1 * 3;*/

		Mat_<float> lab = l*Mat_<float>::ones(1, 1);
		labels.push_back(lab);

		Mat_<double> shape = predictor(img, bbox);
		Geom G;	initGeom(shape, G);
		Pose P; calcPose(G, P);

		Mat_<uchar> lEye, rEye;
		regularize(img, bbox, P, shape, lEye, rEye);

		vector<float> lRlt;
		vector<float> rRlt;
		calcMultiHog(lEye, lRlt);
		calcMultiHog(rEye, rRlt);

		vector<float> _hog2nd_vec;
		for (int k = 0; k < lRlt.size(); k++)
			_hog2nd_vec.push_back(lRlt[k]);
		for (int k = 0; k < rRlt.size(); k++)
			_hog2nd_vec.push_back(rRlt[k]);
		Mat_<float> _hog2nd_row = Mat_<float>(_hog2nd_vec).reshape(1, 1);
		multihog.push_back(_hog2nd_row);

		vector<float> _ldmks;
		for (int i = 28; i < 48; i++) {
			_ldmks.push_back((shape(i, 0) - bbox.cx) / bbox.w);
			_ldmks.push_back((shape(i, 1) - bbox.cy) / bbox.h);
		}
		float mouthx = (shape(51, 0) + shape(62, 0) + shape(66, 0) + shape(57, 0)) / 4;
		float mouthy = (shape(51, 1) + shape(62, 1) + shape(66, 1) + shape(57, 1)) / 4;
		_ldmks.push_back((mouthx - bbox.cx) / bbox.w);
		_ldmks.push_back((mouthy - bbox.cy) / bbox.h);
		float maxVal = *std::max_element(_ldmks.begin(), _ldmks.end());
		for (int i = 0; i < _ldmks.size(); i++) _ldmks[i] *= 1.0 / maxVal; // scale to [-1, 1]

		Mat_<float> ldmks = Mat_<float>(_ldmks).reshape(1, 1);
		landmarks.push_back(ldmks);
	}
	fin.close();
}
예제 #16
0
void computePoseDifference(Mat img1, Mat img2, CommandArgs args, Mat k, Mat& dist_coefficients, double& worldScale, Mat& R, Mat& t, Mat& img_matches)
{
   cout << "%===============================================%" << endl;

   Mat camera_matrix = k.clone();
   if (args.resize_factor > 1) 
   {
      resize(img1, img1, Size(img1.cols / args.resize_factor, 
               img1.rows / args.resize_factor)); // make smaller for performance and displayablity
      resize(img2, img2, Size(img2.cols / args.resize_factor,
               img2.rows / args.resize_factor));
      // scale matrix down according to changed resolution
      camera_matrix = camera_matrix / args.resize_factor;
      camera_matrix.at<double>(2,2) = 1;
   }

   Mat K1, K2;
   K1 = K2 = camera_matrix;
   if (img1.rows > img1.cols) // it is assumed the camera has been calibrated in landscape mode, so undistortion must also be performed in landscape orientation, or the camera matrix must be modified (fx,fy and cx,cy need to be exchanged)
   {
      swap(K1.at<double>(0,0), K1.at<double>(1,1));
      swap(K1.at<double>(0,2), K1.at<double>(1,2));
   }
   if (img2.rows > img2.cols)
   {
      swap(K2.at<double>(0,0), K2.at<double>(1,1));
      swap(K2.at<double>(0,2), K2.at<double>(1,2));
   }

   // Feature detection + extraction
   vector<KeyPoint> KeyPoints_1, KeyPoints_2;
   Mat descriptors_1, descriptors_2;

   Ptr<Feature2D> feat_detector;
   if (args.detector == DETECTOR_KAZE) 
   {
      feat_detector = AKAZE::create(args.detector_data.upright ? AKAZE::DESCRIPTOR_MLDB_UPRIGHT : AKAZE::DESCRIPTOR_MLDB, 
            args.detector_data.descriptor_size,
            args.detector_data.descriptor_channels,
            args.detector_data.threshold,
            args.detector_data.nOctaves,
            args.detector_data.nOctaveLayersAkaze);

   } else if (args.detector == DETECTOR_SURF)
   {
      feat_detector = xfeatures2d::SURF::create(args.detector_data.minHessian, 
            args.detector_data.nOctaves, args.detector_data.nOctaveLayersAkaze, args.detector_data.extended, args.detector_data.upright);
   } else if (args.detector == DETECTOR_SIFT)
   {
      feat_detector = xfeatures2d::SIFT::create(args.detector_data.nFeatures, 
            args.detector_data.nOctaveLayersSift, args.detector_data.contrastThreshold, args.detector_data.sigma);
   }

   feat_detector->detectAndCompute(img1, noArray(), KeyPoints_1, descriptors_1);
   feat_detector->detectAndCompute(img2, noArray(), KeyPoints_2, descriptors_2);

   cout << "Number of feature points (img1, img2): " << "(" << KeyPoints_1.size() << ", " << KeyPoints_2.size() << ")" << endl;

   // Find correspondences
   BFMatcher matcher;
   vector<DMatch> matches;
   if (args.use_ratio_test) 
   {
      if (args.detector == DETECTOR_KAZE) 
         matcher = BFMatcher(NORM_HAMMING, false);
      else matcher = BFMatcher(NORM_L2, false);

      vector<vector<DMatch>> match_candidates;
      const float ratio = args.ratio;
      matcher.knnMatch(descriptors_1, descriptors_2, match_candidates, 2);
      for (int i = 0; i < match_candidates.size(); i++)
         if (match_candidates[i][0].distance < ratio * match_candidates[i][1].distance)
            matches.push_back(match_candidates[i][0]);

      cout << "Number of matches passing ratio test: " << matches.size() << endl;

   } else
   {
      if (args.detector == DETECTOR_KAZE) 
         matcher = BFMatcher(NORM_HAMMING, true);
      else matcher = BFMatcher(NORM_L2, true);
      matcher.match(descriptors_1, descriptors_2, matches);
      cout << "Number of matching feature points: " << matches.size() << endl;
   }


   // Convert correspondences to vectors
   vector<Point2f>imgpts1,imgpts2;

   for(unsigned int i = 0; i < matches.size(); i++) 
   {
      imgpts1.push_back(KeyPoints_1[matches[i].queryIdx].pt); 
      imgpts2.push_back(KeyPoints_2[matches[i].trainIdx].pt); 
   }

   Mat mask; // inlier mask
   if (args.undistort) 
   {
      undistortPoints(imgpts1, imgpts1, K1, dist_coefficients, noArray(), K1);
      undistortPoints(imgpts2, imgpts2, K2, dist_coefficients, noArray(), K2);
   } 

   double focal = camera_matrix.at<double>(0,0);
   Point2d principalPoint(camera_matrix.at<double>(0,2),camera_matrix.at<double>(1,2));

   Mat E = findEssentialMat(imgpts1, imgpts2, focal, principalPoint, RANSAC, 0.999, 1, mask);
   /* Mat F = camera_matrix.t().inv() * E * camera_matrix.inv(); */
   Mat F = findFundamentalMat(imgpts1, imgpts2, CV_FM_RANSAC);

   correctMatches(F, imgpts1, imgpts2, imgpts1, imgpts2);
   cout << "Reprojection error:\n " << computeReprojectionError(imgpts1, imgpts2, mask, F) << endl;

   int inliers = recoverPose(E, imgpts1, imgpts2, R, t, focal, principalPoint, mask);

   cout << "Matches used for pose recovery:\n " << inliers << endl;

   /* Mat R1, R2, ProjMat1, ProjMat2, Q; */
   /* stereoRectify(camera_matrix, dist_coefficients, camera_matrix, dist_coefficients, img1.size(), R, t, R1, R2, ProjMat1, ProjMat2, Q); */
   /* cout << "P1=" << ProjMat1 << endl; */
   /* cout << "P2=" << ProjMat2 << endl; */
   /* cout << "Q=" << Q << endl; */

   Mat mtxR, mtxQ;
   Mat Qx, Qy, Qz;
   Vec3d angles = RQDecomp3x3(R, mtxR, mtxQ, Qx, Qy, Qz);
   /* cout << "Qx:\n " << Qx << endl; */
   /* cout << "Qy:\n " << Qy << endl; */
   /* cout << "Qz:\n " << Qz << endl; */
   cout << "Translation:\n " << t.t() << endl;
   cout << "Euler angles [x y z] in degrees:\n " << angles.t() << endl;

   if (args.epilines)
   {
      drawEpilines(Mat(imgpts1), 1, F, img2);
      drawEpilines(Mat(imgpts2), 2, F, img1);
   }

   drawMatches(img1, KeyPoints_1, img2, KeyPoints_2, // draw only inliers given by mask
         matches, img_matches, Scalar::all(-1), Scalar::all(-1), mask);

   vector<Point2f> imgpts1_masked, imgpts2_masked;
   for (int i = 0; i < imgpts1.size(); i++) 
   {
      if (mask.at<uchar>(i,0) == 1) 
      {
         imgpts1_masked.push_back(imgpts1[i]);
         imgpts2_masked.push_back(imgpts2[i]);
      }
   }

   Mat pnts4D;
   Mat P1 = camera_matrix * Mat::eye(3, 4, CV_64FC1), P2;
   Mat p2[2] = { R, t }; 
   hconcat(p2, 2, P2);
   P2 = camera_matrix * P2;

#define USE_OPENCV_TRIANGULATION
#ifndef USE_OPENCV_TRIANGULATION // strangely, both methods yield identical results
   vector<Point3d> homogPoints1, homogPoints2;
   for (int i = 0; i < imgpts1_masked.size(); i++) 
   {
      Point2f currentPoint1 = imgpts1_masked[i];
      homogPoints1.push_back(Point3d(currentPoint1.x, currentPoint1.y, 1));
      Point2f currentPoint2 = imgpts2_masked[i];
      homogPoints2.push_back(Point3d(currentPoint2.x, currentPoint2.y, 1));
   }

   Mat dehomogenized(imgpts1_masked.size(), 3, CV_64FC1);
   for (int i = 0; i < imgpts1_masked.size(); i++) 
   {
      Mat_<double> triangulatedPoint = IterativeLinearLSTriangulation(homogPoints1[i], P1, homogPoints2[i], P2);
      Mat r = triangulatedPoint.t();
      r.colRange(0,3).copyTo(dehomogenized.row(i)); // directly assigning to dehomogenized.row(i) compiles but does nothing, wtf?
   }
#else
   triangulatePoints(P1, P2, imgpts1_masked, imgpts2_masked, pnts4D);
   pnts4D = pnts4D.t();
   Mat dehomogenized;
   convertPointsFromHomogeneous(pnts4D, dehomogenized);
   dehomogenized = dehomogenized.reshape(1); // instead of 3 channels and 1 col, we want 1 channel and 3 cols
#endif


   double mDist = 0;
   int n = 0;
   int pos = 0, neg = 0;

   /* Write ply file header */
   ofstream ply_file("points.ply", ios_base::trunc);
   ply_file << 
      "ply\n"
      "format ascii 1.0\n"
      "element vertex " << dehomogenized.rows << "\n"
      "property float x\n"
      "property float y\n"
      "property float z\n"
      "property uchar red\n"
      "property uchar green\n"
      "property uchar blue\n"
      "end_header" << endl;

   Mat_<double> row;
   for (int i = 0; i < dehomogenized.rows; i++) 
   {
      row = dehomogenized.row(i);
      double d = row(2);
      if (d > 0) 
      {
         pos++;
         mDist += norm(row);
         n++;
         /* float startx=imgpts1_masked[i].x - 1, starty=imgpts1_masked[i].y - 1, endx=imgpts1_masked[i].x + 1, endy=imgpts1_masked[i].y + 1; */
         /* cout << "startx,endx = " << startx << "," << endx << endl; */
         /* cout << "starty,endy = " << starty << "," << endy << endl; */
         Vec3b rgb = img1.at<Vec3b>(imgpts1_masked[i].x, imgpts1_masked[i].y);
         ply_file << row(0) << " " << row(1) << " " << row(2) << " " << (int)rgb[2] << " " << (int)rgb[1] << " " << (int)rgb[0] << "\n";
      } else
      {
         neg++;
         ply_file << 0 << " " << 0 << " " << 0 << " " << 0 << " " << 0 << " " << 0 << "\n"; 
      }
   }
   ply_file.close();
   mDist /= n;
   worldScale = mDist;
   cout << "Mean distance of " << n << " points to camera:\n " << mDist << " (dehomogenized)" << endl;
   cout << "pos=" << pos << ", neg=" << neg << endl;


   /* char filename[100]; */
   /* sprintf(filename, "mat_1%d", i+1); */

   /* Ptr<Formatter> formatter = Formatter::get(Formatter::FMT_CSV); */
   /* Ptr<Formatted> formatted = formatter->format(dehomogenized); */
   /* ofstream file(filename, ios_base::trunc); */
   /* file << formatted << endl; */

   /* Removed until cmake has been fathomed */
   /* vector< Point3d > points3D; */
   /* vector< vector< Point2d > > pointsImg; */
   /* int NPOINTS=dehomogenized.rows; // number of 3d points */
   /* int NCAMS=2; // number of cameras */

   /* points3D.resize(NPOINTS); */
   /* for (int i = 0; i < NPOINTS; i++) */ 
   /* { */
   /*    points3D[i] = Point3d(dehomogenized.at<double>(i,0), */
   /*          dehomogenized.at<double>(i,1), */
   /*          dehomogenized.at<double>(i,2) */
   /*          ); */
   /* } */
   /* // fill image projections */
   /* vector<vector<int> > visibility(2, vector<int>(NPOINTS, 1)); */
   /* vector<Mat> camera_matrices(2, camera_matrix); */
   /* vector<Mat> Rs(2); */
   /* Rodrigues(Mat::eye(3, 3, CV_64FC1), Rs[0]); */
   /* Rodrigues(R, Rs[0]); */
   /* vector<Mat> Ts = { Mat::zeros(3,1, CV_64FC1), t }; */
   /* vector<Mat> dist_coefficientss(2, dist_coefficients); */

   /* pointsImg.resize(NCAMS); */
   /* for(int i=0; i<NCAMS; i++) pointsImg[i].resize(NPOINTS); */
   /* for (int i = 0; i < NPOINTS; i++) */ 
   /* { */
   /*    pointsImg[0][i] = Point2d(imgpts1_masked[i].x, imgpts1_masked[i].y); */
   /*    pointsImg[1][i] = Point2d(imgpts2_masked[i].x, imgpts2_masked[i].y); */
   /* } */
   /*  cvsba::Sba sba; */
   /*   sba.run(points3D, pointsImg, visibility, camera_matrices, Rs, Ts, dist_coefficientss); */

   /*   cout<<"Initial error="<<sba.getInitialReprjError()<<". "<< */
   /*              "Final error="<<sba.getFinalReprjError()<<endl; */

   cout << "%===============================================%" << endl;
}
int main(int argc, char **argv)
{
  ros::init(argc, argv, "rovioTest");
  ros::NodeHandle n;
  imageClient = n.serviceClient<rovio_base::image>("rovioImage");
  controlClient = n.serviceClient<rovio_base::manDrv>("rovioControl");
  reportClient = n.serviceClient<rovio_base::report>("rovioReport");

  Mat imgOrd = getImage();
  cvtColor(imgOrd, imgOrd, CV_RGB2GRAY);
  const int AREA_SIZE = 16;
  int ordRows = imgOrd.rows;
  int ordCols = imgOrd.cols;
  int areaNumY = ordRows / AREA_SIZE;
  int areaNumX = ordCols / AREA_SIZE;
  int marginY = ordRows % AREA_SIZE;
  int areaNum = areaNumX * areaNumY;
  Rect ior(0, marginY, areaNumX * AREA_SIZE, areaNumY * AREA_SIZE);
  Mat img = getImg(imgOrd, ior);
  VideoWriter videoWriter("/home/viki/Rovio.avi", CV_FOURCC('M', 'J', 'P', 'G'), 3.0, Size(img.cols, img.rows));
  int lastDirection = 1;
  for (int i = 0; i < 1000; i++)
  {
    Mat img = getImg(getImage(), ior);
    Mat_<int> regionMap(img.size());
    regionMap.setTo(0);
    regionMap(regionMap.rows - 18, regionMap.cols / 2) = 1;
    regionMap(regionMap.rows - 10, regionMap.cols / 2) = 1;
    RegionGrowthAlg alg;
    alg.calcRegionMap(img, regionMap);

    // Detect forward
    int fAreaWidth = img.cols - 200;
    int fAreaHeight = img.rows / 4;
    int fTopX = (img.cols - fAreaWidth) / 2;
    int fTopY = img.rows - fAreaHeight;
    int ignorePixels = 1000;
    Mat_<int> fArea = regionMap(Rect(fTopX, fTopY, fAreaWidth, fAreaHeight));
    int fAreaSum = 0;
    for (int i = 0; i < fArea.rows; i++)
    {
      int* pRow = fArea.ptr<int>(i);
      for (int j = 0; j < fArea.cols; j++)
      {
        if (pRow[j] == 0)
          fAreaSum++;
      }
    }
    bool flagForward = fAreaSum < ignorePixels;

    // Detect left and right
    int lrAreaWidth = 100;
    int marginX = 0;
    int lrAreaHeight = fAreaHeight;
    int lrTopY = img.rows - lrAreaHeight;
    int lTopX = marginX;
    int rTopX = img.cols - marginX - lrAreaWidth;
    int lrIgnorePixels = 1000;
    Mat_<int> lArea = regionMap(Rect(lTopX, lrTopY, lrAreaWidth, lrAreaHeight));
    Mat_<int> rArea = regionMap(Rect(rTopX, lrTopY, lrAreaWidth, lrAreaHeight));
    int lAreaSum = 0;
    int rAreaSum = 0;
    for (int i = 0; i < lArea.rows; i++)
    {
      int* plRow = lArea.ptr<int>(i);
      int* prRow = rArea.ptr<int>(i);
      for (int j = 0; j < lArea.cols; j++)
      {
        if (plRow[j] == 0)
          lAreaSum++;
        if (prRow[j] == 0)
          rAreaSum++;
      }
    }
    bool flagLeft = lAreaSum < lrIgnorePixels;
    bool flagRight = rAreaSum < lrIgnorePixels;

    //fArea.setTo(2);
    lArea.setTo(3);
    rArea.setTo(4);

    Utility util;
    util.drawSegmentBorder(img, regionMap);

    // Mark info
    //标记
    int leftSum = 0;
    int rightSum = 0;
    int loopi = img.rows;
    int loopj = img.cols;
    for (int i = 0; i < loopi; i++)
    {
      int* pLeftRow = regionMap.ptr<int>(i);
      int* pRIghtRow = pLeftRow + loopj / 2;
      int loop = loopj / 2;
      for (int j = 0; j < loop; j++)
      {
        if (pLeftRow[j] > 0)
        {
          leftSum++;
        }
        if (pRIghtRow[j] > 0)
        {
          rightSum++;
        }
      }
    }
    Point pos(loopj / 2 - 150, loopi / 2);
    std::stringstream ss;
    string tmp;
    ss << leftSum;
    ss >> tmp;
    putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0));
    pos.x = loopj / 2 + 100;
    ss.str("");
    ss.clear();
    ss << rightSum;
    ss >> tmp;
    putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0));
    int textLen = 40;
    pos.x = fArea.cols / 2 - textLen + fTopX;
    pos.y = fArea.rows / 2 + fTopY;
    ss.str("");
    ss.clear();
    ss << fAreaSum;
    ss >> tmp;
    putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255));
    pos.x = lArea.cols / 2 - textLen + lTopX;
    pos.y = lArea.rows / 2 + lrTopY;
    ss.str("");
    ss.clear();
    ss << lAreaSum;
    ss >> tmp;
    putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255));
    pos.x = rArea.cols / 2 - textLen + rTopX;
    pos.y = rArea.rows / 2 + lrTopY;
    ss.str("");
    ss.clear();
    ss << rAreaSum;
    ss >> tmp;
    putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255));

    //检测直线区域
    int lineLen = 200;
    int lineStartX = img.cols / 2 - lineLen / 2;
    int lineEndX = img.cols / 2 + lineLen / 2;
    int lineY = img.rows - 140;
    Point lineStart(lineStartX, lineY);
    Point lineEnd(lineEndX, lineY);
    line(img, lineStart, lineEnd, Scalar(255, 20, 20));

    int blockNum = 0;
    int* pLineRow = regionMap.ptr<int>(lineY);
    for (int j = lineStartX; j < lineEndX; j++)
    {
      if (pLineRow[j] == 0)
      {
        blockNum++;
      }
    }
    bool isBlocked = blockNum > lineLen / 2;

    //视频
    //cvtColor(img, img, CV_GRAY2RGB);
    imshow("", img);
    waitKey(10);
    videoWriter << img;

    //控制
    isBlocked = (!flagLeft && !flagRight) || isBlocked;
    int waitTime = 1;
    rvMCUReport rvMcu = getReport();
    if (rvMcu.isIrOn && rvMcu.isDetectedBarrier)
      isBlocked = true;
    if (true)
    {
      if (isBlocked)
      {
        int maxDif = 5000;
        if (leftSum - rightSum > maxDif)
        {
          lastDirection = -1;
        }
        else if (rightSum - leftSum > maxDif)
        {
          lastDirection = 1;
        }
        if (lastDirection == -1)
        {
          control(5, 8);
          waitKey(waitTime);
        }
        else
        {
          control(6, 8);
          waitKey(waitTime);
        }
      }
      else if (flagForward)
      {
        control(1, 6);
      }
      else if (flagLeft)
      {
        control(3, 8);
      }
      else if (flagRight)
      {
        control(4, 8);
      }
      else
      {
        printf("Error control");
      }
    }

  }

  return 0;
}
예제 #18
0
// We are implementing SLIC here. I'm too lazy to enfoce the connectivity
Mat_< int > Superpixel::slic( const Mat_< Vec3f >& im ) const {
	// Compute the spacing and grid size of the superpixels
	double sp_area = 1.0 * im.cols * im.rows / K_;
	int Kx = 0.5 + im.cols / sqrt( sp_area ), Ky = 0.5 + im.rows / sqrt( sp_area );
	int K = Kx*Ky;
	
	int win_sz = 1.0 * sqrt(sp_area) + 1;
	
	// Initialize the seeds on a regular grid
	std::vector< int64_t > cnt( K );
	std::vector< Point2d > seedsd( K );
	std::vector< Point > seeds( K );
	for( int i=0,k=0; i<Kx; i++ )
		for( int j=0; j<Ky; j++, k++ )
			seeds[k] = Point( (i+0.5)*(im.cols-1)/Kx, (j+0.5)*(im.rows-1)/Ky );
	
	// Run k-means
	Mat_<float> dist( im.size() );
	Mat_<int> label( im.size() );
	for( int it=0; it<n_iter_; it++ ) {
		// Assignment step
		dist = std::numeric_limits<float>::max();
		label = -1;
		for( int k=0; k<K; k++ ) {
			Vec3f c = im( seeds[k] );
			for( int j=std::max(0,seeds[k].y-win_sz); j<im.rows && j<=seeds[k].y+win_sz; j++ )
				for( int i=std::max(0,seeds[k].x-win_sz); i<im.cols && i<=seeds[k].x+win_sz; i++ ){
					double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y);
					double cd = ( im( j, i ) - c ).dot( im( j, i ) - c );
					d += col_w_ * col_w_ * cd;
					if( d < dist( j, i ) ) {
						dist( j, i ) = d;
						label( j, i ) = k;
					}
				}
		}
		
		// Update
		for( int k=0; k<K; k++ ) {
			seedsd[k] = Point2d(0,0);
			cnt[k] = 0;
		}
		for( int j=0; j<im.rows; j++ )
			for( int i=0; i<im.cols; i++ ) {
				// Fix all the pixels we messed up!
				if ( label( j, i ) < 0 ) {
// 					printf("Oops that wasn't very slick :(\n");
					for( int k=0; k<K; k++ ){
						Vec3f c = im( seeds[k] );
						double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y);
						double cd = ( im( j, i ) - c ).dot( im( j, i ) - c );
						d += col_w_ * col_w_ * cd;
						if( d < dist( j, i ) ) {
							dist( j, i ) = d;
							label( j, i ) = k;
						}
					}
				}
				
				seedsd[ label( j, i ) ] += Point2d( i, j );
				cnt[ label( j, i ) ] += 1;
			}
		
		for( int k=0; k<K; k++ )
			if (cnt[k] > 0)
				seeds[k] = Point( 0.5 + seedsd[k].x / cnt[k], 0.5 + seedsd[k].y / cnt[k] );
	}
	return label;
}
예제 #19
0
int copyMakeBorder(/*const*/ Mat_<_Tp, chs>& src, Mat_<_Tp, chs>& dst, int top, int bottom, int left, int right, int borderType, const Scalar& value = Scalar())
{
	FBC_Assert(top >= 0 && bottom >= 0 && left >= 0 && right >= 0);

	if (src.isSubmatrix() && (borderType & BORDER_ISOLATED) == 0) {
		Size wholeSize;
		Point ofs;
		src.locateROI(wholeSize, ofs);
		int dtop = std::min(ofs.y, top);
		int dbottom = std::min(wholeSize.height - src.rows - ofs.y, bottom);
		int dleft = std::min(ofs.x, left);
		int dright = std::min(wholeSize.width - src.cols - ofs.x, right);
		src.adjustROI(dtop, dbottom, dleft, dright);
		top -= dtop;
		left -= dleft;
		bottom -= dbottom;
		right -= dright;
	}

	if (dst.empty() || dst.rows != (src.rows + top + bottom) || dst.cols != (src.cols + left + right)) {
		dst.release();
		dst = Mat_<_Tp, chs>(src.rows + top + bottom, src.cols + left + right);
	}

	if (top == 0 && left == 0 && bottom == 0 && right == 0) {
		if (src.data != dst.data || src.step != dst.step)
			src.copyTo(dst);
		return 0;
	}

	borderType &= ~BORDER_ISOLATED;

	if (borderType != BORDER_CONSTANT) {
		copyMakeBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, src.elemSize(), borderType);
	} else {
		int cn = src.channels, cn1 = cn;
		AutoBuffer<double> buf(cn);

		scalarToRawData<_Tp, chs>(value, buf, cn);
		copyMakeConstBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, (int)src.elemSize(), (uchar*)(double*)buf);
	}

	return 0;
}
예제 #20
0
Mat_< int > Superpixel::geodesicSegmentation( const Mat_< Vec3f >& im ) const {
	//std::tr1::uniform_int<int> distribution(-2, 2);
	//std::tr1::mt19937 engine; // Mersenne twister MT19937
	//auto randint = std::tr1::bind(distribution, engine);
	int s;
	srand((unsigned)time(NULL));
	s=rand()%4-2;
	
	// Compute the spacing and grid size of the superpixels
	double sp_area = 1.0 * im.cols * im.rows / K_;
	int Kx = 0.5 + im.cols / sqrt( sp_area ), Ky = 0.5 + im.rows / sqrt( sp_area );
	int K = Kx*Ky;
	
	int win_sz = 1.0 * sqrt(sp_area) + 1;
	
	// Initialize the seeds on a regular grid
	std::vector< int64_t > cnt( K );
	std::vector< Point2d > seedsd( K );
	std::vector< Point > seeds( K );
	for( int i=0,k=0; i<Kx; i++ )
		for( int j=0; j<Ky; j++, k++ )
			seeds[k] = Point( (i+0.5)*(im.cols-1)/Kx, (j+0.5)*(im.rows-1)/Ky ) + Point(rand()%4-2,rand()%4-2);/*Point( randint(), randint() );*/
	
	Mat_<float> dx( im.size() ), dy( im.size() );
	
	for( int j=0; j<im.rows; j++ )
		for( int i=0; i<im.cols; i++ ) {
			if (i)
				dx(j,i-1) = col_w_*sqrt( (im(j,i)-im(j,i-1)).dot(im(j,i)-im(j,i-1)) ) + 1;
			if (j)
				dy(j-1,i) = col_w_*sqrt( (im(j-1,i)-im(j,i)).dot(im(j-1,i)-im(j,i)) ) + 1;
		}
	
	// Run k-means
	Mat_<float> dist( im.size() );
	Mat_<int> label( im.size() );
	for( int it=0; it<n_iter_; it++ ) {
		// Assignment step
		dist = std::numeric_limits<float>::max();
		label = -1;
		for( int k=0; k<K; k++ ) {
			dist( seeds[k] ) = 0;
			label( seeds[k] ) = k;
		}
		for( int IT=0; IT<2; IT++ ){
			for( int j=0; j<im.rows; j++ )
				for( int i=0; i<im.cols; i++ ) {
					if (i && dist(j,i-1) + dx(j,i-1) < dist(j,i)) {
						dist(j,i) = dist(j,i-1) + dx(j,i-1);
						label(j,i) = label(j,i-1);
					}
					if (j && dist(j-1,i) + dx(j-1,i) < dist(j,i)) {
						dist(j,i) = dist(j-1,i) + dy(j-1,i);
						label(j,i) = label(j-1,i);
					}
				}
			for( int j=im.rows-1; j>=0; j-- )
				for( int i=im.cols-1; i>=0; i-- ) {
					if (i && dist(j,i) + dx(j,i-1) < dist(j,i-1)) {
						dist(j,i-1) = dist(j,i) + dx(j,i-1);
						label(j,i-1) = label(j,i);
					}
					if (j && dist(j,i) + dx(j-1,i) < dist(j-1,i)) {
						dist(j-1,i) = dist(j,i) + dy(j-1,i);
						label(j-1,i) = label(j,i);
					}
				}
		}
		for( int k=0; k<K; k++ ) {
			Vec3f c = im( seeds[k] );
			for( int j=std::max(0,seeds[k].y-win_sz); j<im.rows && j<=seeds[k].y+win_sz; j++ )
				for( int i=std::max(0,seeds[k].x-win_sz); i<im.cols && i<=seeds[k].x+win_sz; i++ ){
					double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y);
					double cd = ( im( j, i ) - c ).dot( im( j, i ) - c );
					d += col_w_ * col_w_ * cd;
					if( d < dist( j, i ) ) {
						dist( j, i ) = d;
						label( j, i ) = k;
					}
				}
		}
		
		// Update
		for( int k=0; k<K; k++ ) {
			seedsd[k] = Point2d(0,0);
			cnt[k] = 0;
		}
		for( int j=0; j<im.rows; j++ )
			for( int i=0; i<im.cols; i++ ) {
				// Fix all the pixels we messed up!
				if ( label( j, i ) < 0 ) {
// 					printf("Oops that wasn't very slick :(\n");
					for( int k=0; k<K; k++ ){
						Vec3f c = im( seeds[k] );
						double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y);
						double cd = ( im( j, i ) - c ).dot( im( j, i ) - c );
						d += col_w_ * col_w_ * cd;
						if( d < dist( j, i ) ) {
							dist( j, i ) = d;
							label( j, i ) = k;
						}
					}
				}
				
				seedsd[ label( j, i ) ] += Point2d( i, j );
				cnt[ label( j, i ) ] += 1;
			}
		
		for( int k=0; k<K; k++ )
			if (cnt[k] > 0)
				seeds[k] = Point( 0.5 + seedsd[k].x / cnt[k], 0.5 + seedsd[k].y / cnt[k] );
	}
	return label;
}
void FaceAnalyser::AddNextFrame(const cv::Mat& frame, const CLMTracker::CLM& clm_model, double timestamp_seconds, bool online, bool visualise)
{
	// Check if a reset is needed first (TODO same person no reset)
	//if(face_bounding_box.area() > 0)
	//{
	//	Rect_<double> new_bounding_box = clm.GetBoundingBox();

	//	// If the box overlaps do not need a reset
	//	double intersection_area = (face_bounding_box & new_bounding_box).area();
	//	double union_area = face_bounding_box.area() + new_bounding_box.area() - 2 * intersection_area;

	//	// If the model is already tracking what we're detecting ignore the detection, this is determined by amount of overlap
	//	if( intersection_area/union_area < 0.5)
	//	{
	//		this->Reset();
	//	}

	//	face_bounding_box = new_bounding_box;
	//}
	//if(!clm.detection_success)
	//{
	//	this->Reset();
	//}

	frames_tracking++;

	// First align the face if tracking was successfull
	if(clm_model.detection_success)
	{
		AlignFaceMask(aligned_face, frame, clm_model, triangulation, true, align_scale, align_width, align_height);
	}
	else
	{
		aligned_face = Mat(align_height, align_width, CV_8UC3);
		aligned_face.setTo(0);
	}

	if(aligned_face.channels() == 3)
	{
		cvtColor(aligned_face, aligned_face_grayscale, CV_BGR2GRAY);
	}
	else
	{
		aligned_face_grayscale = aligned_face.clone();
	}

	// Extract HOG descriptor from the frame and convert it to a useable format
	Mat_<double> hog_descriptor;
	Extract_FHOG_descriptor(hog_descriptor, aligned_face, this->num_hog_rows, this->num_hog_cols);

	// Store the descriptor
	hog_desc_frame = hog_descriptor;

	Vec3d curr_orient(clm_model.params_global[1], clm_model.params_global[2], clm_model.params_global[3]);
	int orientation_to_use = GetViewId(this->head_orientations, curr_orient);

	// Only update the running median if predictions are not high
	// That is don't update it when the face is expressive (just retrieve it)
	bool update_median = true;

	// TODO test if this would be useful or not
	//if(!this->AU_predictions.empty())
	//{
	//	for(size_t i = 0; i < this->AU_predictions.size(); ++i)
	//	{
	//		if(this->AU_predictions[i].second > 1)
	//		{
	//			update_median = false;				
	//			break;
	//		}
	//	}
	//}

	update_median = update_median & clm_model.detection_success;

	// A small speedup
	if(frames_tracking % 2 == 1)
	{
		UpdateRunningMedian(this->hog_desc_hist[orientation_to_use], this->hog_hist_sum[orientation_to_use], this->hog_desc_median, hog_descriptor, update_median, this->num_bins_hog, this->min_val_hog, this->max_val_hog);
	}	
	// Geom descriptor and its median
	geom_descriptor_frame = clm_model.params_local.t();
	
	if(!clm_model.detection_success)
	{
		geom_descriptor_frame.setTo(0);
	}

	// Stack with the actual feature point locations (without mean)
	Mat_<double> locs = clm_model.pdm.princ_comp * geom_descriptor_frame.t();
	
	cv::hconcat(locs.t(), geom_descriptor_frame.clone(), geom_descriptor_frame);
	
	// A small speedup
	if(frames_tracking % 2 == 1)
	{
		UpdateRunningMedian(this->geom_desc_hist, this->geom_hist_sum, this->geom_descriptor_median, geom_descriptor_frame, update_median, this->num_bins_geom, this->min_val_geom, this->max_val_geom);
	}

	// First convert the face image to double representation as a row vector
	Mat_<uchar> aligned_face_cols(1, aligned_face.cols * aligned_face.rows * aligned_face.channels(), aligned_face.data, 1);
	Mat_<double> aligned_face_cols_double;
	aligned_face_cols.convertTo(aligned_face_cols_double, CV_64F);
	
	// TODO get rid of this completely as it takes too long?
	//UpdateRunningMedian(this->face_image_hist[orientation_to_use], this->face_image_hist_sum[orientation_to_use], this->face_image_median, aligned_face_cols_double, update_median, 256, 0, 255);

	// Visualising the median HOG
	if(visualise)
	{
		FaceAnalysis::Visualise_FHOG(hog_descriptor, num_hog_rows, num_hog_cols, hog_descriptor_visualisation);
	}

	// Perform AU prediction	
	AU_predictions_reg = PredictCurrentAUs(orientation_to_use);

	std::vector<std::pair<std::string, double>> AU_predictions_reg_corrected;
	if(online)
	{
		AU_predictions_reg_corrected = CorrectOnlineAUs(AU_predictions_reg, orientation_to_use, true, false, clm_model.detection_success);
	}

	// Keep only closer to in-plane faces
	double angle_norm = cv::sqrt(clm_model.params_global[2] * clm_model.params_global[2] + clm_model.params_global[3] * clm_model.params_global[3]);

	// Add the reg predictions to the historic data
	for (size_t au = 0; au < AU_predictions_reg.size(); ++au)
	{

		// Find the appropriate AU (if not found add it)		
		// Only add if the detection was successful and not too out of plane
		if(clm_model.detection_success && angle_norm < 0.4)
		{
			AU_predictions_reg_all_hist[AU_predictions_reg[au].first].push_back(AU_predictions_reg[au].second);
		}
		else
		{
			AU_predictions_reg_all_hist[AU_predictions_reg[au].first].push_back(-100.0);
		}
	}
	
	AU_predictions_class = PredictCurrentAUsClass(orientation_to_use);

	for (size_t au = 0; au < AU_predictions_class.size(); ++au)
	{

		// Find the appropriate AU (if not found add it)		
		// Only add if the detection was successful and not too out of plane
		if(clm_model.detection_success && angle_norm < 0.4)
		{
			AU_predictions_class_all_hist[AU_predictions_class[au].first].push_back(AU_predictions_class[au].second);
		}
		else
		{
			AU_predictions_class_all_hist[AU_predictions_class[au].first].push_back(-100.0);
		}
	}
	

	if(online)
	{
		AU_predictions_reg = AU_predictions_reg_corrected;
	}

	this->current_time_seconds = timestamp_seconds;

	view_used = orientation_to_use;
			
	bool success = clm_model.detection_success && angle_norm < 0.4;

	confidences.push_back(clm_model.detection_certainty);
	valid_preds.push_back(success);
}
예제 #22
0
파일: spmbp.cpp 프로젝트: IanDaisy/spm-bp
void spm_bp::runspm_bp(cv::Mat_<cv::Vec2f>& flowResult)
{
    clock_t start_disp, finish_disp;
    start_disp = clock();
    printf("==================================================\n");
    printf("SPM-BP begins\n");

    Mat_<Vec2f> label_k(height1 * width1, NUM_TOP_K);
    Mat_<float> dcost_k(height1 * width1, NUM_TOP_K);

#ifdef _WIN32
    printf("Running on Windows\n");
    // Dummy code that speeds up program on windows considerably
    // Save label
    vector<vector<Vec2f> > label_saved(numOfSP1);
    //label_saved.reserve(10000);
    vector<vector<Mat_<float> > > dcost_saved(numOfSP1);
    //dcost_saved.reserve(10000);

    for (int i = 0; i < numOfSP1; i++) {
        label_saved[i].reserve(100000);
        dcost_saved[i].reserve(100000);
    }
#endif

    //initiate labels
    init_label_super(label_k, dcost_k);

    //MESSAGE 0:left, 1:right, 2:up, 3:down
    Mat_<Vec<float, NUM_TOP_K> > message(height1 * width1, 4);
    message.setTo(0);

    //precompute smooth weight
    float omega[256];
    for (int i = 0; i < 256; ++i)
        omega[i] = lambda_smooth * std::exp(-float(i) / 20);

    // TODO: Change to Mat4f
    Mat_<Vec4f> smoothWt(height1, width1);
    smoothWt.setTo(lambda_smooth);

// # pragma omp parallel for
    for (int i = 1; i < height1 - 1; ++i)
    {
        for (int j = 1; j < width1 - 1; ++j) {
            const Vec3f &ref = im1f[i][j];
            // TODO: Don't need abs here since norm >= 0
            // smoothWt[i][j][0] = omega[int(abs(norm(ref - im1f[i][j - 1])))];
            // smoothWt[i][j][1] = omega[int(abs(norm(ref - im1f[i][j + 1])))];
            // smoothWt[i][j][2] = omega[int(abs(norm(ref - im1f[i - 1][j])))];
            // smoothWt[i][j][3] = omega[int(abs(norm(ref - im1f[i + 1][j])))];

            smoothWt[i][j][0] = omega[int(norm(ref - im1f[i][j - 1]))];
            smoothWt[i][j][1] = omega[int(norm(ref - im1f[i][j + 1]))];
            smoothWt[i][j][2] = omega[int(norm(ref - im1f[i - 1][j]))];
            smoothWt[i][j][3] = omega[int(norm(ref - im1f[i + 1][j]))];
        }
    }

    float dis_belief_l[NUM_TOP_K];
    float dis_belief_r[NUM_TOP_K];
    float dis_belief_u[NUM_TOP_K];
    float dis_belief_d[NUM_TOP_K];

    const int BUFSIZE = NUM_TOP_K * 50;

    vector<Vec2f> vec_label(BUFSIZE), vec_label_nei(BUFSIZE);
    vector<float> vec_mes_l(BUFSIZE),
                  vec_mes_r(BUFSIZE),
                  vec_mes_u(BUFSIZE),
                  vec_mes_d(BUFSIZE),
                  vec_belief(BUFSIZE),
                  vec_d_cost(BUFSIZE);
    Mat_<float> DataCost_nei;

    start_disp = clock();
    double wall_timer = omp_get_wtime();
    if (display)
        Show_WTA_Flow(-1, label_k, dcost_k, message, flowResult);

    int spBegin, spEnd, spStep;
    for (int iter = 0; iter < iterNum; iter++) {
        if (iter % 2) {
            spBegin = numOfSP1 - 1, spEnd = -1, spStep = -1;
        }
        else {
            spBegin = 0, spEnd = numOfSP1, spStep = 1;
        }

        for (int sp = spBegin; sp != spEnd; sp += spStep) {
            int curSPP = superpixelsList1[sp][0];
            Vec4i curRange = subRange1[curSPP];
            int y = curRange[0];
            int x = curRange[1];
            int w = curRange[2] - curRange[0] + 1;
            int h = curRange[3] - curRange[1] + 1;

            // rand neighbor pixel and store
            vec_label_nei.clear();
            std::set<int>::iterator sIt;
            std::set<int>& sAdj = spGraph1[iter % 2].adjList[sp];
            for (sIt = sAdj.begin(); sIt != sAdj.end(); ++sIt) {
                repPixels1[*sIt] = superpixelsList1[*sIt][rand() % superpixelsList1[*sIt].size()];
                Vec2f test_label;
                //vector<Vec2f> & cur_label_k = label_k[repPixels1[*sIt]];
                for (int k = 0; k < NUM_TOP_K; k++) {
                    //test_label= cur_label_k[k];
                    test_label = label_k[repPixels1[*sIt]][k];
                    if (isNewLabel_PMF_PMBP(vec_label_nei, test_label) == -1)
                        vec_label_nei.push_back(test_label);
                }
            }

            repPixels1[sp] = superpixelsList1[sp][rand() % superpixelsList1[sp].size()];
            for (int k = 0; k < NUM_TOP_K; k++) {
                Vec2f test_label = label_k[repPixels1[sp]][k];
                float mag = min(disp_range_u, disp_range_v) / 8;
                for (; mag >= (1.0 / upScale); mag /= 2.0) {
                    Vec2f test_label_random;
                    float tmpVerLabel = test_label[0] + ((float(rand()) / RAND_MAX) - 0.5) * 2.0 * mag;
                    float tmpHorLabel = test_label[1] + ((float(rand()) / RAND_MAX) - 0.5) * 2.0 * mag;

                    test_label_random[0] = floor(tmpVerLabel * upScale + 0.5) / upScale;
                    test_label_random[1] = floor(tmpHorLabel * upScale + 0.5) / upScale;

                    if (test_label_random[0] >= -disp_range_u && test_label_random[0] <= disp_range_u
                        && test_label_random[1] >= -disp_range_v && test_label_random[1] <= disp_range_v
                        && isNewLabel_PMF_PMBP(vec_label_nei, test_label_random) == -1) //here
                    {
                        vec_label_nei.push_back(test_label_random);
                    }
                }
            }

            const int vec_size = vec_label_nei.size();

            // DataCost_nei.release();
            DataCost_nei.create(h, w * vec_size);
            DataCost_nei.setTo(0);
#if USE_CLMF0_TO_AGGREGATE_COST
            cv::Mat_<cv::Vec4b> leftCombinedCrossMap;
            leftCombinedCrossMap.create(h, w);
            subCrossMap1[sp].copyTo(leftCombinedCrossMap);
            CFFilter cff;
#endif

// Compute matching costs for each candidate particles
            // printf("Computing matching costs start\n");
#pragma omp parallel for num_threads(NTHREADS)
            for (int i = 0; i < vec_size; ++i) {
                int kx = i * w;
                Mat_<float> rawCost;
                getLocalDataCostPerlabel(sp, vec_label_nei[i], rawCost);
#if USE_CLMF0_TO_AGGREGATE_COST
                cff.FastCLMF0FloatFilterPointer(leftCombinedCrossMap, rawCost, rawCost);
#endif
                rawCost.copyTo(DataCost_nei(cv::Rect(kx, 0, w, h)));
            }
            //getLocalDataCost(sp, vec_label_nei, DataCost_nei);

            // printf("Computing matching costs end\n");

            Vec4i curRange_s = spRange1[curSPP];
            // int spw = curRange_s[2] - curRange_s[0] + 1;
            // int sph = curRange_s[3] - curRange_s[1] + 1;

            int spy_s, spy_e, spx_s, spx_e, spx_step, spy_step;
            spy_s = curRange_s[0];
            spy_e = curRange_s[2] + 1;
            spx_s = curRange_s[1];
            spx_e = curRange_s[3] + 1;
            spx_step = 1;
            spy_step = 1;

            if (iter % 4 == 1) {
                spy_s = curRange_s[2];
                spy_e = curRange_s[0] - 1;
                spx_s = curRange_s[3];
                spx_e = curRange_s[1] - 1;
                spx_step = -1;
                spy_step = -1;
            }
            else if (iter % 4 == 2) {
                spy_s = curRange_s[2];
                spy_e = curRange_s[0] - 1;
                spy_step = -1;
                spx_s = curRange_s[1];
                spx_e = curRange_s[3] + 1;
                spx_step = 1;
            }
            else if (iter % 4 == 3) 
            {
                spy_s = curRange_s[0];
                spy_e = curRange_s[2] + 1;
                spy_step = 1;
                spx_s = curRange_s[3];
                spx_e = curRange_s[1] - 1;
                spx_step = -1;
            }

            for (int bi = spx_s; bi != spx_e; bi = bi + spx_step)
                for (int bj = spy_s; bj != spy_e; bj = bj + spy_step) {

                    int p1 = bi * width1 + bj;
                    int pl = bi * width1 + (bj - 1);
                    int pu = (bi - 1) * width1 + bj;
                    int pr = bi * width1 + (bj + 1);
                    int pd = (bi + 1) * width1 + bj;

                    //Compute disbelief: three incoming message + data_cost
                    for (int k = 0; k < NUM_TOP_K; ++k) {
                        if (bj != 0)
                            dis_belief_l[k] = message[pl][0][k] + message[pl][2][k] + message[pl][3][k] + dcost_k[pl][k];
                        if (bj != width1 - 1)
                            dis_belief_r[k] = message[pr][1][k] + message[pr][2][k] + message[pr][3][k] + dcost_k[pr][k];
                        if (bi != 0)
                            dis_belief_u[k] = message[pu][0][k] + message[pu][1][k] + message[pu][2][k] + dcost_k[pu][k];
                        if (bi != height1 - 1)
                            dis_belief_d[k] = message[pd][0][k] + message[pd][1][k] + message[pd][3][k] + dcost_k[pd][k];
                    }

                    vec_label.clear();
                    vec_mes_l.clear();
                    vec_mes_r.clear();
                    vec_mes_u.clear();
                    vec_mes_d.clear();
                    vec_belief.clear();
                    vec_d_cost.clear();

                    // Update and save messages with current reference pixel's labels
                    Vec4f wt_s = smoothWt[bi][bj];

                    for (int k = 0; k < NUM_TOP_K; ++k) {
                        Vec2f test_label = label_k[p1][k];
                        vec_label.push_back(test_label);
                        float dcost = dcost_k[p1][k];
                        vec_d_cost.push_back(dcost);
                        //start_disp = clock();
                        float _mes_l = 0, _mes_r = 0, _mes_u = 0, _mes_d = 0;
                        if (bj != 0) {
                            _mes_l = ComputeMes_PMBP_per_label(dis_belief_l, label_k, pl, test_label, wt_s[0], tau_s);
                            vec_mes_l.push_back(_mes_l);
                        }
                        if (bj != width1 - 1) {
                            _mes_r = ComputeMes_PMBP_per_label(dis_belief_r, label_k, pr, test_label, wt_s[1], tau_s);
                            vec_mes_r.push_back(_mes_r);
                        }
                        if (bi != 0) {
                            _mes_u = ComputeMes_PMBP_per_label(dis_belief_u, label_k, pu, test_label, wt_s[2], tau_s);
                            vec_mes_u.push_back(_mes_u);
                        }
                        if (bi != height1 - 1) {
                            _mes_d = ComputeMes_PMBP_per_label(dis_belief_d, label_k, pd, test_label, wt_s[3], tau_s);
                            vec_mes_d.push_back(_mes_d);
                        }
                        vec_belief.push_back(_mes_l + _mes_r + _mes_u + _mes_d + dcost);
                    }

                    //propagation + random search

                    int kx;
                    for (int test_id = 0; test_id < vec_label_nei.size(); ++test_id) {
                        Vec2f test_label = vec_label_nei[test_id];
                        //if(isNewLabel_PMF_PMBP(vec_label,test_label)==-1)
                        {
                            vec_label.push_back(test_label);
                            kx = test_id * w;
                            const Mat_<float>& local = DataCost_nei(cv::Rect(kx, 0, w, h));
                            float dcost = local[bi - x][bj - y];
                            vec_d_cost.push_back(dcost);
                            //start_disp = clock();
                            float _mes_l = 0, _mes_r = 0, _mes_u = 0, _mes_d = 0;
                            if (bj != 0) {
                                _mes_l = ComputeMes_PMBP_per_label(dis_belief_l, label_k, pl, test_label, wt_s[0], tau_s);
                                vec_mes_l.push_back(_mes_l);
                            }
                            if (bj != width1 - 1) {
                                _mes_r = ComputeMes_PMBP_per_label(dis_belief_r, label_k, pr, test_label, wt_s[1], tau_s);
                                vec_mes_r.push_back(_mes_r);
                            }
                            if (bi != 0) {
                                _mes_u = ComputeMes_PMBP_per_label(dis_belief_u, label_k, pu, test_label, wt_s[2], tau_s);
                                vec_mes_u.push_back(_mes_u);
                            }
                            if (bi != height1 - 1) {
                                _mes_d = ComputeMes_PMBP_per_label(dis_belief_d, label_k, pd, test_label, wt_s[3], tau_s);
                                vec_mes_d.push_back(_mes_d);
                            }
                            vec_belief.push_back(_mes_l + _mes_r + _mes_u + _mes_d + dcost);
                        }
                    }

                    Compute_top_k(vec_belief, vec_label, vec_mes_l, vec_mes_r, vec_mes_u, vec_mes_d, vec_d_cost, message, label_k, dcost_k, p1, NUM_TOP_K);
                    //cout<<label_k[p1][0]<<endl;
                    Message_normalization_PMF_PMBP(message, p1, NUM_TOP_K);
                }
            //cout<<"Message "<<clock()-start_disp<<endl;
            // propagation end
        } //superpixel scan end
        if (display)
            Show_WTA_Flow(iter, label_k, dcost_k, message, flowResult);
        finish_disp = clock();
        std::cout << " time on clock(): " << (double)(clock() - start_disp) / CLOCKS_PER_SEC
                  << " time on wall: " << omp_get_wtime() - wall_timer << "\n";
    } //iteration
    Show_WTA_Flow(iterNum, label_k, dcost_k, message, flowResult);
    printf("==================================================\n");
    printf("SPM-BP finished\n==================================================\n");
}
예제 #23
0
void CThinPlateSpline::computeSplineCoeffs(std::vector<Point>& iPIn, std::vector<Point>& iiPIn, float lambda,const TPS_INTERPOLATION tpsInter)
{

	std::vector<Point>* iP = NULL;
	std::vector<Point>*	iiP = NULL;

	if(tpsInter == FORWARD_WARP)
	{
		iP = &iPIn;
		iiP = &iiPIn;

		// keep information which coefficients are set
		FLAG_COEFFS_BACK_WARP_SET = true;
		FLAG_COEFFS_FORWARD_WARP_SET = false;
	}
	else if(tpsInter == BACK_WARP)
	{
		iP = &iiPIn;
		iiP = &iPIn;

		// keep information which coefficients are set
		FLAG_COEFFS_BACK_WARP_SET = false;
		FLAG_COEFFS_FORWARD_WARP_SET = true;
	}

	//get number of corresponding points
	int dim = 2;
	int n = iP->size();

	//Initialize mathematical datastructures
	Mat_<float> V(dim,n+dim+1,0.0);
	Mat_<float> P(n,dim+1,1.0);
	Mat_<float> K = (K.eye(n,n)*lambda);
	Mat_<float> L(n+dim+1,n+dim+1,0.0);

	// fill up K und P matrix
	std::vector<Point>::iterator itY;
	std::vector<Point>::iterator itX;

	int y = 0;
	for (itY = iP->begin(); itY != iP->end(); ++itY, y++) {
		int x = 0;
		for (itX = iP->begin(); itX != iP->end(); ++itX, x++) {
			if (x != y) {
				K(y, x) = (float)fktU(*itY, *itX);
			}
		}
		P(y,1) = (float)itY->y;
		P(y,2) = (float)itY->x;
	}

	Mat Pt;
	transpose(P,Pt);

	// insert K into L
	Rect range = Rect(0, 0, n, n);
	Mat Lr(L,range);
	K.copyTo(Lr);


	// insert P into L
	range = Rect(n, 0, dim + 1, n);
	Lr = Mat(L,range);
	P.copyTo(Lr);

	// insert Pt into L
	range = Rect(0,n,n,dim+1);
	Lr = Mat(L,range);
	Pt.copyTo(Lr);

	// fill V array
	std::vector<Point>::iterator it;
	int u = 0;

	for(it = iiP->begin(); it != iiP->end(); ++it)
	{
		V(0,u) = (float)it->y;
		V(1,u) = (float)it->x;
		u++;
	}

	// transpose V
	Mat Vt;
	transpose(V,Vt);

	cMatrix = Mat_<float>(n+dim+1,dim,0.0);

	// invert L
	Mat invL;
	invert(L,invL,DECOMP_LU);

	//multiply(invL,Vt,cMatrix);
	cMatrix = invL * Vt;

	//compensate for rounding errors
	for(int row = 0; row < cMatrix.rows; row++)
	{
		for(int col = 0; col < cMatrix.cols; col++)
		{
			double v = cMatrix(row,col);
			if(v > (-1.0e-006) && v < (1.0e-006) )
			{
				cMatrix(row,col) = 0.0;
			}
		}
	}
}
예제 #24
0
파일: spmbp.cpp 프로젝트: IanDaisy/spm-bp
void spm_bp::init_label_super(Mat_<Vec2f>& label_super_k, Mat_<float>& dCost_super_k) //, vector<vector<Vec2f> > &label_saved, vector<vector<Mat_<float> > > &dcost_saved)
{
    printf("==================================================\n");
    printf("Initiating particles...Done!\n");
    vector<Vec2f> label_vec;
    Mat_<float> localDataCost;
    for (int sp = 0; sp < numOfSP1; ++sp) {
        int id = repPixels1[sp];
        int y = subRange1[id][0];
        int x = subRange1[id][1];
        int h = subRange1[id][3] - subRange1[id][1] + 1;
        int w = subRange1[id][2] - subRange1[id][0] + 1;

        label_vec.clear();
        int k = 0;

        while (k < NUM_TOP_K) {
            float du = (float(rand()) / RAND_MAX - 0.5) * 2 * (float)disp_range_u;
            float dv = (float(rand()) / RAND_MAX - 0.5) * 2 * (float)disp_range_v;
            du = floor(du * upScale + 0.5) / upScale;
            dv = floor(dv * upScale + 0.5) / upScale;

            if (du >= -disp_range_u && du <= disp_range_u && dv >= -disp_range_v && dv <= disp_range_v) {
                int index_tp = 1;
                for (int k1 = 0; k1 < k; ++k1) {
                    if (checkEqual_PMF_PMBP(label_super_k[repPixels1[sp]][k1], Vec2f(du, dv)))
                        index_tp = 0;
                }

                if (index_tp == 1) {
                    for (int ii = 0; ii < superpixelsList1[sp].size(); ++ii)
                        label_super_k[superpixelsList1[sp][ii]][k] = Vec2f(du, dv);

                    label_vec.push_back(Vec2f(du, dv));
                    ++k;
                }
            }
        }
#if USE_CLMF0_TO_AGGREGATE_COST
        cv::Mat_<cv::Vec4b> leftCombinedCrossMap;
        leftCombinedCrossMap.create(h, w);
        subCrossMap1[sp].copyTo(leftCombinedCrossMap);
        CFFilter cff;
#endif
        int vec_size = label_vec.size();
        localDataCost.create(h, w * vec_size);
        localDataCost.setTo(0);
#pragma omp parallel for num_threads(NTHREADS)
        for (int i = 0; i < vec_size; ++i) {
            int kx = i * w;
            Mat_<float> rawCost;
            getLocalDataCostPerlabel(sp, label_vec[i], rawCost);
#if USE_CLMF0_TO_AGGREGATE_COST
            cff.FastCLMF0FloatFilterPointer(leftCombinedCrossMap, rawCost, rawCost);
#endif
            rawCost.copyTo(localDataCost(cv::Rect(kx, 0, w, h)));
        }

        //getLocalDataCost( sp, label_vec, localDataCost);

        int pt, px, py, kx;
        for (int ii = 0; ii < superpixelsList1[sp].size(); ++ii) {
            //cout<<ii<<endl;
            pt = superpixelsList1[sp][ii];
            px = pt / width1;
            py = pt % width1;
            for (int kk = 0; kk < NUM_TOP_K; kk++) {
                kx = kk * w;
                const Mat_<float>& local = localDataCost(cv::Rect(kx, 0, w, h));
                dCost_super_k[pt][kk] = local[px - x][py - y];
            }
        }
    }
    printf("==================================================\n");
}
예제 #25
0
void visualize_3d_points(const Mat_<cv::Vec3f>& points_3d, std::string name) {
  Mat points_3d_display;
  points_3d.convertTo(points_3d_display, CV_8UC3, 100);
  imshow(name, points_3d_display);
}
예제 #26
0
파일: spmbp.cpp 프로젝트: IanDaisy/spm-bp
void spm_bp::getLocalDataCost(int sp, vector<Vec2f>& flowList, Mat_<float>& localDataCost) //, vector<vector<Vec2f> > &label_saved, vector<vector<Mat_<float> > > &dcost_saved)
{
    int dSize = flowList.size();

    //USE_COLOR_FEATURES
    cv::Mat_<cv::Vec3f> subLt = subImage1[sp];
#if USE_CENSUS
    vector<vector<bitset<CENSUS_SIZE_OF> > > subLt_css = subCensusBS1[sp];
//cv::Mat_<Vec_css> subLt_css = subCensus1[sp];
//cv::Mat_<Vec_css_bit> subLt_css_bit = subCensus_bit1[sp];
#endif

    int upHeight, upWidth;
    upHeight = im1Up.rows;
    upWidth = im1Up.cols;

    // extract sub-image from subrange
    int p1 = repPixels1[sp];
    int w = subRange1[p1][2] - subRange1[p1][0] + 1;
    int h = subRange1[p1][3] - subRange1[p1][1] + 1;
    int x = subRange1[p1][0];
    int y = subRange1[p1][1];

#if USE_CLMF0_TO_AGGREGATE_COST
    cv::Mat_<cv::Vec4b> leftCombinedCrossMap;
    leftCombinedCrossMap.create(h, w);
    subCrossMap1[sp].copyTo(leftCombinedCrossMap);
    CFFilter cff;
#endif

    Mat_<float> rawCost(h, w);
    cv::Mat_<Vec3f> subRt(h, w);
    cv::Mat_<Vec2f> subRt_g(h, w);
#if USE_CENSUS
    vector<vector<bitset<CENSUS_SIZE_OF> > > subRt_css(h, vector<bitset<CENSUS_SIZE_OF> >(w));
#endif

    cv::Mat_<float> tmpCost(h, w);
    // localDataCost.release();
    localDataCost.create(h, w * dSize);
    int kx;
    for (int kd = 0; kd < dSize; ++kd) {
        kx = kd * w;
        clock_t start = clock();
        clock_t end;
//int check_id = isNewLabel_PMF_PMBP(label_saved[sp],flowList[kd]);

#if SAVE_DCOST
        if (check_id >= 0) {
            cout << "hit" << endl;
            //dcost_saved[sp][check_id].copyTo(localDataCost(cv::Rect(kx, 0, w, h)));
            //cout<<"hit"<<end-start<<endl;
            //continue;
        }
#endif
        start = clock();

        Vec2f fl = flowList[kd];

        Vec3f* subLtPtr = (cv::Vec3f*)(subLt.ptr(0));
        Vec3f* subRtPtr = (cv::Vec3f*)(subRt.ptr(0));
        float* rawCostPtr = (float*)(rawCost.ptr(0));

        int cy, cx, oy, ox;
        oy = y;
        for (cy = 0; cy < h; ++cy, ++oy) {
            ox = x;
            for (cx = 0; cx < w; ++cx, ++ox) {
                int oyUp, oxUp;
                oyUp = (oy + fl[0]) * upScale;
                oxUp = (ox + fl[1]) * upScale;

                if (oyUp < 0)
                    oyUp = 0;
                if (oyUp >= upHeight)
                    oyUp = upHeight - 1;
                if (oxUp < 0)
                    oxUp = 0;
                if (oxUp >= upWidth)
                    oxUp = upWidth - 1;
#if USE_POINTER_WISE
                *subRtPtr++ = im2Up[oyUp][oxUp];
#else
                subRt[cy][cx] = im2Up[oyUp][oxUp];
#endif

#if USE_CENSUS
                subRt_css[cy][cx] = censusBS2[oyUp][oxUp];
#endif
            }
        }
        // calculate raw cost
        subLtPtr = (cv::Vec3f*)(subLt.ptr(0));
        subRtPtr = (cv::Vec3f*)(subRt.ptr(0));

        rawCostPtr = (float*)(rawCost.ptr(0));

        int iy, ix;
        for (iy = 0; iy < h; ++iy) {
            for (ix = 0; ix < w; ++ix) {

#if DATA_COST_ADCENSUS
                bitset<CENSUS_SIZE_OF> tmpBS = subRt_css[iy][ix] ^ subLt_css[iy][ix];
#if USE_POINTER_WISE
                float dist_c = fabs((float)(*subLtPtr)[0] - (*subRtPtr)[0])
                    + fabs((float)(*subLtPtr)[1] - (*subRtPtr)[1])
                    + fabs((float)(*subLtPtr++)[2] - (*subRtPtr++)[2]);
#else
                float dist_c = std::abs(subLt[iy][ix][0] - subRt[iy][ix][0])
                    + std::abs(subLt[iy][ix][1] - subRt[iy][ix][1])
                    + std::abs(subLt[iy][ix][2] - subRt[iy][ix][2]);
#endif
                float dist_css = expCensusDiffTable[tmpBS.count()];
                //cout<<dist_c/3<<endl;
                float dist_ce = expColorDiffTable[int(dist_c / 3)];
//cout<<dist_css<<"and               "<<dist_c1<<endl;
//float dist_css = 1-std::exp( - float(HammingDis_bit(subLt_css_bit[iy][ix],subRt_css_bit[iy][ix]))/miu);
#if USE_POINTER_WISE
                *rawCostPtr++ = 255 * (dist_css + dist_ce);
#else
                rawCost[iy][ix] = 255 * (dist_css + dist_ce); //beta*min(dist_c/3,tau_c);//beta*min(dist_c/3,tau_c);//*255 + beta*min(dist_c/3,tau_c);
#endif

#endif
            }
        }

#if USE_CLMF0_TO_AGGREGATE_COST
        cff.FastCLMF0FloatFilterPointer(leftCombinedCrossMap, rawCost, tmpCost);
//cout<<"filtering"<<clock()-end<<endl;
#endif
#if SAVE_DCOST
        label_saved[sp].push_back(fl);
//dcost_saved[sp].push_back(tmpCost);
#endif
        tmpCost.copyTo(localDataCost(cv::Rect(kx, 0, w, h)));
        end = clock();
        //cout<<"not hit"<<end-start<<endl;
    }
    //cout<<label_saved[sp].size()<<endl;
}
예제 #27
0
int main(int argc, const char * argv[]) {

    //----------------------
    // Open an OpenNI device
    //----------------------

    //TODO: You'll want to open an RGB camera stream here too (the one with wich you wish to register the depth)

    cout << "Device opening ..." << endl;

    VideoCapture capture;
    capture.open( CAP_OPENNI );

    if( !capture.isOpened() )
    {
        cout << "Can not open a capture object." << endl;
        return -1;
    }

    // We don't want registration on, since we're going to do it ourselves.
    // Some devices with RGB cameras can perform registration on device
    bool modeRes=false;
    modeRes = capture.set( CAP_PROP_OPENNI_REGISTRATION, 0 );

    if (!modeRes) {
        cout << "Can't disable registration. That's crazy!\n" << endl;
        return -1;
    }

    // Display the current configuration
    cout << "\nDepth generator output mode:" << endl <<
        "FRAME_WIDTH      " << capture.get( CAP_PROP_FRAME_WIDTH ) << endl <<
        "FRAME_HEIGHT     " << capture.get( CAP_PROP_FRAME_HEIGHT ) << endl <<
        "FRAME_MAX_DEPTH  " << capture.get( CAP_PROP_OPENNI_FRAME_MAX_DEPTH ) << " mm" << endl <<
        "FPS              " << capture.get( CAP_PROP_FPS ) << endl <<
        "REGISTRATION     " << capture.get( CAP_PROP_OPENNI_REGISTRATION ) << endl;


    //---------------------------------------
    // Specify camera properties and geometry
    //--------------------------------------

    //TODO: Fill in the values for your setup.

    // Depth camera intrinsics
    Matx33f unregisteredCameraMatrix = Matx33f::eye();
    unregisteredCameraMatrix(0,0) = 570.0f;
    unregisteredCameraMatrix(1,1) = 570.0f;
    unregisteredCameraMatrix(0,2) = 320.0f-0.5f;
    unregisteredCameraMatrix(1,2) = 240.0f-0.5f;

    // NOTE: The depth distortion coefficients are currently not used by the Registration class.
    Vec<float, 5> unregisteredDistCoeffs(0,0,0,0,0);


    // RGB camera intrinsics
    Matx33f registeredCameraMatrix = Matx33f::eye();
    registeredCameraMatrix(0,0) = 570.0f;
    registeredCameraMatrix(1,1) = 570.0f;
    registeredCameraMatrix(0,2) = 320.0f-0.5f;
    registeredCameraMatrix(1,2) = 240.0f-0.5f;

    Vec<float, 5> registeredDistCoeffs(0,0,0,0,0);

    Size2i registeredImagePlaneSize = Size2i(640, 480);

    // The rigid body transformation between cameras.
    // Used as: uv_rgb = K_rgb * [R | t] * z * inv(K_ir) * uv_ir
    Matx44f registrationRbt = Matx44f::eye();
    registrationRbt(0,3) = .04;


    //------------------------------
    // Create our registration class
    //------------------------------
    oc::Registration registration(unregisteredCameraMatrix,
                                  unregisteredDistCoeffs,
                                  registeredCameraMatrix,
                                  registeredDistCoeffs,
                                  registrationRbt);

    for (;;) {

        Mat_<uint16_t> depthMap;

        if( !capture.grab() )
        {
            cout << "Can't grab depth." << endl;
            return -1;
        }
        else
        {
            if( capture.retrieve( depthMap, CAP_OPENNI_DEPTH_MAP ) )
            {

                // Actually perform the registration
                Mat_<uint16_t> registeredDepth;
                bool performDilation = false;
                registration.registerDepthToColor(depthMap,
                                                  registeredImagePlaneSize,
                                                  registeredDepth,
                                                  performDilation);


                //Display the unregistered and registered depth
                const float scaleFactor = 0.05f;
                {
                    Mat_<uint8_t> show;
                    depthMap.convertTo( show, CV_8UC1, scaleFactor );
                    imshow( "depth map", show );
                }
                {
                    Mat_<uint8_t> show;
                    registeredDepth.convertTo( show, CV_8UC1, scaleFactor );
                    imshow( "registered map", show );
                }

            }

        }

        if( waitKey( 1 ) >= 0 )
            break;
    }



    return 0;
}
예제 #28
0
파일: spmbp.cpp 프로젝트: IanDaisy/spm-bp
void spm_bp::getLocalDataCostPerlabel(int sp, const Vec2f& fl, Mat_<float>& localDataCost)
{
    //USE_COLOR_FEATURES
    cv::Mat_<cv::Vec3f> subLt = subImage1[sp];
#if USE_CENSUS
    vector<vector<bitset<CENSUS_SIZE_OF> > > subLt_css = subCensusBS1[sp];
#endif

    int upHeight, upWidth;
    upHeight = im1Up.rows;
    upWidth = im1Up.cols;

    // extract sub-image from subrange
    int p1 = repPixels1[sp];
    int w = subRange1[p1][2] - subRange1[p1][0] + 1;
    int h = subRange1[p1][3] - subRange1[p1][1] + 1;
    int x = subRange1[p1][0];
    int y = subRange1[p1][1];

    // localDataCost.release();
    localDataCost.create(h, w);
    //Mat_<float> rawCost(h, w);
    Mat_<Vec3f> subRt(h, w);
    vector<vector<bitset<CENSUS_SIZE_OF> > > subRt_css(h, vector<bitset<CENSUS_SIZE_OF> >(w));
#if SAVE_DCOST
    if (check_id >= 0) {
        cout << "hit" << endl;
    }
#endif

    Vec3f* subLtPtr = (cv::Vec3f*)(subLt.ptr(0));
    Vec3f* subRtPtr = (cv::Vec3f*)(subRt.ptr(0));
    float* rawCostPtr = (float*)(localDataCost.ptr(0));

    cv::Vec3f *im2UpPtr = (cv::Vec3f*) im2Up.ptr(0);
    int im2UpWidth = im2Up.cols;
    int maxHeight = upHeight - 1;
    int maxWidth = upWidth - 1;

    int cy, cx, oy, ox;
    float fl0 = fl[0], fl1 = fl[1];
#pragma omp parallel for
    for (cy = 0; cy < h; ++cy) {
        oy = y + cy;
        int oyUp = (oy + fl0) * upScale;
        if (oyUp < 0)
            oyUp = 0;
        if (oyUp >= upHeight)
            oyUp = maxHeight;

        for (cx = 0; cx < w; ++cx) {
            ox = x + cx;
            int oxUp = (ox + fl1) * upScale;
            if (oxUp < 0)
                oxUp = 0;
            if (oxUp >= upWidth)
                oxUp = maxWidth;

#if USE_POINTER_WISE
            // *subRtPtr++ = im2Up[oyUp][oxUp];
            subRtPtr[cy * w + cx] = im2UpPtr[oyUp * im2UpWidth + oxUp];
#else
            subRt[cy][cx] = im2Up[oyUp][oxUp];
#endif

#if USE_CENSUS
            subRt_css[cy][cx] = censusBS2[oyUp][oxUp];
#endif
        }

    }

    // calculate raw cost
    subLtPtr = (cv::Vec3f*)(subLt.ptr(0));
    subRtPtr = (cv::Vec3f*)(subRt.ptr(0));
    rawCostPtr = (float*)(localDataCost.ptr(0));

    int iy, ix;
    for (iy = 0; iy < h; ++iy) {
        for (ix = 0; ix < w; ++ix) {

#if DATA_COST_ADCENSUS
            bitset<CENSUS_SIZE_OF> tmpBS = subRt_css[iy][ix] ^ subLt_css[iy][ix];

#if USE_POINTER_WISE

            float dist_c = fabsf((*subLtPtr)[0] - (*subRtPtr)[0])
                         + fabsf((*subLtPtr)[1] - (*subRtPtr)[1])
                         + fabsf((*subLtPtr)[2] - (*subRtPtr)[2]);
            ++subLtPtr;
            ++subRtPtr;
#else
            float dist_c = std::abs(subLt[iy][ix][0] - subRt[iy][ix][0])
                + std::abs(subLt[iy][ix][1] - subRt[iy][ix][1])
                + std::abs(subLt[iy][ix][2] - subRt[iy][ix][2]);
#endif

            float dist_css = expCensusDiffTable[tmpBS.count()];
            float dist_ce = expColorDiffTable[int(dist_c / 3)];

#if USE_POINTER_WISE
            *rawCostPtr++ = 255 * (dist_css + dist_ce);
#else
            localDataCost[iy][ix] = 255 * (dist_css + dist_ce); //beta*min(dist_c/3,tau_c);//beta*min(dist_c/3,tau_c);//*255 + beta*min(dist_c/3,tau_c);
#endif

#endif
        }
    }

#if SAVE_DCOST
    label_saved[sp].push_back(fl);
#endif

}
예제 #29
0
int main(int argc, char** argv)
{	
    /*Polynomial2 poly2;
    poly2.kuu = -1; 
    poly2.kuv = 1; 
    poly2.kvv= -1; 
    poly2.ku = 0.25; 
    poly2.kv = 0.25; 
    poly2.k1 = 5;
    
    CurveRasterizer<Polynomial2> raster(1, 1, -100, 100, poly2);
    CurveRasterizer2<Polynomial2> raster2(1, 1, -100, 100, poly2);

    auto tr0 = clock();
    int x1 = 0;
    int x2 = 0;
    for (int i = 0; i < 10000000; i++)
    {
        raster.step();
        x1 += raster.x;
    }
    auto tr1 = clock();
    
    for (int i = 0; i < 10000000; i++)
    {
        raster2.step();
        x2 += raster2.x;
    }
    auto tr2 = clock();
    
    cout << "optimized " << double(tr1 - tr0) / CLOCKS_PER_SEC << endl;
    cout << "simple " << double(tr2 - tr1) / CLOCKS_PER_SEC << endl;
    cout << x1 << " " << x2 << endl;
    return 0;*/
    ifstream paramFile(argv[1]);
    if (not paramFile.is_open())
    {
        cout << argv[1] << " : ERROR, file is not found" << endl;
        return 0;
    }
    
    array<double, 6> params;
    
    cout << "EU Camera model parameters :" << endl;
    for (auto & p: params) 
    {
        paramFile >> p;
        cout << setw(15) << p;
    }
    cout << endl;
    paramFile.ignore();
    
    array<double, 6> cameraPose;
    cout << "Camera pose wrt the robot :" << endl;
    for (auto & e: cameraPose) 
    {
        paramFile >> e;
        cout << setw(15) << e;
    }
    cout << endl;
    paramFile.ignore();
    Transformation<double> TbaseCamera(cameraPose.data());
    
    array<double, 6> planePose;
    cout << "Plane pose :" << endl;
    for (auto & e: cameraPose) 
    {
        paramFile >> e;
        cout << setw(15) << e;
    }
    cout << endl;
    paramFile.ignore();
    Transformation<double> TbasePlane(cameraPose.data());
    
    StereoParameters stereoParams;
    paramFile >> stereoParams.u0;
    paramFile >> stereoParams.v0;
    paramFile >> stereoParams.disparityMax;
    paramFile >> stereoParams.blockSize;
    paramFile.ignore();
    
    string imageDir;
    getline(paramFile, imageDir);
    
    string imageInfo, imageName;
    array<double, 6> robotPose1, robotPose2;
    getline(paramFile, imageInfo);
    istringstream imageStream(imageInfo);
    imageStream >> imageName;
    for (auto & x : robotPose1) imageStream >> x;

    Mat8 img1 = imread(imageDir + imageName, 0);
    int counter = 2;
    while (getline(paramFile, imageInfo))
    {
        istringstream imageStream(imageInfo);
        
        imageStream >> imageName;
        for (auto & x : robotPose2) imageStream >> x;
    
        Transformation<double> T01(robotPose1.data()), T02(robotPose2.data());
        Transformation<double> TleftRight = T01.compose(TbaseCamera).inverseCompose(T02.compose(TbaseCamera));
        
        
        Mat8 img2 = imread(imageDir + imageName, 0);

        EnhancedStereo stereo(TleftRight, img1.cols, img1.rows, params.data(), params.data(), stereoParams);

        cv::Mat_<uint8_t> res;
        auto t2 = clock();
        stereo.comuteStereo(img1, img2, res);
        auto t3 = clock();
//        cout << double(t3 - t2) / CLOCKS_PER_SEC << endl;
        Mat_<float> distMat;
        Mat_<float> planeMat;
        
        stereo.computeDistance(distMat);
        Transformation<double> T0Camera = T01.compose(TbaseCamera);
        stereo.generatePlane(T0Camera.inverseCompose(TbasePlane), planeMat,
         vector<Vector3d>{Vector3d(-0.1, -0.1, 0), Vector3d(-0.1 + 3 * 0.45, -0.1, 0),
                          Vector3d(-0.1 + 3 * 0.45, 0.5, 0), Vector3d(-0.1, 0.5, 0) } );
        imshow("dist" + to_string(counter) , distMat);
        imshow("plane" , planeMat);
        imwrite("/home/bogdan/projects/plane.png", planeMat);
        double err = 0;
        double err2 = 0;
        double dist = 0;
        int N = 0;
        int Nmax = 0;
        Mat_<float> inlierMat(planeMat.size());
        inlierMat.setTo(0);
        for (int u = 0; u < distMat.cols; u++)
        {
            for (int v = 0; v < distMat.rows; v++)
            {
                
                if (planeMat(v, u) == 0) continue;
                Nmax++;
                dist += planeMat(v, u);
                inlierMat(v, u) = 1;
                if (distMat(v, u) == 0 or distMat(v, u) != distMat(v, u) or planeMat(v, u) != planeMat(v, u)) continue;
                if (abs(distMat(v, u) - planeMat(v, u)) > 0.10) continue;
                inlierMat(v, u) = 0;
                err += distMat(v, u) - planeMat(v, u);
                err2 += pow(distMat(v, u) - planeMat(v, u), 2);
                N++;
            }
        }
//        cout << (counter - 1) * 7 << " & " << dist/ Nmax * 1000 << " & " << err / N *1000 << " & " << sqrt(err2 / N)*1000  
//                << " & " << 100 * N / double(Nmax) << "\\\\" << endl << "\\hline" << endl;
        cout << "avg err : " << err / N *1000 << " avg err2 : " 
<< sqrt(err2 / N)*1000  << " number of inliers : " << 100 * N / double(Nmax) << endl;
        imshow("diff" + to_string(counter), abs(planeMat - distMat));
        imshow("inliers" + to_string(counter), inlierMat);
        counter++;
        
    }
    waitKey();
    return 0;
}
예제 #30
0
	// 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(1));
			source_points.push_back(tmp_source.row(2));
			source_points.push_back(tmp_source.row(3));
			source_points.push_back(tmp_source.row(4));
			source_points.push_back(tmp_source.row(12));
			source_points.push_back(tmp_source.row(13));
			source_points.push_back(tmp_source.row(14));
			source_points.push_back(tmp_source.row(15));
			source_points.push_back(tmp_source.row(27));
			source_points.push_back(tmp_source.row(28));
			source_points.push_back(tmp_source.row(29));
			source_points.push_back(tmp_source.row(31));
			source_points.push_back(tmp_source.row(32));
			source_points.push_back(tmp_source.row(33));
			source_points.push_back(tmp_source.row(34));
			source_points.push_back(tmp_source.row(35));
			source_points.push_back(tmp_source.row(36));
			source_points.push_back(tmp_source.row(39));
			source_points.push_back(tmp_source.row(40));
			source_points.push_back(tmp_source.row(41));
			source_points.push_back(tmp_source.row(42));
			source_points.push_back(tmp_source.row(45));
			source_points.push_back(tmp_source.row(46));
			source_points.push_back(tmp_source.row(47));

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

			// Push back the rigid points
			destination_points.push_back(tmp_dest.row(1));
			destination_points.push_back(tmp_dest.row(2));
			destination_points.push_back(tmp_dest.row(3));
			destination_points.push_back(tmp_dest.row(4));
			destination_points.push_back(tmp_dest.row(12));
			destination_points.push_back(tmp_dest.row(13));
			destination_points.push_back(tmp_dest.row(14));
			destination_points.push_back(tmp_dest.row(15));
			destination_points.push_back(tmp_dest.row(27));
			destination_points.push_back(tmp_dest.row(28));
			destination_points.push_back(tmp_dest.row(29));
			destination_points.push_back(tmp_dest.row(31));
			destination_points.push_back(tmp_dest.row(32));
			destination_points.push_back(tmp_dest.row(33));
			destination_points.push_back(tmp_dest.row(34));
			destination_points.push_back(tmp_dest.row(35));
			destination_points.push_back(tmp_dest.row(36));
			destination_points.push_back(tmp_dest.row(39));
			destination_points.push_back(tmp_dest.row(40));
			destination_points.push_back(tmp_dest.row(41));
			destination_points.push_back(tmp_dest.row(42));
			destination_points.push_back(tmp_dest.row(45));
			destination_points.push_back(tmp_dest.row(46));
			destination_points.push_back(tmp_dest.row(47));
		}
	}