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