void arglCameraFrustumRH(const ARParam *cparam, const ARdouble focalmin, const ARdouble focalmax, ARdouble m_projection[16]) { ARdouble icpara[3][4]; ARdouble trans[3][4]; ARdouble p[3][3], q[4][4]; int width, height; int i, j; width = cparam->xsize; height = cparam->ysize; if (arParamDecompMat(cparam->mat, icpara, trans) < 0) { ARLOGe("arglCameraFrustum(): arParamDecompMat() indicated parameter error.\n"); return; } for (i = 0; i < 4; i++) { icpara[1][i] = (height - 1)*(icpara[2][i]) - icpara[1][i]; } for(i = 0; i < 3; i++) { for(j = 0; j < 3; j++) { p[i][j] = icpara[i][j] / icpara[2][2]; } } q[0][0] = (2.0 * p[0][0] / (width - 1)); q[0][1] = (2.0 * p[0][1] / (width - 1)); q[0][2] = -((2.0 * p[0][2] / (width - 1)) - 1.0); q[0][3] = 0.0; q[1][0] = 0.0; q[1][1] = -(2.0 * p[1][1] / (height - 1)); q[1][2] = -((2.0 * p[1][2] / (height - 1)) - 1.0); q[1][3] = 0.0; q[2][0] = 0.0; q[2][1] = 0.0; q[2][2] = (focalmax + focalmin)/(focalmin - focalmax); q[2][3] = 2.0 * focalmax * focalmin / (focalmin - focalmax); q[3][0] = 0.0; q[3][1] = 0.0; q[3][2] = -1.0; q[3][3] = 0.0; for (i = 0; i < 4; i++) { // Row. // First 3 columns of the current row. for (j = 0; j < 3; j++) { // Column. m_projection[i + j*4] = q[i][0] * trans[0][j] + q[i][1] * trans[1][j] + q[i][2] * trans[2][j]; } // Fourth column of the current row. m_projection[i + 3*4] = q[i][0] * trans[0][3] + q[i][1] * trans[1][3] + q[i][2] * trans[2][3] + q[i][3]; } }
// // Convert a camera parameter structure into an OpenGL projection matrix. // void arglCameraFrustum(const ARParam *cparam, const double focalmin, const double focalmax, GLdouble m_projection[16]) { double icpara[3][4]; double trans[3][4]; double p[3][3], q[4][4]; int width, height; int i, j; width = cparam->xsize; height = cparam->ysize; if (arParamDecompMat(cparam->mat, icpara, trans) < 0) { printf("arglCameraFrustum(): arParamDecompMat() indicated parameter error.\n"); // Windows bug: when running multi-threaded, can't write to stderr! return; } for (i = 0; i < 4; i++) { icpara[1][i] = (height - 1)*(icpara[2][i]) - icpara[1][i]; } for(i = 0; i < 3; i++) { for(j = 0; j < 3; j++) { p[i][j] = icpara[i][j] / icpara[2][2]; } } q[0][0] = (2.0 * p[0][0] / (width - 1)); q[0][1] = (2.0 * p[0][1] / (width - 1)); q[0][2] = ((2.0 * p[0][2] / (width - 1)) - 1.0); q[0][3] = 0.0; q[1][0] = 0.0; q[1][1] = (2.0 * p[1][1] / (height - 1)); q[1][2] = ((2.0 * p[1][2] / (height - 1)) - 1.0); q[1][3] = 0.0; q[2][0] = 0.0; q[2][1] = 0.0; q[2][2] = (focalmax + focalmin)/(focalmax - focalmin); q[2][3] = -2.0 * focalmax * focalmin / (focalmax - focalmin); q[3][0] = 0.0; q[3][1] = 0.0; q[3][2] = 1.0; q[3][3] = 0.0; for (i = 0; i < 4; i++) { // Row. // First 3 columns of the current row. for (j = 0; j < 3; j++) { // Column. m_projection[i + j*4] = q[i][0] * trans[0][j] + q[i][1] * trans[1][j] + q[i][2] * trans[2][j]; } // Fourth column of the current row. m_projection[i + 3*4] = q[i][0] * trans[0][3] + q[i][1] * trans[1][3] + q[i][2] * trans[2][3] + q[i][3]; } }
int arParamDecomp( const ARParam *source, ARParam *icpara, ARdouble trans[3][4] ) { int i; if (source->dist_function_version < 1 || source->dist_function_version > AR_DIST_FUNCTION_VERSION_MAX) return (-1); icpara->dist_function_version = source->dist_function_version; icpara->xsize = source->xsize; icpara->ysize = source->ysize; for (i = 0; i < arParamVersionInfo[source->dist_function_version - 1].dist_factor_num; i++) icpara->dist_factor[i] = source->dist_factor[i]; return arParamDecompMat( source->mat, icpara->mat, trans ); }
static void argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, float m[16] ) { double icpara[3][4]; double trans[3][4]; double p[3][3], q[4][4]; int i, j; if( arParamDecompMat(cparam, icpara, trans) < 0 ) { printf("gConvGLcpara: Parameter error!!\n"); exit(0); } for( i = 0; i < 3; i++ ) { for( j = 0; j < 3; j++ ) { p[i][j] = icpara[i][j] / icpara[2][2]; } } q[0][0] = (2.0 * p[0][0] / width); q[0][1] = (2.0 * p[0][1] / width); q[0][2] = ((2.0 * p[0][2] / width) - 1.0); q[0][3] = 0.0; q[1][0] = 0.0; q[1][1] = (2.0 * p[1][1] / height); q[1][2] = ((2.0 * p[1][2] / height) - 1.0); q[1][3] = 0.0; q[2][0] = 0.0; q[2][1] = 0.0; q[2][2] = (gfar + gnear)/(gfar - gnear); q[2][3] = -2.0 * gfar * gnear / (gfar - gnear); q[3][0] = 0.0; q[3][1] = 0.0; q[3][2] = 1.0; q[3][3] = 0.0; for( i = 0; i < 4; i++ ) { for( j = 0; j < 3; j++ ) { m[i+j*4] = q[i][0] * trans[0][j] + q[i][1] * trans[1][j] + q[i][2] * trans[2][j]; } m[i+3*4] = q[i][0] * trans[0][3] + q[i][1] * trans[1][3] + q[i][2] * trans[2][3] + q[i][3]; } }
void CProxyCamera::GetGLMatrices(double glprojmatrix[], double glmodelviewmatrix[], const int glvpmatrix[], double znear, double zfar) const { double mat[3][4]; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { mat[i][j] = m_KR(i,j); } } mat[0][3] = m_KT[0]; mat[1][3] = m_KT[1]; mat[2][3] = m_KT[2]; Matrix3d LHC = Matrix3d::Zero(); LHC(0, 0) = LHC(1, 1) = LHC(2, 2) = -1; LHC(0, 0) = 1; double icpara[3][4], trans[3][4]; if (arParamDecompMat(mat, icpara, trans) < 0) { printf("Fatal error: proj decompose failed!\n"); exit(0); } Matrix3d R; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { R(i, j) = trans[i][j]; } } Matrix3d LHCR = LHC * R; Matrix4d modelViewMatrix = Matrix4d::Identity(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { modelViewMatrix(i, j) = LHCR(i,j); } } modelViewMatrix(0, 3) = trans[0][3]; modelViewMatrix(1, 3) = trans[1][3]; modelViewMatrix(2, 3) = trans[2][3]; modelViewMatrix(1, 3) = modelViewMatrix(1, 3) * (-1); modelViewMatrix(2, 3) = modelViewMatrix(2, 3) * (-1); modelViewMatrix(3, 3) = 1.0; Matrix4d finalModelM=modelViewMatrix; for (int i=0;i<16;i++) { glmodelviewmatrix[i]=finalModelM(i); } double w = glvpmatrix[2]; double h = glvpmatrix[3]; Matrix4d H_inv = Matrix4d::Identity(); H_inv(0, 0) = 2.0 / w; H_inv(0, 2) = -1; H_inv(1, 1) = -2.0 / h; H_inv(1, 2) = 1.0; H_inv(3, 2) = 1.0; Matrix3d K = Matrix3d::Zero(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { K(i, j) = icpara[i][j] / icpara[2][2]; } } Matrix3d y = K * LHC; Matrix4d y_ = Matrix4d::Zero(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { y_(i, j) = y(i,j); } } Matrix4d result = H_inv * (y_); double C_ = -(zfar + znear) / (zfar - znear); double D_ = -(2 * zfar * znear) / (zfar - znear); result(2, 2) = C_; result(2, 3) = D_; Matrix4d finalRes=result; for (int i=0;i<16;i++) { glprojmatrix[i]=finalRes(i); } }
/*! * */ void CameraParameters::argConvGLcpara2(double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert)throw(cv::Exception) { double icpara[3][4]; double trans[3][4]; double p[3][3], q[4][4]; int i, j; cparam[0][2] *= -1.0; cparam[1][2] *= -1.0; cparam[2][2] *= -1.0; if ( arParamDecompMat(cparam, icpara, trans) < 0 ) throw cv::Exception(9002,"parameter error","MarkerDetector::argConvGLcpara2",__FILE__, __LINE__); for ( i = 0; i < 3; i++ ) { for ( j = 0; j < 3; j++ ) { p[i][j] = icpara[i][j] / icpara[2][2]; } } q[0][0] = (2.0 * p[0][0] / width); q[0][1] = (2.0 * p[0][1] / width); q[0][2] = ((2.0 * p[0][2] / width) - 1.0); q[0][3] = 0.0; q[1][0] = 0.0; q[1][1] = (2.0 * p[1][1] / height); q[1][2] = ((2.0 * p[1][2] / height) - 1.0); q[1][3] = 0.0; q[2][0] = 0.0; q[2][1] = 0.0; q[2][2] = (gfar + gnear)/(gfar - gnear); q[2][3] = -2.0 * gfar * gnear / (gfar - gnear); q[3][0] = 0.0; q[3][1] = 0.0; q[3][2] = 1.0; q[3][3] = 0.0; for ( i = 0; i < 4; i++ ) { for ( j = 0; j < 3; j++ ) { m[i+j*4] = q[i][0] * trans[0][j] + q[i][1] * trans[1][j] + q[i][2] * trans[2][j]; } m[i+3*4] = q[i][0] * trans[0][3] + q[i][1] * trans[1][3] + q[i][2] * trans[2][3] + q[i][3]; } if (!invert) { m[13]=-m[13] ; m[1]=-m[1]; m[5]=-m[5]; m[9]=-m[9]; } }