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