private static bool kpmUtilGetPose_binary(NyARParam i_cparam, FeaturePairStack matchData, NyARDoubleMatrix44 i_transmat, NyARTransMatResultParam i_resultparam)
{
NyARDoubleMatrix44 initMatXw2Xc = new NyARDoubleMatrix44();
// ARdouble err;
int i;
if (matchData.getLength() < 4)
{
return(false);
}
NyARDoublePoint2d[] sCoord = NyARDoublePoint2d.createArray(matchData.getLength());
NyARDoublePoint3d[] wCoord = NyARDoublePoint3d.createArray(matchData.getLength());
for (i = 0; i < matchData.getLength(); i++)
{
sCoord[i].x = matchData.getItem(i).query.x;
sCoord[i].y = matchData.getItem(i).query.y;
wCoord[i].x = matchData.getItem(i).ref_.pos3d.x;
wCoord[i].y = matchData.getItem(i).ref_.pos3d.y;
wCoord[i].z = 0.0;
}
NyARIcpPlane icp_planer = new NyARIcpPlane(i_cparam.getPerspectiveProjectionMatrix());
if (!icp_planer.icpGetInitXw2Xc_from_PlanarData(sCoord, wCoord, matchData.getLength(), initMatXw2Xc))
{
return(false);
}
/*
* printf("--- Init pose ---\n"); for( int j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) printf(" %8.3f",
* initMatXw2Xc[j][i]); printf("\n"); }
*/
NyARIcpPoint icp_point = new NyARIcpPoint(i_cparam.getPerspectiveProjectionMatrix());
icp_point.icpPoint(sCoord, wCoord, matchData.getLength(), initMatXw2Xc, i_transmat, i_resultparam);
if (i_resultparam.last_error > 10.0f)
{
return(false);
}
return(true);
}
public NyARTransMat(NyARParam i_param)
{
NyARCameraDistortionFactor dist = i_param.getDistortionFactor();
NyARPerspectiveProjectionMatrix pmat = i_param.getPerspectiveProjectionMatrix();
this._transsolver = new NyARTransportVectorSolver(pmat, 4);
//互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。
//理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。
this._rotmatrix = new NyARRotMatrix(pmat);
this._mat_optimize = new NyARPartialDifferentiationOptimize(pmat);
this._ref_dist_factor = dist;
this._projection_mat_ref = pmat;
}
public NyARTransMat(NyARParam i_param)
{
NyARCameraDistortionFactor dist = i_param.getDistortionFactor();
NyARPerspectiveProjectionMatrix pmat = i_param.getPerspectiveProjectionMatrix();
this._transsolver = new NyARTransportVectorSolver(pmat, 4);
//互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。
//理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。
this._rotmatrix = new NyARRotMatrix(pmat);
this._mat_optimize = new NyARPartialDifferentiationOptimize(pmat);
this._ref_dist_factor = dist;
this._projection_mat_ref = pmat;
}
/**
* コンストラクタです。
* 座標計算に必要なカメラパラメータの参照値を元に、インスタンスを生成します。
* @param i_param
* ARToolKit形式のカメラパラメータです。
* インスタンスは、この中から樽型歪み矯正オブジェクト、射影変換オブジェクトを参照します。
* @
*/
public NyARTransMat(NyARParam i_param)
{
initInstance(i_param.getDistortionFactor(), i_param.getPerspectiveProjectionMatrix());
return;
}
public static void toCameraFrustumRH(NyARParam i_arparam, double i_near, double i_far, ref Matrix o_d3d_projection)
{
toCameraFrustumRH(i_arparam.getPerspectiveProjectionMatrix(),i_arparam.getScreenSize(),1.0,i_near, i_far,ref o_d3d_projection);
}
/**
* コンストラクタ。
* 樽型歪みが少ない、または補正済みの画像を入力するときには、{@link #NyARReality(NyARIntSize, double, double, NyARPerspectiveProjectionMatrix, NyARCameraDistortionFactor, int, int)}
* のi_dist_factorにnullを指定すると、より高速な動作が期待できます。
* @param i_param
* カメラパラメータを指定します。
* @param i_near
* 視錐体のnear-pointをmm単位で指定します。
* default値は{@link #FRASTRAM_ARTK_NEAR}です。
* @param i_far
* 視錐体のfar-pointをmm単位で指定します。
* default値は{@link #FRASTRAM_ARTK_FAR}です。
* @param i_max_known_target
* Knownターゲットの最大数を指定します。
* @param i_max_unknown_target
* UnKnownターゲットの最大数を指定します。
* @throws NyARException
*/
public NyARReality(NyARParam i_param,double i_near,double i_far,int i_max_known_target,int i_max_unknown_target)
{
//定数とかいろいろ
this.MAX_LIMIT_KNOWN=i_max_known_target;
this.MAX_LIMIT_UNKNOWN=i_max_unknown_target;
this.initInstance(i_param.getScreenSize(),i_near,i_far,i_param.getPerspectiveProjectionMatrix(),i_param.getDistortionFactor());
return;
}
/**
* コンストラクタです。
* 座標計算に必要なカメラパラメータの参照値を元に、インスタンスを生成します。
* @param i_param
* ARToolKit形式のカメラパラメータです。
* インスタンスは、この中から樽型歪み矯正オブジェクト、射影変換オブジェクトを参照します。
* @
*/
public NyARTransMat(NyARParam i_param)
: this(i_param.getDistortionFactor(), i_param.getPerspectiveProjectionMatrix())
{
return;
}
/**
* コンストラクタです。
* 座標計算に必要なカメラパラメータの参照値を元に、インスタンスを生成します。
* @param i_param
* ARToolKit形式のカメラパラメータです。
* インスタンスは、この中から樽型歪み矯正オブジェクト、射影変換オブジェクトを参照します。
* @
*/
public NyARTransMat(NyARParam i_param)
{
initInstance(i_param.getDistortionFactor(), i_param.getPerspectiveProjectionMatrix());
return;
}
/**
* コンストラクタです。
* 座標計算に必要なカメラパラメータの参照値を元に、インスタンスを生成します。
* @param i_param
* ARToolKit形式のカメラパラメータです。
* インスタンスは、この中から樽型歪み矯正オブジェクト、射影変換オブジェクトを参照します。
* @
*/
public NyARTransMat(NyARParam i_param)
: this(i_param.getDistortionFactor(), i_param.getPerspectiveProjectionMatrix())
{
return;
}
public NyARIcpPlane(NyARParam i_param)
{
this._cparam = i_param.getPerspectiveProjectionMatrix();
}
public NyARIcpPlane(NyARParam i_param)
{
this._cparam = i_param.getPerspectiveProjectionMatrix();
}
private Matrix GetProjectionMatrix(NyARParam i_arparam, float near, float far)
{
NyARMat trans_mat = new NyARMat(3, 4);
NyARMat icpara_mat = new NyARMat(3, 4);
double[,] p = new double[3, 3], q = new double[4, 4];
int width, height;
int i, j;
NyARIntSize size = i_arparam.getScreenSize();
width = size.w;
height = size.h;
i_arparam.getPerspectiveProjectionMatrix().decompMat(icpara_mat, trans_mat);
double[][] icpara = icpara_mat.getArray();
double[][] trans = trans_mat.getArray();
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] = (far + near) / (far - near);
q[2, 3] = -2.0 * far * near / (far - near);
q[3, 0] = 0.0;
q[3, 1] = 0.0;
q[3, 2] = 1.0;
q[3, 3] = 0.0;
Matrix mat = Matrix.Identity;
mat.M11 = (float)(q[0, 0] * trans[0][0] + q[0, 1] * trans[1][0] + q[0, 2] * trans[2][0]);
mat.M12 = (float)(q[1, 0] * trans[0][0] + q[1, 1] * trans[1][0] + q[1, 2] * trans[2][0]);
mat.M13 = (float)(q[2, 0] * trans[0][0] + q[2, 1] * trans[1][0] + q[2, 2] * trans[2][0]);
mat.M14 = (float)(q[3, 0] * trans[0][0] + q[3, 1] * trans[1][0] + q[3, 2] * trans[2][0]);
mat.M21 = (float)(q[0, 1] * trans[0][1] + q[0, 1] * trans[1][1] + q[0, 2] * trans[2][1]);
mat.M22 = (float)(q[1, 1] * trans[0][1] + q[1, 1] * trans[1][1] + q[1, 2] * trans[2][1]);
mat.M23 = (float)(q[2, 1] * trans[0][1] + q[2, 1] * trans[1][1] + q[2, 2] * trans[2][1]);
mat.M24 = (float)(q[3, 1] * trans[0][1] + q[3, 1] * trans[1][1] + q[3, 2] * trans[2][1]);
mat.M31 = (float)(q[0, 2] * trans[0][2] + q[0, 1] * trans[1][2] + q[0, 2] * trans[2][2]);
mat.M32 = (float)(q[1, 2] * trans[0][2] + q[1, 1] * trans[1][2] + q[1, 2] * trans[2][2]);
mat.M33 = -(float)(q[2, 2] * trans[0][2] + q[2, 1] * trans[1][2] + q[2, 2] * trans[2][2]);
mat.M34 = -(float)(q[3, 2] * trans[0][2] + q[3, 1] * trans[1][2] + q[3, 2] * trans[2][2]);
mat.M41 = (float)(q[0, 3] * trans[0][3] + q[0, 1] * trans[1][3] + q[0, 2] * trans[2][3] + q[0, 3]);
mat.M42 = (float)(q[1, 3] * trans[0][3] + q[1, 1] * trans[1][3] + q[1, 2] * trans[2][3] + q[1, 3]);
mat.M43 = (float)(q[2, 3] * trans[0][3] + q[2, 1] * trans[1][3] + q[2, 2] * trans[2][3] + q[2, 3]);
mat.M44 = (float)(q[3, 3] * trans[0][3] + q[3, 1] * trans[1][3] + q[3, 2] * trans[2][3] + q[3, 3]);
return mat;
}
/**
* この関数は、ARToolKitスタイルのカメラパラメータから、 CameraFrustamを計算します。
* カメラパラメータの要素のうち、ProjectionMatrix成分のみを使います。
* @param i_arparam
* ARToolKitスタイルのカメラパラメータ。
* @param i_scale
* スケール値を指定します。1=1mmです。10ならば1=1cm,1000ならば1=1mです。
* 2.53以前のNyARToolkitと互換性を持たせるときは、{@link #SCALE_FACTOR_toCameraFrustumRH_NYAR2}を指定してください。
* @param i_near
* 視錐体のnearPointを指定します。単位は、i_scaleに設定した値で決まります。
* @param i_far
* 視錐体のfarPointを指定します。単位は、i_scaleに設定した値で決まります。
* @param o_gl_projection
* OpenGLスタイルのProjectionMatrixです。double[16]を指定します。
*/
public static void toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,ref Matrix4x4 o_mat)
{
toCameraFrustumRH(i_arparam.getPerspectiveProjectionMatrix(),i_arparam.getScreenSize(),i_scale,i_near,i_far,ref o_mat);
return;
}
