Example #1
0
void Camera::ProjectPoints(const CvMat* object_points, double gl[16], CvMat* image_points) const
{
	double glm[4][4] = {
		gl[0], gl[4], gl[8],  gl[12],
		gl[1], gl[5], gl[9],  gl[13],
		gl[2], gl[6], gl[10], gl[14],
		gl[3], gl[7], gl[11], gl[15],
	};
	CvMat glm_mat = cvMat(4, 4, CV_64F, glm);
	
	// For some reason we need to mirror both y and z ???
	double cv_mul_data[4][4];
	CvMat cv_mul = cvMat(4, 4, CV_64F, cv_mul_data);
	cvSetIdentity(&cv_mul);
	cvmSet(&cv_mul, 1, 1, -1);
	cvmSet(&cv_mul, 2, 2, -1);
	cvMatMul(&cv_mul, &glm_mat, &glm_mat);
	
	// Rotation
	Rotation r;
	r.SetMatrix(&glm_mat);
	double rod[3]; 
	CvMat rod_mat=cvMat(3, 1, CV_64F, rod);
	r.GetRodriques(&rod_mat);
	// Translation
	double tra[3] = { glm[0][3], glm[1][3], glm[2][3] };
	CvMat tra_mat = cvMat(3, 1, CV_64F, tra);
	// Project points
	ProjectPoints(object_points, &rod_mat, &tra_mat, image_points);
}