C++ Mat_::col方法代码示例

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);
}

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;
}

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);
	

}

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;
//.........这里部分代码省略.........

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;
}