private bool solveHomography4PointsInhomogenous(NyARDoubleMatrix33 i_homography_mat,
NyARDoublePoint2d x1, NyARDoublePoint2d x2, NyARDoublePoint2d x3, NyARDoublePoint2d x4,
NyARDoublePoint2d xp1, NyARDoublePoint2d xp2, NyARDoublePoint2d xp3, NyARDoublePoint2d xp4)
{
double[][] _mat_A = ArrayUtils.newDouble2dArray(8, 9);
// x1.setValue(0, 0);x2.setValue(10, 0);x3.setValue(10, 10);x4.setValue(0, 10);
// xp1.setValue(10, 10);xp2.setValue(10, 0);xp3.setValue(0, 0);xp4.setValue(0, 10);
//Homography4PointsInhomogeneousConstraint
AddHomographyPointContraint(_mat_A, 0, x1, xp1);
AddHomographyPointContraint(_mat_A, 2, x2, xp2);
AddHomographyPointContraint(_mat_A, 4, x3, xp3);
AddHomographyPointContraint(_mat_A, 6, x4, xp4);
//SolveHomography4PointsInhomogenous
if (!this.solveNullVector8x9Destructive(i_homography_mat, _mat_A))
{
return(false);
}
if (Math.Abs(i_homography_mat.determinant()) < 1e-5)
{
return(false);
}
return(true);
}
/**
* コンストラクタです。
* 行列のサイズを指定して、NyARMatオブジェクトを作成します。
* @param i_row
* 行数です。
* @param i_clm
* 列数です。
*/
public NyARMat(int i_row, int i_clm)
{
this._m = ArrayUtils.newDouble2dArray(i_row, i_clm);
this.clm = i_clm;
this.row = i_row;
this.__matrixSelfInv_nos = new int[i_row];
return;
}
/**
* i_row x i_clmサイズの行列を格納できるように行列サイズを変更します。 実行後、行列の各値は不定になります。
*
* @param i_row
* @param i_clm
*/
public void realloc(int i_row, int i_clm)
{
if (i_row <= this.m.Length && i_clm <= this.m[0].Length)
{
// 十分な配列があれば何もしない。
}
else
{
// 不十分なら取り直す。
m = ArrayUtils.newDouble2dArray(i_row, i_clm);
}
this.clm = i_clm;
this.row = i_row;
}
/**
* 行列のサイズを変更します。
* 関数を実行すると、行列の各値は不定になります。
* @param i_row
* 新しい行数
* @param i_clm
* 新しい列数。
*/
public void realloc(int i_row, int i_clm)
{
if (i_row <= this._m.Length && i_clm <= this._m[0].Length)
{
// 十分な配列があれば何もしない。
}
else
{
// 不十分なら取り直す。
this._m = ArrayUtils.newDouble2dArray(i_row, i_clm);
this.__matrixSelfInv_nos = new int[i_row];
}
this.clm = i_clm;
this.row = i_row;
return;
}
/**
* Solve for the null vector x of an 8x9 matrix A such A*x=0. The matrix A is destroyed in the process. This system
* is solved using QR decomposition with Gram-Schmidt.
*/
private bool solveNullVector8x9Destructive(NyARDoubleMatrix33 x, double[][] A)
{
double[][] Q = ArrayUtils.newDouble2dArray(8, 9);
if (!OrthogonalizePivot8x9Basis0(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis1(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis2(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis3(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis4(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis5(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis6(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis7(Q, A))
{
return(false);
}
return(OrthogonalizeIdentity8x9(x, Q));
}
/**
* 右手系の視錐台を作ります。
* この視錐台は、ARToolKitのarglCameraViewRHの作る視錐台と同じです。
* @param i_screen_width
* スクリーンサイズを指定します。
* @param i_screen_height
* スクリーンサイズを指定します。
* @param i_dist_min
* near pointを指定します(mm単位)
* @param i_dist_max
* far pointを指定します(mm単位)
* @param o_frustum
* 視錐台の格納先オブジェクトを指定します。
*/
public void makeCameraFrustumRH(double i_screen_width, double i_screen_height, double i_dist_min, double i_dist_max, NyARDoubleMatrix44 o_frustum)
{
NyARMat trans_mat = new NyARMat(3, 4);
NyARMat icpara_mat = new NyARMat(3, 4);
double[][] p = ArrayUtils.newDouble2dArray(3, 3);
int i;
this.decompMat(icpara_mat, trans_mat);
double[][] icpara = icpara_mat.getArray();
double[][] trans = trans_mat.getArray();
for (i = 0; i < 4; i++)
{
icpara[1][i] = (i_screen_height - 1) * (icpara[2][i]) - icpara[1][i];
}
p[0][0] = icpara[0][0] / icpara[2][2];
p[0][1] = icpara[0][1] / icpara[2][2];
p[0][2] = icpara[0][2] / icpara[2][2];
p[1][0] = icpara[1][0] / icpara[2][2];
p[1][1] = icpara[1][1] / icpara[2][2];
p[1][2] = icpara[1][2] / icpara[2][2];
p[2][0] = icpara[2][0] / icpara[2][2];
p[2][1] = icpara[2][1] / icpara[2][2];
p[2][2] = icpara[2][2] / icpara[2][2];
double q00, q01, q02, q03, q10, q11, q12, q13, q20, q21, q22, q23, q30, q31, q32, q33;
//視錐台への変換
q00 = (2.0 * p[0][0] / (i_screen_width - 1));
q01 = (2.0 * p[0][1] / (i_screen_width - 1));
q02 = -((2.0 * p[0][2] / (i_screen_width - 1)) - 1.0);
q03 = 0.0;
o_frustum.m00 = q00 * trans[0][0] + q01 * trans[1][0] + q02 * trans[2][0];
o_frustum.m01 = q00 * trans[0][1] + q01 * trans[1][1] + q02 * trans[2][1];
o_frustum.m02 = q00 * trans[0][2] + q01 * trans[1][2] + q02 * trans[2][2];
o_frustum.m03 = q00 * trans[0][3] + q01 * trans[1][3] + q02 * trans[2][3] + q03;
q10 = 0.0;
q11 = -(2.0 * p[1][1] / (i_screen_height - 1));
q12 = -((2.0 * p[1][2] / (i_screen_height - 1)) - 1.0);
q13 = 0.0;
o_frustum.m10 = q10 * trans[0][0] + q11 * trans[1][0] + q12 * trans[2][0];
o_frustum.m11 = q10 * trans[0][1] + q11 * trans[1][1] + q12 * trans[2][1];
o_frustum.m12 = q10 * trans[0][2] + q11 * trans[1][2] + q12 * trans[2][2];
o_frustum.m13 = q10 * trans[0][3] + q11 * trans[1][3] + q12 * trans[2][3] + q13;
q20 = 0.0;
q21 = 0.0;
q22 = (i_dist_max + i_dist_min) / (i_dist_min - i_dist_max);
q23 = 2.0 * i_dist_max * i_dist_min / (i_dist_min - i_dist_max);
o_frustum.m20 = q20 * trans[0][0] + q21 * trans[1][0] + q22 * trans[2][0];
o_frustum.m21 = q20 * trans[0][1] + q21 * trans[1][1] + q22 * trans[2][1];
o_frustum.m22 = q20 * trans[0][2] + q21 * trans[1][2] + q22 * trans[2][2];
o_frustum.m23 = q20 * trans[0][3] + q21 * trans[1][3] + q22 * trans[2][3] + q23;
q30 = 0.0;
q31 = 0.0;
q32 = -1.0;
q33 = 0.0;
o_frustum.m30 = q30 * trans[0][0] + q31 * trans[1][0] + q32 * trans[2][0];
o_frustum.m31 = q30 * trans[0][1] + q31 * trans[1][1] + q32 * trans[2][1];
o_frustum.m32 = q30 * trans[0][2] + q31 * trans[1][2] + q32 * trans[2][2];
o_frustum.m33 = q30 * trans[0][3] + q31 * trans[1][3] + q32 * trans[2][3] + q33;
return;
}
public NyARMat(int i_row, int i_clm)
{
m = ArrayUtils.newDouble2dArray(i_row, i_clm);
clm = i_clm;
row = i_row;
}