float CalculateError(const Mat_<float>& ground_truth_shape, const Mat_<float>& predicted_shape) {
	Mat_<float> temp;
	//temp = ground_truth_shape.rowRange(36, 37)-ground_truth_shape.rowRange(45, 46);
	temp = ground_truth_shape.rowRange(1, 2) - ground_truth_shape.rowRange(6, 7);
	float x = mean(temp.col(0))[0];
	float y = mean(temp.col(1))[1];
	float interocular_distance = sqrt(x*x + y*y);
	float sum = 0;
	for (int i = 0; i < ground_truth_shape.rows; i++) {
		sum += norm(ground_truth_shape.row(i) - predicted_shape.row(i));
	}
	return sum / (ground_truth_shape.rows*interocular_distance);
}
예제 #2
0
Mat_<double> estimatePose(Mat_<double> const imagePts)
{
    // Note that given R, t, the camera location in world
    // coordinates is not just t, but instead -inv(R)*t.

    Mat_<double> const worldPts = getWorldPts();
    Mat_<double> const rotTransl = estimateRotTransl(worldPts, imagePts);
    Mat_<double> rotMatrix(3, 3);
    Mat_<double> translation(3, 1);
    rotTransl.col(0).copyTo(rotMatrix.col(0));
    rotTransl.col(1).copyTo(rotMatrix.col(1));
    rotTransl.col(2).copyTo(rotMatrix.col(2));
    rotTransl.col(3).copyTo(translation);
    Mat_<double> cameraLoc = -rotMatrix.t() * translation;
    Mat_<double> simplePose(4, 1);
    simplePose(0) = cameraLoc(0);  // x
    simplePose(1) = cameraLoc(1);  // y
    simplePose(2) = cameraLoc(2);  // z
    // Yaw (rotation around z axis):
    // See http://planning.cs.uiuc.edu/node103.html
    simplePose(3) = atan2(rotMatrix(1, 0), rotMatrix(0, 0));
    // cout << "simplePose: " << simplePose << endl;
    return simplePose;
}
예제 #3
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);
}
// Need to move this all to opengl
void DrawBox(Mat image, Vec6d pose, Scalar color, int thickness, float fx, float fy, float cx, float cy)
{
	float boxVerts[] = {-1, 1, -1,
						1, 1, -1,
						1, 1, 1,
						-1, 1, 1,
						1, -1, 1,
						1, -1, -1,
						-1, -1, -1,
						-1, -1, 1};
	Mat_<float> box = Mat(8, 3, CV_32F, boxVerts).clone() * 100;


	Matx33f rot = Euler2RotMat(Vec3d(pose[3], pose[4], pose[5]));
	Mat_<float> rotBox;
	
	Mat((Mat(rot) * box.t())).copyTo(rotBox);
	rotBox = rotBox.t();

	rotBox.col(0) = rotBox.col(0) + pose[0];
	rotBox.col(1) = rotBox.col(1) + pose[1];
	rotBox.col(2) = rotBox.col(2) + pose[2];

	// draw the lines
	Mat_<float> rotBoxProj;
	Project(rotBoxProj, rotBox, image.size(), fx, fy, cx, cy);
	
	Mat begin;
	Mat end;

	rotBoxProj.row(0).copyTo(begin);
	rotBoxProj.row(1).copyTo(end);
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
		
	rotBoxProj.row(1).copyTo(begin);
	rotBoxProj.row(2).copyTo(end);
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
	
	rotBoxProj.row(2).copyTo(begin);
	rotBoxProj.row(3).copyTo(end);
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
	
	rotBoxProj.row(0).copyTo(begin);
	rotBoxProj.row(3).copyTo(end);
	//std::cout << begin <<endl;
	//std::cout << end <<endl;
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
	
	rotBoxProj.row(2).copyTo(begin);
	rotBoxProj.row(4).copyTo(end);
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
	
	rotBoxProj.row(1).copyTo(begin);
	rotBoxProj.row(5).copyTo(end);
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
	
	rotBoxProj.row(0).copyTo(begin);
	rotBoxProj.row(6).copyTo(end);
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
	
	rotBoxProj.row(3).copyTo(begin);
	rotBoxProj.row(7).copyTo(end);
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
	
	rotBoxProj.row(6).copyTo(begin);
	rotBoxProj.row(5).copyTo(end);
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
	
	rotBoxProj.row(5).copyTo(begin);
	rotBoxProj.row(4).copyTo(end);
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
		
	rotBoxProj.row(4).copyTo(begin);
	rotBoxProj.row(7).copyTo(end);
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
	
	rotBoxProj.row(7).copyTo(begin);
	rotBoxProj.row(6).copyTo(end);
	cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
	

}
예제 #5
0
void Tree::Splitnode(const vector<Mat_<uchar> >& images,
					 const vector<Mat_<double> >& ground_truth_shapes,
					 const vector<Mat_<double> >& current_shapes,
					 const vector<BoundingBox> & bounding_box,
					 const Mat_<double>& mean_shape,
					 const Mat_<double>& shapes_residual,
					 const vector<int> &ind_samples,
					 // output
					 double& thresh,
					 double* feat,
					 bool& isvaild,
					 vector<int>& lcind,
					 vector<int>& rcind
					 ){
	if (ind_samples.size() == 0){
		thresh = 0;
		feat = new double[4];
		feat[0] = 0;
		feat[1] = 0;
		feat[2] = 0;
		feat[3] = 0;
		lcind.clear();
		rcind.clear();
		isvaild = 1;
		return;
	}
	// get candidate pixel locations
	static int randomseedforfeature = 0;
	RNG random_generator(getTickCount());
	Mat_<double> candidate_pixel_locations(max_numfeats_,4);
	for(unsigned int i = 0;i < max_numfeats_;i++){
		double x1 = random_generator.uniform(-1.0,1.0);
		double y1 = random_generator.uniform(-1.0,1.0);
		double x2 = random_generator.uniform(-1.0,1.0);
		double y2 = random_generator.uniform(-1.0,1.0);

		//伪随机数
		//double x1 = -1.0 + randomseedforfeature * 17%57 * (2.0/57);
		//randomseedforfeature++;
		//double y1 = -1.0 + randomseedforfeature * 17%57 * (2.0/57);
		//randomseedforfeature++;
		//double x2 = -1.0 + randomseedforfeature * 17%57 * (2.0/57);
		//randomseedforfeature++;
		//double y2 = -1.0 + randomseedforfeature * 17%57 * (2.0/57);
		//randomseedforfeature++;

		if((x1*x1 + y1*y1 > 1.0)||(x2*x2 + y2*y2 > 1.0)){
			i--;
			continue;
		}
	   // cout << x1 << " "<<y1 <<" "<< x2<<" "<< y2<<endl;
		candidate_pixel_locations(i,0) = x1 * max_radio_radius_;
		candidate_pixel_locations(i,1) = y1 * max_radio_radius_;
		candidate_pixel_locations(i,2) = x2 * max_radio_radius_;
		candidate_pixel_locations(i,3) = y2 * max_radio_radius_;
	}
	// get pixel difference feature
	Mat_<int> densities(max_numfeats_,(int)ind_samples.size());
	for (int i = 0;i < ind_samples.size();i++){
		Mat_<double> rotation;
		double scaleX, scaleY;
		Mat_<double> temp = ProjectShape(current_shapes[ind_samples[i]],bounding_box[ind_samples[i]]);
		SimilarityTransform(temp,mean_shape,rotation,scaleX, scaleY);
		// whether transpose or not ????
		for(int j = 0;j < max_numfeats_;j++){
		//some problems??? Ming
		/*fixed bug, by Ming, 2015.08.20*/
		//原来代码似乎没错.. 先改回去 2015.12.09, Ming
		double project_x1 = rotation(0,0) * candidate_pixel_locations(j,0) + rotation(0,1) * candidate_pixel_locations(j,1);
		double project_y1 = rotation(1,0) * candidate_pixel_locations(j,0) + rotation(1,1) * candidate_pixel_locations(j,1);
		
			//double project_x1 = rotation(0,0) * candidate_pixel_locations(j,0) + rotation(1,0) * candidate_pixel_locations(j,1);
			//double project_y1 = rotation(0,1) * candidate_pixel_locations(j,0) + rotation(1,1) * candidate_pixel_locations(j,1);
			project_x1 = scaleX * project_x1 * bounding_box[ind_samples[i]].width / 2.0;
			project_y1 = scaleY * project_y1 * bounding_box[ind_samples[i]].height / 2.0;
			int real_x1 = project_x1 + current_shapes[ind_samples[i]](landmarkID_,0);
			int real_y1 = project_y1 + current_shapes[ind_samples[i]](landmarkID_,1);
			real_x1 = max(0.0,min((double)real_x1,images[ind_samples[i]].cols-1.0));
			real_y1 = max(0.0,min((double)real_y1,images[ind_samples[i]].rows-1.0));
			/*fixed bug, by Ming, 2015.08.20*/
			//原来代码似乎没错.. 先改回去 2015.12.09, Ming
double project_x2 = rotation(0,0) * candidate_pixel_locations(j,2) + rotation(0,1) * candidate_pixel_locations(j,3);
			double project_y2 = rotation(1,0) * candidate_pixel_locations(j,2) + rotation(1,1) * candidate_pixel_locations(j,3);

			//double project_x2 = rotation(0,0) * candidate_pixel_locations(j,2) + rotation(1,0) * candidate_pixel_locations(j,3);
			//double project_y2 = rotation(0,1) * candidate_pixel_locations(j,2) + rotation(1,1) * candidate_pixel_locations(j,3);
			project_x2 = scaleX * project_x2 * bounding_box[ind_samples[i]].width / 2.0;
			project_y2 = scaleY * project_y2 * bounding_box[ind_samples[i]].height / 2.0;
			int real_x2 = project_x2 + current_shapes[ind_samples[i]](landmarkID_,0);
			int real_y2 = project_y2 + current_shapes[ind_samples[i]](landmarkID_,1);
			real_x2 = max(0.0,min((double)real_x2,images[ind_samples[i]].cols-1.0));
			real_y2 = max(0.0,min((double)real_y2,images[ind_samples[i]].rows-1.0));
			
			densities(j,i) = ((int)(images[ind_samples[i]](real_y1,real_x1))-(int)(images[ind_samples[i]](real_y2,real_x2)));
		}
	}
	// pick the feature
	Mat_<int> densities_sorted = densities.clone();
	cv::sort(densities, densities_sorted, CV_SORT_ASCENDING);
	vector<double> lc1,lc2;
	vector<double> rc1,rc2;
	lc1.reserve(ind_samples.size());
	rc1.reserve(ind_samples.size());
	lc2.reserve(ind_samples.size());
	rc2.reserve(ind_samples.size());
//    double E_x_2 = mean(shapes_residual.col(0).mul(shapes_residual.col(0)))[0];
//    double E_x = mean(shapes_residual.col(0))[0];
//    double E_y_2 = mean(shapes_residual.col(1).mul(shapes_residual.col(1)))[0];
//    double E_y = mean(shapes_residual.col(1))[0];
//    double var_overall = ind_samples.size()*((E_x_2 - E_x*E_x) + (E_y_2 - E_y*E_y));
	double var_overall =(calculate_var(shapes_residual.col(0))+calculate_var(shapes_residual.col(1))) * ind_samples.size();
	double max_var_reductions = 0;
	double threshold = 0;
	double var_lc = 0;
	double var_rc = 0;
	double var_reduce = 0;
	double max_id = 0;
	int thresholdTempNum = 10;
	for (int i = 0; i <max_numfeats_;i++){
		double bestReduce = -999999;
		double bestTempThresd = 0;
		for(int k=0; k<thresholdTempNum; ++k)
		{
			lc1.clear();
			lc2.clear();
			rc1.clear();
			rc2.clear();
			int ind =(int)(ind_samples.size() * random_generator.uniform(0.05,0.95));
			//int ind =(int)(ind_samples.size() * 0.05 + k*0.9/thresholdTempNum);

			//printf("%d th numfeats  |  %d th img sample \n",i,ind);

			threshold = densities_sorted(i,ind);
			for (int j=0;j < ind_samples.size();j++){
				if (densities(i,j) < threshold){
					lc1.push_back(shapes_residual(j,0));
					lc2.push_back(shapes_residual(j,1));
				}
				else{
					rc1.push_back(shapes_residual(j,0));
					rc2.push_back(shapes_residual(j,1));            }
			}
			var_lc = (calculate_var(lc1)+calculate_var(lc2)) * lc1.size();
			var_rc = (calculate_var(rc1)+calculate_var(rc2)) * rc1.size();
			var_reduce = var_overall - var_lc - var_rc;

			if(var_reduce > bestReduce)
			{
				bestReduce = var_reduce;
				bestTempThresd = threshold;
			}
		}
		//printf("var_reduce : %lf\n",var_reduce);

//       cout << var_reduce<<endl;
		if (bestReduce > max_var_reductions){
			max_var_reductions = bestReduce;
			thresh = bestTempThresd;
			max_id = i;
		}
	}

	//printf("max_var_reductions : %lf\n",max_var_reductions);

	isvaild = 1;
	//feat[0] =candidate_pixel_locations(max_id,0)/max_radio_radius_;
	//feat[1] =candidate_pixel_locations(max_id,1)/max_radio_radius_;
	//feat[2] =candidate_pixel_locations(max_id,2)/max_radio_radius_;
	//feat[3] =candidate_pixel_locations(max_id,3)/max_radio_radius_;

	feat[0] =candidate_pixel_locations(max_id,0);
	feat[1] =candidate_pixel_locations(max_id,1);
	feat[2] =candidate_pixel_locations(max_id,2);
	feat[3] =candidate_pixel_locations(max_id,3);
//    cout << max_id<< " "<<max_var_reductions <<endl;
//    cout << feat[0] << " "<<feat[1] <<" "<< feat[2]<<" "<< feat[3]<<endl;
	lcind.clear();
	rcind.clear();
	for (int j=0;j < ind_samples.size();j++){
		if (densities(max_id,j) < thresh){
			lcind.push_back(ind_samples[j]);
		}
		else{
			rcind.push_back(ind_samples[j]);
		}
	}
}
예제 #6
0
Mat_<double> estimateRotTransl(
    Mat_<double> const worldPts,
    Mat_<double> const imagePts)
{
    assert(imagePts.cols == 2);
    assert(worldPts.cols == 3);
    assert(imagePts.rows == worldPts.rows);
    // TODO verify all worldPts have z=0

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

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

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

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

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

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

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

    Mat_<double> rotTransl(3, 4);
    rot.col(0).copyTo(rotTransl.col(0));
    rot.col(1).copyTo(rotTransl.col(1));
    rot.col(2).copyTo(rotTransl.col(2));
    transl.copyTo(rotTransl.col(3));
    return rotTransl;
}